cartographer三部曲（二）：Efficient Sparse Pose Adjustment for 2D Mapping

前言：前端多技巧，后端多理论是slam的一个特性。因此要理解本论文需要一定的理论基础。即需要对加权LM算法十分熟悉，对矩阵求导、SO（3）上的导数、稀疏线性方程组求解等理论非常熟悉，还有slam问题中图结构的构造。

总体评价：创新点不大，将视觉中的BA问题直接类比到了激光中，使用了一些工程技巧提高了算法性能。

主要工作：采用和视觉BA问题类似的方法构建图结构进行优化，利用矩阵的稀疏结构对算法进行加速。

优势：

• 考虑约束中的协方差信息使结构更加精确
• SPA对于初始值不敏感，只有非常小的概率陷入局部最优
• 收敛非常快，仅仅需要几次迭代即可
• 不像EKF或者UKF将非线性问题转换成线性问题求解，SPA是完全非线性的估计

算法实现

文中采用著名的L-M算法作为框架，利用了矩阵的稀疏性进行加速求解，称为SPA。

误差公式

优化变量：一系列的全局位姿$c_i=[t_1,{\theta }_i]=[x_i,y_i,{\theta }_i]$

约束：约束是一个节点$c_j$到另外一个参考节点$c_i$的观测量，及相对变换关系${\overline{z}}_{ij}$和协方差矩阵${\Sigma }_{ij}$。

误差计算

假设$c_i$和$c_j$两节点间有一个约束${\overline{z}}_{ij}$，由于${\overline{z}}_{ij}$代表在i坐标系下j节点的测量位置，需要计算出图中节点cjc_jc