iSAM2 部分状态更新算法 (I - 原理解读)

Title: iSAM2 部分状态更新算法 (I-原理解读)

---

---

关联博客:

因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”

基于 GTSAM 的因子图简单实例

GTSAM 中的鲁棒噪声模型与 M-估计 (GTSAM Robust Noise Model and M-Estimator)

贝叶斯树定义与构建的寻行数墨 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

贝叶斯树增量式更新的细嚼慢咽 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

iSAM2 部分状态更新算法 (I-原理解读) $\Leftarrow$ 本篇

---

I. 前言

SLAM 后端优化中基于因子图 (Factor Graph) 的增量式平滑和建图算法 iSAM2^[1] 的原理以及 GTSAM 的实现, 内容丰富非常精彩, 尝试理解的过程中会很烧脑.

我们此处只选其中一小部分 “iSAM 中的部分状态更新子算法”, 尝试对其原理和源码分别解读一下.

本篇先讲原理部分, 对应于论文 “iSAM2: Incremental smoothing and mapping using the Bayes tree”^[1] 中的 “Algorithm 7 Partial state update: solving the Bayes tree in the nonlinear case returns an update $\Delta$ to the current linearization point $\Theta$”.

---

II. 部分状态的更新 (Partial State Update)

部分状态更新算法的伪代码如下^[1]:

部分状态更新算法: 解非线性情况下的贝叶斯树, 返回当前线性化点 (状态) $\Theta$ 的更新 $\Delta$

输入: 贝叶斯树 $\mathcal{T}$

输出: 状态的更新 $\Delta$

[1]   从根团 $C_r=F_r$ 开始执行:

[2]        对于现在的团 $C_k=F_k:S_k$, 从局部条件概率密度 $P(F_k|S_k)$ 中计算前端变量 $F_k$ 的状态更新 ${\Delta }_k$.

[3]        如果状态更新 ${\Delta }_k$ 中的变量 ${\Delta }_{k_j}$ 大于阈值 $\alpha$, 则对包含该变量的子团递归地执行本算法.

需要说明:

- 部分状态更新的驱动因素源自贝叶斯树的更新, 如整个因子图中加入了新的因子、贝叶斯树需要重线性化等.

- 算法名中所谓的 “部分” 就是仅对部分变量进行更新计算, 只有贝叶斯树的团中包含大于阈值的变量更新时, 更新计算才向子团传播.

- 状态更新和重线性化 (Fluid Relinearization) 是不同的, 状态更新只计算线性化后的状态偏差 $\Delta$ 偏离零点 $0$ 的程度, 而重线性化是在新的线性化点 ${\Theta }^{[0]}$ 上作关于变量 $\Theta$ 的因子图与贝叶斯树的线性化.

状态变量 $\Theta$ 及其偏差 $\Delta$ 之间的相互关系如下:
$$\begin{array}{llc}{\Theta }^{[0]}& ⟼& \Theta \\ |& & |\\ 0& ⟼& \Delta =\Theta -{\Theta }^{[0]}\end{array}$$

消元运算 (Elimination) 是贯穿于整个因子图算法的重要技术, 状态更新也和消元运算紧密相关, 我们还是从消元运算开始.

不同于之前博文 “因子图、边缘化与消元算法的抽丝剥茧 —— Notes for ‘Factor Graphs for Robot Perception’” 侧重于定性分析, 本文重点在于消元的计算过程. 因为要用作变量的数值更新计算, 所以定量计算公式是重点.

实际问题都是非线性问题, 定量计算一般要从线性化着手.

---

III. 因子图的线性化 (Linearization of Factor Grahps)

1. 简单实例的设定

此处还是借用最简单的 SLAM 例子作说明, 稍有不同的是这次在 SLAM 运行过程中新增加了因子 ${\varphi }_{10}$, 如 Fig .1. 在本例中具体新增哪个因子或什么样的因子并不重要, 只是新增加因子就会触发 iSAM2 算法更新贝叶斯树以及执行增量式优化, 故也引起了部分状态更新算法的执行.

