ISAM1论文讲解

引言

ISAM的全称为增量式平滑和建图（Incremental Smoothing and Mapping），它是一种用于SLAM问题的非线性优化算法，与常见的高斯牛顿法、LM等非线性优化算法不同，ISAM可以增量式的求解非线性优化问题。目前有ISAM1和ISAM2两个版本，这个两个版本的主要区别在于做重新线性化（relinearization）的时机不同。ISAM1只支持周期性重新线性化而ISAM2则会通过贝叶斯树自动判断重新线性化的时机，该机制比较复杂，后续会再写一篇博客专门讲解ISAM2重新线性化的机制。本篇博客主要讲解ISAM1，下面我们将详细讲解一下ISAM中增量式到底是怎么实现的。

一、SLAM中的非线性最小二乘问题

第一步： 我们对SLAM中的最小二乘问题建模：
假设运动方程为和观测方程如下所示：

运动方程： $$x_i=f_i(x_{i-1},u_i)+w_i$$ 观测方程：$$z_k=h_k(x_{i_k}，l_{j_k})+v_k$$
第二步： 那么对应的最小二乘问题则如下所示：
$$X^{\ast },L^{\ast }=\underset{X,L}{\mathrm{argmin}}\sum _{i=1}^M||f_i(x_{i-1},u_i)-x_i|{|}_{\Lambda }^2+\sum _{i=1}^K||h_k(X_{i_k},I_{j_k})-z_k|{|}_{\Gamma }^2$$
第三步：对上述运动方程的残差进行线性化（一阶泰勒展开）有：
f i ( x i − 1 , u i ) − x i ≈ { f i ( x i − 1 0 , u i ) + F i i − 1 δ x i − 1 } − { x i 0 + δ x i } = { F i i − 1 δ x i − 1 − δ x i } − a i \begin{aligned} f_i(x_{i - 1}, u_i) - x_i & \approx \{f_i(x_{i-1}^0, u_i) + F_i^{i - 1}\delta x_{i-1}\} - \{ x_i^0 + \delta x_i \} \\ & = \{F_i^{i-1} \delta x_{i-1} - \delta x_i \} -a_i \end{aligned} fi​(xi−1​,ui​)−xi​​≈{fi​(xi−10​,ui​)+Fii−1​δxi−1​}−{xi0​+δxi​}={Fii−1​δxi−1​−δxi​}−ai​​

其中
$$F_i^{i-1}:={\frac{\partial f_i(x_{i-1},u_i)}{\partial x_{i-1}}|}_{x_{i-1}^0}$$
$$a_i=x_i^0-f_i(x_{i-1}^0,u_i)$$

第四步：对上述观测方程的残差进行线性化（一阶泰勒展开）有：
$$\begin{array}{rl}{h}_k({\mathbf{x}}_{ik},{\mathbf{l}}_{jk})-{\mathbf{z}}_k& \approx \left\{h_k({\mathbf{x}}_{ik}^0,{\mathbf{l}}_{jk}^0)+H_k^i\delta {\mathbf{x}}_{ik}+J_k^j\delta {\mathbf{l}}_{jk}\right\}-{\mathbf{z}}_k\\ & =\left\{H_k^i\delta {\mathbf{x}}_{ik}+J_k^j\delta {\mathbf{l}}_{jk}\right\}-{\mathbf{c}}_k\end{array}$$

其中
$$H_k^{ik}:={\frac{\partial h_k(x_{ik},l_{jk})}{\partial x_{ik}}|}_{x_{ik}^0,l_{jk}^0}$$
$$J_k^{jk}:={\frac{\partial h_k(x_{ik},l_{jk})}{\partial l_{jk}}|}_{x_{ik}^0,l_{jk}^0}$$
$$c_k=z_k-h_k(x_{ik}^0,l_{jk}^0)$$
第五步：那么最小二乘可化简为：
$$\delta {\theta }^{\ast }=\underset{\delta \theta }{\mathrm{argmin}}\sum _{i=1}^M||F_i^{i-1}\delta x_{i-1}+G_i^i\delta x_i-a_i|{|}_{\Lambda }^2+\sum _{i=1}^K||H_k^{ik}\delta x_{ik}+J_k^{jk}\delta l_{jk}-c_k|{|}_{\Gamma }^2$$

