在SLAM中非线性最小二乘问题利用舒尔补简化求解增量

高斯牛顿求解流程
有如下最小二乘系统，对应的模型如下所示：
$$\xi =\underset{\xi }{\mathrm{argmin}}\frac{1}{2}\sum _i\Vert r_i{\Vert }_{{\sum }_i}^2$$对上式展开求导，令其导数为零，可得到$Hx=b$的形式，上式对应的高斯牛顿求解 normal equation：$$\underset{HorA}{\underbrace{J^{\top }{\Sigma }^{-1}J}}\delta \xi =\underset{b}{\underbrace{-J^{\top }{\Sigma }^{-1}r}}$$如果$H$可逆，则可以直接求解$\Delta x=-H^{-1}b$，但其计算量较大。在另一篇关于舒尔补的博客中可以知道，舒尔补从概率上可以加速问题求解，利用SLAM问题的稀疏性。

例如某单目BA问题，其信息矩阵如下所使：
$$\left[\begin{array}{cc}{H}_{pp}& H_{pl}\\ H_{lp}& H_{ll}\end{array}\right]\left[\begin{array}{c}\Delta x_p^{\ast }\\ \Delta x_l^{\ast }\end{array}\right]=\left[\begin{array}{c}-b_p\\ -b_l\end{array}\right]$$其中$H_{pp}$代表相机pose之间的信息矩阵，$H_{pl}$代表相机与路标点之间的信息矩阵，$H_{ll}$代表路标点之间的信息矩阵。
接下来利用如下舒尔补的操作，使上式信息矩阵变成下三角矩阵。
$$\left[\begin{array}{cc}I& -B{D}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]=\left[\begin{array}{cc}A-B{D}^{-1}C& 0\\ C& D\end{array}\right]$$$$\left[\begin{array}{cc}I& -H_{pl}{H}_{ll}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{H}_{pp}& H_{pl}\\ H_{lp}& H_{ll}\end{array}\right]=\left[\begin{array}{cc}{H}_{pp}-H_{pl}{H}_{ll}^{-1}{H}_{lp}& 0\\ H_{lp}& H_{ll}\end{array}\right]$$$$\Downarrow 带入到信息矩阵中$$$$\left[\begin{array}{cc}I& -H_{pl}{H}_{ll}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{H}_{pp}& H_{pl}\\ H_{lp}& H_{ll}\end{array}\right]\left[\begin{array}{c}\Delta x_p^{\ast }\\ \Delta x_l^{\ast }\end{array}\right]=\left[\begin{array}{cc}I& -H_{pl}{H}_{ll}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}-b_p\\ -b_l\end{array}\right]$$$$⟹\left[\begin{array}{cc}{H}_{pp}-H_{pl}{H}_{ll}^{-1}{H}_{lp}& 0\\ H_{lp}& H_{ll}\end{array}\right]\left[\begin{array}{c}\Delta x_p^{\ast }\\ \Delta x_l^{\ast }\end{array}\right]=\left[\begin{array}{cc}I& -H_{pl}{H}_{ll}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}-b_p\\ -b_l\end{array}\right]$$从而得到：$$(H_{pp}-H_{pl}{H}_{ll}^{-1}{H}_{lp})\Delta x_p^{\ast }=-b_p+H_{pl}{H}_{ll}^{-1}{b}_l$$求得$\Delta x_p^{\ast }$后再计算$\Delta x_l^{\ast }$：$$H_{ll}\Delta x_l^{\ast }=-b_l-H_{pl}^{\top }\Delta x_p^{\ast }$$

---

在SLAM中整个solver流程可以总结为：
利用IMU构建残差，重投影误差构建姿态和路标点关系。
构建误差函数$r_{ij}\to$计算雅可比$J_{ij}\to (H+\lambda I)\delta x=b\to$利用舒尔补简化求解 $\to$求解出增量$\Delta x$作用到之前变量上，更新残差、更新雅可比，不断迭代。

---

但是在实际情况中，信息矩阵H并不满秩，可使用LM算法，$H=H+\lambda I$，加阻尼因子使得系统满秩，但是求解的结果可能会往零空间变化。简单来说就是，在残差构建的过程中，得到的是相机pose与路标点landmark之间或者相机之间的相对关系，并没有建立与世界坐标系之间的绝对关系，因此可能因为第一帧的漂移导致整个系统发生漂移。

另一种方法是添加先验，增加系统的可观性。例如约束第一帧相机的pose被固定在$(0,0,0)$原点，即先验信息。在g2o turorial中对第一个pose的信息矩阵加上单位阵，$H_{[11]}+=I$，存在$(H_{[11]}+I)\Delta x=b$，即$I\Delta x=0$，使得第一帧的pose不动。但是尺度依然无法确定，常用的办法可以再固定一个landmark，也可以再固定一个相机pose，固定其相对的位移。

优化过程中处理 H 自由度的不同操作方式可参考博客对《On the comparison of gauge freedom handling in optimization-based visual-inertial state estimation》。