Fig. 1 一个简单的 SLAM 中的因子图例子

该例子中的变量 $\Theta =\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\theta }_3\\ {\theta }_4\\ {\theta }_5\end{array}\right]\triangleq \left[\begin{array}{c}{l}_1\\ l_2\\ x_1\\ x_2\\ x_3\end{array}\right]$.

而其中每个变量又由于应用场景的不同, 又都是不同维度的向量, 如二维应用中 $l_1=\left[\begin{array}{c}{l}_{1,x}\\ l_{1,y}\end{array}\right]\in {\mathbb{R}}^2$ 或 $x_1=\left[\begin{array}{c}{x}_{1,x}\\ x_{1,y}\\ x_{1,rot}\end{array}\right]\in \mathbb{SE}(3)$. 此处我们忽略不同流形的空间结构, 都视作欧式空间.

例子中有因子 ${\varphi }_1,\cdots ,{\varphi }_{10}$, 所谓因子图的线性化就是先对其中的每个因子进行线性化, 组成的整个因子图也就完成了线性化.

下面我们以变量 $l_1$ 关联的三个因子 { ϕ 3 , ϕ 7 , ϕ 8 } \{\phi_3, \phi_7, \phi_8\} {ϕ3​,ϕ7​,ϕ8​} 为例进行因子的线性化.

---

2. 一个线性化计算

假设路标 $l_1$ 的先验因子是高斯概率分布因子
$$\begin{array}{r}{\varphi }_4=\mathcal{N}(l_1;z_4,{\Sigma }_4)\propto \exp (-\frac{1}{2}{\Vert l_1-z_4\Vert }_{{\Sigma }_4}^2)\end{array}$$
变量 ${\theta }_1\triangleq l_1$, 测量函数 $h_4({\theta }_1)={\theta }_1$. 则在线性化点 ${\theta }_1^{[0]}$ 处的测量雅可比矩阵 (Measurement Jacobian) 为
$$H_4={\frac{\partial h_4({\theta }_1)}{\partial {\theta }_1}|}_{{\theta }_1^{[0]}}=1$$
定义变量相对于线性化点的偏差 ${\Delta }_1\triangleq {\theta }_1-{\theta }_1^{[0]}$, 则测量函数的一阶泰勒展开为 $h_4({\theta }_1)\approx {\theta }_1^{[0]}+H_4({\theta }_1-{\theta }_1^{[0]})={\theta }_1^{[0]}+H_4{\Delta }_1$.

将一阶近似代入因子
$$\begin{array}{rl}{\varphi }_4({\theta }_1)& \propto \exp (-\frac{1}{2}{\Vert {\theta }_1^{[0]}+H_4{\Delta }_1-z_4\Vert }_{{\Sigma }_4}^2)\\ & \propto \exp (-\frac{1}{2}{\Vert {\Sigma }_4^{-\frac{1}{2}}{H}_4{\Delta }_1-{\Sigma }_4^{-\frac{1}{2}}\left(z_4-{\theta }_1^{[0]}\right)\Vert }^2)\end{array}$$
再定义 $A_{41}\triangleq {\Sigma }_4^{-\frac{1}{2}}{H}_4$, $b_4\triangleq {\Sigma }_4^{-\frac{1}{2}}\left(z_4-{\theta }_1^{[0]}\right)$. 得到在线性化点 ${\theta }_1^{[0]}$ 处的线性化高斯因子
$${\varphi }_4({\theta }_1)\propto \exp (-\frac{1}{2}{\Vert A_{41}{\Delta }_1-b_4\Vert }^2)\text{\hspace{1em}(II-2-1)}$$

---

3. 其他线性化计算