第六步：将上述最小二乘写成矩阵形式，把雅可比矩阵放到矩阵$A$中，把$a_i$和$c_k$放到向量$b$中，那么最终的最小二乘形式如下：
$$\delta {\theta }^{\ast }=\underset{\delta \theta }{\mathrm{argmin}}||A\delta \theta -b|{|}^2$$

二、通过QR分解求解上述最小二乘

对测量雅可比$A$进行QR分解可得：
$$A=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$$
注：这里的$R$等于$A^{T}A$的Cholesky分解的上三角矩阵。

将QR分解结果应用到上面的最小二乘中可得：
$$\begin{array}{rl}\Vert A\mathbf{\theta }-\mathbf{b}{\Vert }^2& ={\Vert Q\left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }-\mathbf{b}\Vert }^2\\ & ={\Vert Q^T\left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }-Q^T\mathbf{b}\Vert }^2\\ & ={\Vert \left[\begin{array}{c}R\\ 0\end{array}\right]\mathbf{\theta }-\left[\begin{array}{c}\mathbf{d}\\ \mathbf{e}\end{array}\right]\Vert }^2\\ & =\Vert R\mathbf{\theta }-\mathbf{d}{\Vert }^2+\Vert \mathbf{e}{\Vert }^2\end{array}$$

由此便可得到最小二乘解为：
$$R{\mathbf{\theta }}^{\ast }=\mathbf{d}$$
这里由于$R$是上三角矩阵，因此我们可通过从下往上的回代很快的求出${\mathbf{\theta }}^{\ast }$

三、通过Givens旋转，增量式更新矩阵$R$

到这一步就是整个算法最核心的步骤：增量式更新，常规的高斯牛顿法是构造一个$J^{T}J$形式的信息矩阵来迭代求解$\theta$，每次迭代的时候都需要重新做线性化并构造$J^{T}J$，然后通过Cholesky分解或者QR分解求解正规方程，而ISAM则是从始至终只会维护一个$R$矩阵，每当有新的观测时，可通过Given旋转直接更新R矩阵,无需再做QR分解或者Cholesky分解（关于Givens旋转含义可参考博客下方链接[2]），具体过程如下：

假设新的测量雅可比矩阵为${\mathbf{w}}^T$，RHS(right hand side)向量为$\gamma$，将其插入到之前的最小二乘中有：
$$\left[\begin{array}{c}{Q}^T\\ 1\end{array}\right]\left[\begin{array}{c}A\\ {\mathbf{w}}^T\end{array}\right]=\left[\begin{array}{c}R\\ {\mathbf{w}}^T\end{array}\right],\text{new rhs: }\left[\begin{array}{c}\mathbf{d}\\ \gamma \end{array}\right].$$
最终通过Givens旋转便可求得更新后的正规方程$R^{{}^{\prime }}{\theta }^{{}^{\prime }}=b^{{}^{\prime }}$，完整的更新过程如下图所示：

四： 回环和变量重排序

当发生回环时，直接通过Given旋转更新$R$会使得矩阵中出现很多的非零项，也称fill-in现象，ISAM中通过变量重排序来避免fill-in现象的发生。ISAM1中会周期性的执行变量重排序，在重排序的同时也会重新做线性化重置$R$矩阵。为啥是周期性的执行重排序呢，是因为它不知道什么时候要做重排序，但为了防止fill-in的发生，它就周期性的执行重排序。在ISAM2中，通过贝叶斯解决了这个问题，ISAM2会判断当前状态是否需要做重排序，只有当需要做重排序的时候才会做重排序。

五： 小结

1. ISAM通过Giens旋转增量式的更新QR分解中的$R$矩阵，避免了每次求解释从头构造$J^{T}J$，也避免了每次都对这个大的$J^{T}J$做Cholesky分解，极大的提高了效率。
2. 但是ISAM每次观测来的时候默认只会更新一次$R$矩阵，精度应该会比常用的GN、LM差一点，应为GN和LM每次优化的时候都会多次迭代。
3. ISAM1无法判断时候该做重新线性化，所以为了防止fill-in的现象，它只能周期性的执行重新线性化和重排序，ISAM2通过贝叶斯树解决了这个问题。

参考资料

1. Michael Kaess. iSAM: Incremental Smoothing and Mapping
2. Given旋转: https://zhuanlan.zhihu.com/p/136551885