前言：前端多技巧，后端多理论是slam的一个特性。因此要理解本论文需要一定的理论基础。即需要对加权LM算法十分熟悉，对矩阵求导、SO（3）上的导数、稀疏线性方程组求解等理论非常熟悉，还有slam问题中图结构的构造。
总体评价：创新点不大，将视觉中的BA问题直接类比到了激光中，使用了一些工程技巧提高了算法性能。
主要工作：采用和视觉BA问题类似的方法构建图结构进行优化，利用矩阵的稀疏结构对算法进行加速。
优势：
- 考虑约束中的协方差信息使结构更加精确
- SPA对于初始值不敏感，只有非常小的概率陷入局部最优
- 收敛非常快，仅仅需要几次迭代即可
- 不像EKF或者UKF将非线性问题转换成线性问题求解，SPA是完全非线性的估计
算法实现
文中采用著名的L-M算法作为框架，利用了矩阵的稀疏性进行加速求解，称为SPA。
误差公式
优化变量：一系列的全局位姿 c i = [ t 1 , θ i ] = [ x i , y i , θ i ] c_i=[t_1,\theta_i]=[x_i,y_i,\theta_i] ci?=[t1?,θi?]=[xi?,yi?,θi?]
约束：约束是一个节点 c j c_j cj?到另外一个参考节点 c i c_i ci?的观测量，及相对变换关系 z ? i j \overline{z}_{ij} zij?和协方差矩阵 Σ i j \Sigma_{ij} Σij?。
误差计算
假设 c i c_i ci?和 c j c_j cj?两节点间有一个约束 z ? i j \overline{z}_{ij} zij?，由于 z ? i j \overline{z}_{ij} zij?代表在i坐标系下j节点的测量位置，需要计算出图中节点 c j c_j cj?在 c i c_i ci?系下的位置
h ( c i , c j ) ≡ { R i ? ( t j ? t i ) θ j ? θ i h(c_i,c_j) \equiv \left\{\begin{array}{l} R_{i}^{\top}\left(t_{j}-t_{i}\right) \\ \theta_{j}-\theta_{i} \end{array}\right. h(ci?,cj?)≡{
Ri??(tj??ti?)θj??θi??
两者相减便是误差
e i j ≡ z ? i j ? h ( c i , c j ) e_{ij} \equiv \overline{z}_{ij}-h(c_i,c_j) eij?≡zij??h(ci?,cj?)
最终得到最小二乘误差为
E ( c , p ) = ∑ i j e i j T Σ i j ? 1 e i j = ∑ i j e i j ? Λ i j e i j E(c,p)=\sum_{ij}e_{ij}^T\Sigma_{ij}^{-1}e_{ij} = \sum_{i j} e_{i j}^{\top} \Lambda_{i j} e_{i j} E(c,p)=ij∑?eijT?Σij?1?eij?=ij∑?eij??Λij?eij?
线性系统
采用LM算法求解该非线性最小二乘问题需要在迭代过程中不断求解如下线性方程
( H + λ diag ? H ) Δ x = J ? Λ e (\mathbf{H}+\lambda \operatorname{diag} \mathbf{H}) \Delta \mathbf{x}=\mathbf{J}^{\top} \mathbf{\Lambda} \mathbf{e} (H+λdiagH)Δx=J?Λe
其中 H = J ? Λ J \mathbf{H}=\mathbf{J}^{\top}\mathbf{\Lambda}\mathbf{J} H=J?ΛJ,因此只需要求解误差的雅可比即 J \mathbf{J} J即可构建方程并求解。
误差Jacobians
? e i j ? t i ≡ [ ? R i ? 00 ] ? e i j ? θ i ≡ [ ? ? R i ? / ? θ i ( t j ? t i ) ? 1 ] ? e i j ? t j ≡ [ R i ? 00 ] ? e i j ? θ j ≡ [ 001 ] ? \begin{array}{l} \frac{\partial e_{i j}}{\partial t_{i}} \equiv\left[\begin{array}{c} -R_{i}^{\top} \\ 00 \end{array}\right] \quad \frac{\partial e_{i j}}{\partial \theta_{i}} \equiv\left[\begin{array}{c} -\partial R_{i}^{\top} / \partial \theta_{i}\left(t_{j}-t_{i}\right) \\ -1 \end{array}\right] \\ \frac{\partial e_{i j}}{\partial t_{j}} \equiv\left[\begin{array}{c} R_{i}^{\top} \\ 00 \end{array}\right] \quad \frac{\partial e_{i j}}{\partial \theta_{j}} \equiv[001]^{\top} \end{array} ?ti??eij??≡[?Ri??00?]?θi??eij??≡[??Ri??/?θi?(tj??ti?)?1?]?tj??eij??≡[Ri??00?]?θj??eij??≡[001]??
将其组合起来为
J i j = [ ? ? e i j ? c i ? ? e i j ? c i ? ] \mathbf{J}_{ij} = \left[\begin{array}{c} \cdots & \frac{\partial e_{ij}}{\partial c_i }\cdots \frac{\partial e_{ij}}{\partial c_i }\cdots \end{array}\right] Jij?=[???ci??eij????ci??eij????]
J = ∑ i j J i j \mathbf{J} = \sum_{ij}\mathbf{J}_{ij} J=ij∑?Jij?
稀疏性
由于在优化过程中可能有几千个节点同时被优化，这就导致LM算法在求解过程中对超大矩阵 ( H + λ diag ? H ) (\mathbf{H}+\lambda \operatorname{diag} \mathbf{H}) (H+λdiagH)求逆时需要花费大量的时间，好在由于SLAM问题构成的图具有稀疏性，通常可以将算法复杂度从 O ( n 3 ) O(n^3) O(n3)降低到 O ( n ) O(n) O(n)。
压缩储存列
采用CSS 格式保存稀疏矩阵使得矩阵占用内存大大减少，其中CSS格式就是只保存非零项以及非零项的位置。
可持续的LM系统
大概简述了一下LM算法的流程