同样的方法计算 ${\varphi }_7$ 的线性化,
$$\begin{array}{rl}{\varphi }_7({\theta }_1,{\theta }_3)& \propto \exp (\frac{1}{2}\Vert {\theta }_1-{\theta }_3-z_7{\Vert }_{{\Sigma }_7}^2)\\ & \propto \exp (-\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{71},A_{73}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_3\end{array}\right]-b_7\Vert }^2)\end{array}\text{\hspace{1em}(II-3-1)}$$
其中中间变量有
$$\begin{array}{rl}{h}_7({\theta }_1,{\theta }_3)& ={\theta }_1-{\theta }_3\\ H_7& ={\frac{\partial h_7({\theta }_1,{\theta }_3)}{\partial {\left[\begin{array}{c}{\theta }_1,{\theta }_3\end{array}\right]}^{\mathrm{T}}}|}_{{\theta }_1^{[0]},{\theta }_3^{[0]}}=\left[\begin{array}{c}I,-I\end{array}\right]\\ {\Delta }_3& \triangleq {\theta }_3-{\theta }_3^{[0]}\\ h_7({\theta }_1,{\theta }_3)& ={\theta }_1^{[0]}-{\theta }_3^{[0]}+H_7\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_3\end{array}\right]\\ \left[\begin{array}{c}{A}_{71},A_{73}\end{array}\right]& ={\Sigma }_7^{-\frac{1}{2}}{H}_7\\ b_7& ={\Sigma }_7^{-\frac{1}{2}}(z_7-{\theta }_1^{[0]}+{\theta }_3^{[0]})\end{array}$$

再用同样的方法计算 ${\varphi }_8$ 的线性化
$$\begin{array}{rl}{\varphi }_8({\theta }_1,{\theta }_4)& \propto \exp (\frac{1}{2}\Vert {\theta }_1-{\theta }_4-z_8{\Vert }_{{\Sigma }_8}^2)\\ & \propto \exp (-\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{81},A_{84}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_4\end{array}\right]-b_8\Vert }^2)\end{array}\text{\hspace{1em}(II-3-2)}$$
其中中间变量有
$$\begin{array}{rl}{h}_8({\theta }_1,{\theta }_4)& ={\theta }_1-{\theta }_4\\ H_8& =\left[\begin{array}{c}I,-I\end{array}\right]\\ {\Delta }_4& \triangleq {\theta }_4-{\theta }_4^{[0]}\\ h_8({\theta }_1,{\theta }_4)& ={\theta }_1^{[0]}-{\theta }_4^{[0]}+H_8\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_4\end{array}\right]\\ \left[\begin{array}{c}{A}_{81},A_{84}\end{array}\right]& ={\Sigma }_8^{-\frac{1}{2}}{H}_8\\ b_8& ={\Sigma }_8^{-\frac{1}{2}}(z_8-{\theta }_1^{[0]}+{\theta }_4^{[0]})\end{array}$$

---

4. 状态更新量说明

在线性化过程中, 定义了变量相对于线性化点的偏差 (状态更新或变量更新)
$$\Delta =\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_2\\ {\Delta }_3\\ {\Delta }_4\\ {\Delta }_5\end{array}\right]\triangleq \left[\begin{array}{c}{l}_1-l_1^{[0]}\\ l_2-l_2^{[0]}\\ x_1-x_1^{[0]}\\ x_2-x_2^{[0]}\\ x_3-x_3^{[0]}\end{array}\right]\text{\hspace{1em}(II-4-1)}$$
这是本博文的主角.

线性化点 ${\Theta }^{[0]}={\left[\begin{array}{c}{l}_1^{[0]},l_2^{[0]},x_1^{[0]},x_2^{[0]},x_3^{[0]}\end{array}\right]}^{\mathrm{T}}$ 是贝叶斯树根据之前信息获得的参数最优估计.

如果没有新的因子进入 (如获得新的测量值), 线性化点就认为是实际变量值 (最优估计). 当获得新的因子后, 需要重新计算系统变量的最优估计.

这个新的潜在的变量最优估计和原来的线性化点 (老的最优估计) 之间的差值 (偏差) 就是 $\Delta$, 称之为状态更新量.

状态更新量 $\Delta$ 就这样走向了前台, 这里虽然需要消除的变量是 $\Theta$, 但计算操作都是基于 $\Delta$.

此时, 变量值 $\Theta$ 已被拆分为线性化点 ${\Theta }^{[0]}$ (常量) 和状态更新 $\Delta$ (变量).

作为常量的 ${\Theta }^{[0]}$ 隐藏在了公式中的代换变量中 (如各个 $b_i$).

随着对各个非线性因子的线性化, 相应地因子图也在 ${\Theta }^{[0]}$ 处被线性化了.

线性化后的因子图称为高斯因子图.

---

IV. 部分 QR 分解实现变量消元 (Eliminating Variables Using Partial QR)

上一节完成了因子图的线性化, 得到了高斯因子图, 这一节就针对高斯因子图作消元. 高斯因子图消元与原始因子图消元在原理上是一样的, 但高斯因子图消元具有的优点是可以方便地利用线性代数知识 (如 QR 分解) 进行计算.

1. 首变量的消元

根据之前博文 “因子图、边缘化与消元算法的抽丝剥茧 —— Notes for ‘Factor Graphs for Robot Perception’” 对因子图变量消元算法原理的描述, 先收集变量 ${\theta }_1$ 关联的因子的乘积
$$\begin{array}{rl}\psi ({\theta }_1,S_1)& =\exp (-\frac{1}{2}{\Vert A_{41}{\Delta }_1-b_4\Vert }^2)\\ & \cdot \exp (-\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{71},A_{73}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_3\end{array}\right]-b_7\Vert }^2)\\ & \cdot \exp (-\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{81},A_{84}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_4\end{array}\right]-b_8\Vert }^2)\\ & =\exp (-\frac{1}{2}{\Vert A_{41}{\Delta }_1-b_4\Vert }^2\\ & -\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{71},A_{73}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_3\end{array}\right]-b_7\Vert }^2\\ & -\frac{1}{2}{\Vert \left[\begin{array}{c}{A}_{81},A_{84}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_4\end{array}\right]-b_8\Vert }^2)\\ & =\exp (-\frac{1}{2}{\Vert \left[\begin{array}{lll}{A}_{41}& & \\ A_{71}& A_{73}& \\ A_{81}& & A_{84}\end{array}\right]\left[\begin{array}{c}{\Delta }_1\\ {\Delta }_3\\ {\Delta }_4\end{array}\right]-\left[\begin{array}{c}{b}_4\\ b_7\\ b_8\end{array}\right]\Vert }^2)\end{array}\text{\hspace{1em}(III-1-1)}$$
这里 ${\theta }_1$ 是待消元变量, S 1 = { θ 3 , θ 4 } S_1 = \{\theta_3, \theta_4\} S1​={θ3​,θ4​} 是 ${\theta }_1$ 的分离子集合. 为了消元 ${\theta }_1$ 和后面将涉及的反向替代计算状态更新 ${\Delta }_1$, 需要对分块矩阵
$${\bar{A}}_1\triangleq \left[\begin{array}{lll}{A}_{41}& & \\ A_{71}& A_{73}& \\ A_{81}& & A_{84}\end{array}\right]$$
中的第一列作 QR 分解 (或 Gram-Schmidt 正交化)^{[1, 2]}, 连带着 $\left[\begin{array}{c}{b}_4\\ b_7\\ b_8\end{array}\right]$ 也执行相应的操作. 即
$$\left[\begin{array}{llll}{A}_{41}& & & b_4\\ A_{71}& A_{73}& & b_7\\ A_{81}& & A_{84}& b_8\end{array}\right]=Q_1\left[\begin{array}{lll}{R}_1& T_1& d_1\\ & {\tilde{A}}_{\tau 1}& {\tilde{b}}_{\tau 1}\end{array}\right]\text{\hspace{1em}(III-1-2)}$$
其中 $Q_1$ 是正交矩阵; $R_1$ 是上三角矩阵, 维度和 ${\Delta }_1$ 兼容. 记分离子偏差为 ${\hat{S}}_1\triangleq \left[\begin{array}{c}{\Delta }_3\\ {\Delta }_4\end{array}\right]$.

因为正交矩阵具有保范性, 所以正交阵 $Q_1$ 不会对范数运算产生影响.

这样, 变量 ${\theta }_1$ 关联的因子的乘积可以改写成
$$\begin{array}{rl}\psi ({\theta }_1,S_1)& =\underline{\exp (-\frac{1}{2}{\Vert R_1{\Delta }_1+T_1{\hat{S}}_1-d_1\Vert }^2)}\underline{\underline{\exp (-\frac{1}{2}{\Vert {\tilde{A}}_{\tau 1}{\hat{S}}_1-{\tilde{b}}_{\tau 1}\Vert }^2)}}\\ & =\underline{p({\Delta }_1|{\hat{S}}_1)}\underline{\underline{{\tau }_1({\hat{S}}_1)}}\end{array}\text{\hspace{1em}(III-1-3)}$$
其中 $p({\Delta }_1|{\hat{S}}_1)$ 称为条件概率密度 (Conditional Density), ${\tau }_1({\hat{S}}_1)$ 称为边缘化因子 (Marginal Factor).

因为不是对整个 ${\bar{A}}_1$ 矩阵进行 QR 分解, 而只针对变量 ${\Delta }_1$ 相关的第一列 (分块矩阵中的列) 进行, 故称为部分 QR 分解.

---

2. 中间变量的消元

推广变量 ${\theta }_1$ 的消元公式到一般变量 ($1\le i<5$, 除了消元顺序上的最后一个变量 ${\theta }_5$) 上
$$\begin{array}{rl}\psi ({\theta }_i,S_i)& =\underline{\exp (-\frac{1}{2}{\Vert R_i{\Delta }_i+T_i{\hat{S}}_i-d_i\Vert }^2)}\underline{\underline{\exp (-\frac{1}{2}{\Vert {\tilde{A}}_{\tau i}{\hat{S}}_i-{\tilde{b}}_{\tau i}\Vert }^2)}}\\ & =\underline{p({\Delta }_i|{\hat{S}}_i)}\underline{\underline{p_{\tau i}({\hat{S}}_i)}}\end{array}\text{\hspace{1em}(III-2-1)}$$
中间变量的消元和第一个变量的消元相比没有特别之处.

---

3. 最末变量的消元

当消元只剩下最后一个变量 ${\theta }_5$ 时, 也需要对块矩阵 $\left[\begin{array}{c}{\tilde{A}}_{25}\\ {\tilde{A}}_{35}\\ {\tilde{A}}_{45}\end{array}\right]$ 做 QR 分解, 并连带对 $\left[\begin{array}{c}{b}_{25}\\ b_{35}\\ b_{45}\end{array}\right]$ 执行相同操作. 即
$$\left[\begin{array}{cc}{\tilde{A}}_{25}& b_{25}\\ {\tilde{A}}_{35}& b_{35}\\ {\tilde{A}}_{45}& b_{45}\end{array}\right]=Q_5\left[\begin{array}{cc}{R}_5& d_5\\ 0& {\tilde{b}}_5\end{array}\right]\text{\hspace{1em}(III-2-2)}$$
需要说明, 因子数量 (测量数量) 必然大于变量数量, 所以涉及的线性化矩阵 (如 $\left[\begin{array}{lll}{A}_{41}& & \\ A_{71}& A_{73}& \\ A_{81}& & A_{84}\end{array}\right]$ 或 $\left[\begin{array}{c}{\tilde{A}}_{25}\\ {\tilde{A}}_{35}\\ {\tilde{A}}_{45}\end{array}\right]$) 都是行数大于列数, 且都是列满秩的, 不然系统就没有确切解了.

由式 (III-2-2) 可知最后一变量的消元为
$$\begin{array}{rl}\psi ({\theta }_5)& =\exp (-\frac{1}{2}\Vert R_5{\Delta }_5-d_5{\Vert }^2)\underset{\mathrm{const}}{\underbrace{\exp (-\frac{1}{2}\Vert {\tilde{b}}_5{\Vert }^2)}}\\ & \propto \exp (-\frac{1}{2}\Vert R_5{\Delta }_5-d_5{\Vert }^2)\\ & =p({\Delta }_5)\end{array}\text{\hspace{1em}(III-2-3)}$$
因为是最后消元的, 此处的分离子集合为空集. 因为整个算法求的是最大后验概率对应的变量值, 常数不会对结果产生影响.

完成消元后, 整个因子图转换为了贝叶斯网络 (Bayes Net).

---

V. 状态更新的反向替代 (Back Substitution for State Update)

所谓直接反向替代就是利用 QR 分解得到的上三角矩阵 $R_j$ 的可逆性, 以逆消元顺序, 先求出最后消元变量的最大后验估计 (MAP), 然后向前推进计算所有变量的优化值. 比如, 先计算最后消元的变量偏差
$$R_5{\Delta }_5-d_5=0\Rightarrow {\Delta }_5=R_5^{-1}{d}_5$$
然后, 以最后消元变量偏差 ${\Delta }_5$ 作为前面消元变量偏差 ${\Delta }_4$ 的分离子偏差, 进行计算
$$\begin{array}{r}{R}_4{\Delta }_4+T_4{\hat{S}}_4-d_4=0\\ {\hat{S}}_4={\Delta }_5\end{array}\}\Rightarrow {\Delta }_4=R_4^{-1}(d_4-T_4{\hat{S}}_4)$$
如此以消元逆序从根节点向所有叶节点推进.

高斯分布下的最大后验估计在条件概率密度的均值处取得, 即直接反向替代中每一步都是在计算高斯条件概率均值.

所以有算法如下.

贝叶斯网络中的直接反向替代^[2]

function $Solve(p(\Delta ))$                        $⊳$ 给定贝叶斯网络

        for $j=n,\dots ,1$ do                      $⊳$ 消元顺序的逆

              ${\Delta }_j\leftarrow R_j^{-1}(d_j-T_j{\hat{S}}_j)$                       $⊳$ 给定分离子偏差 ${\hat{S}}_j$ 求变量偏差 ${\Delta }_j$

说明:

- “直接反向替代算法” 逆序遍历整个贝叶斯网络, 更新计算整个贝叶斯树中所有参数偏差.

- 这里所谓的"直接"就是直接解线性方程组, 而不是采用迭代逼近方法获得原非线性系统的解.

- “部分状态更新算法” 伪代码中并没有约束更新计算的方式 (如线性矩阵计算、非线性迭代等). 这是为了算法的一般性描述, 但因为经过了线性化处理, 最终只需直接求解线性方程组即可 (如 GTSAM 源码中).

- “部分状态更新算法” 区别于 “直接反向替代算法” 的是利用阈值来控制反向替代计算的传播. 如果当前节点的变量偏差小于阈值 (即 ${\Delta }_k<\alpha$) 时, 根据线性方程的特点, 可以预测子节点的变量偏差也将比较小 (控制在线型比例范围内). 为了节约计算资源, 不再更新预测为较小变量偏差的节点. 这就实现了贝叶斯树中变量偏差的部分计算更新.

- “部分状态更新算法” 中提到了树团. 该树团只是贝叶斯树中节点的组织形式, 最终计算还是要落在一个个节点变量偏差上的. 也就是说, 贝叶斯树、贝叶斯网络、因子图等不同的描述从不方面刻画同一个 SLAM 优化问题, 虽然有不同的数据结构和洞察力, 但最终都要落在同一计算上.

---

VI. 总结

这样就大概明白的 iSAM2 算法中部分状态更新的原理.

要了解具体如何控制 “部分” 更新, 还需要在源码中找答案.

---

参考文献

[1] Kaess M, Johannsson H, Roberts R, Ila V, Leonard JJ, Dellaert F. iSAM2: Incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research. 31(2):216-235. 2012

[2] Frank Dellaert, Michael Kaess, Factor Graphs for Robot Perception, Foundations and Trends in Robotics, Vol. 6, No. 1-2 (2017) 1–139