Efficient Sparse Pose Adjustment（SPA） for 2D Mapping 翻译和总结

Konolige K , Grisetti G , Rainer Kümmerle, et al. Efficient sparse pose adjustment for 2D mapping[C]// 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010.
这是一篇讲解2D建图中的高效稀疏矩阵图优化的文章，谷歌的cartographer激光slam中位姿图优化主要便是基于思想来实现的。文章主要贡献是在$LM$优化过程中使用乔里斯基分解（Cholesky decomposition）实现稀疏$H$矩阵下的状态增量$\Delta$x高效快速求解。

1. 摘要

图优化已经成为一种解决slam问题的主流方法。位姿图是由一系列非线性约束连接机器人位姿点构成，这些约束来自于位姿点周围共同的特征观测。由于直接非线性优化的计算时间与图的大小的3次方成比例增长，优化大型位姿图成为移动机器人一个比较大的瓶颈问题。文章为了解决这个问题，提出一种有效的方法去构建和解决其中的线性化子问题，这是常规直接非线性优化方法中的性能瓶颈所在。作者将文中所提方法命名为SPA（Sparse Pose Adjustment ），实验表明收敛速度和精度都要显著由于其他方法。实验采用大量的室内实际场景地图数据和一个大型仿真数据集进行验证了该方法的有效性。开源的C++代码和数据集都已经对外公开。

2. 要点介绍

LM的核心问题是大型线性稀疏矩阵的求解。文章提出一种采用直接线性稀疏矩阵求解方法来高效计算约束图的大型稀疏矩阵。作者称为稀疏姿态优化（SPA）方法, 因为其属于处理有限数量的pose间约束问题。SPA方法和图优化相结合来解决线性子问题具有如下优势：

• 考虑约束包含的协方差信息，可提高计算精度
• 在增量式和批量式求解中，SPA对于初始参数均比较鲁棒，不容易失败（陷入局部极小）
• 非常快的收敛速度，因为在LM方法中只需要很少的迭代步骤
• 不像EKF或者信息滤波，SPA是完全非线性的，在每一次迭代中均会相对当前pose的所有约束进行线性化
• SPA在增量式和批量式处理时都是有效的

核心思想：文章提出一种采用直接稀疏乔里斯基分解求解线性系统的方法来有效处理2D位姿图的优化问题。线性系统在求解过程中能够有效减小内存消耗和内存泄漏，因此能够显著提升计算性能。

3. 系统结构

目前比较流行解决SLAM问题的方式是所谓的基于图（Graph based）或者基于网络(Network based)的方法, 主要思想是用图来表示机器人历史的测量结果。图的每一个节点表示一个传感器的测量值，或者一个子地图，并同时包含该测量值是在哪个位置获取的位置标签。边表示两个连接节点之间的旋转约束，通过该旋转可以将两个节点的测量值进行对齐。
图优化中，典型的需要考虑两个不同的问题。一个是基于传感器数据确认约束关系，也被称为数据关联问题。因为环境存在模糊性和对称性，这也是非常难的一件事情，通常SLAM系统中的前端（front-end）负责直接处理此数据关联问题。另一个是基于获取的约束关系去校正机器人的位姿从而得到一个一致的环境地图。此部分工作一般在SLAM里称为优化器或者后端（back-end），其目标是寻找一个节点间的配置关系使得节点间约束的测量概率最大。可以将其类比为弹簧-质量块，节点类比质量块，弹簧类比边的约束，弹簧和质量块间的最小能量配置便类比建图问题的一个solver。

在图优化SLAM中，一般将前端和后端分开处理，如上图2所示。这样要求是因为前端需要一个半优化状态的地图去限制潜在约束关系的搜索范围。当前后端的估计越准，前端生成的约束关系越鲁棒而且越快。因此，此优化问题的精度和性能，对整个建图系统具有决定性的影响。
作者文中非常详细的描述了一个高效处理2D位姿图的完整优化方法。并提出该算法可以与其他处理各种传感器的任意前端进行组合使用。为了使得描述更加清晰，文中只是基于激光laser数据描述前端，但是一般的内容都可以直接应用在不同的传感器中。

4. SPA (Sparse Pose Adjustment)

为了优化一系列位姿和约束，作者使用LM（Levenberg-Marquardt）方法作为优化框架，在2D建图中遇到稀疏矩阵时通过特殊的实现来提高效率。相对于计算机视觉中通过LM有效处理相机和特征的稀疏集束优化方法SBA（Sparse Bundle Adjustment), 作者称自己的系统为SPA。

4.1 误差模型

系统的变量是机器人global系下的位姿$c$集合, 其由位移和方位角组成：
$$c_i=[t_i,{\theta }_i]=[x_i,y_i,{\theta }_i{]}^T$$
一个约束是从一个节点$c_i$到节点$c_j$间的测量值。测量所得的节点$c_i$和$c_j$之间在$c_i$系下的偏差称为${\bar{z}}_{ij}$，此偏差同时具有精度矩阵${\Lambda }_{ij}$(测量协方差的逆)。
对于真实的$c_i$和$c_j$间的偏差（待估计量），可以表示为：
$$h(c_i,c_j)=\{\begin{array}{l}{R}_i^T(t_j-t_i)\\ {\theta }_j-{\theta }_i\end{array}\text{\hspace{1em}(1)}$$
上式（1）中$R_i$是${\theta }_i$的2*2的旋转矩阵，$h(c_i,c_j)$便被称为量测方程（描述待估计变量和测量值间关系的方程）。
误差函数对应一个约束，继而形成总的误差如下：
$$\begin{gathered} e_{ij}={\bar{z}}_{ij}-h(c_i,c_j) \\ {\chi }^2(c,p)=\sum _{ij}{e}_{ij}^T{\Lambda }_{ij}{e}_{ij}\text{\hspace{1em}(2)} \end{gathered}$$
上式$h(c_i,c_j)$中角度一般会被归一化为$(-\pi ,\pi ]$之间。

4.2 系统线性化

通过最小化上式（2）中的总误差得到优化的节点位姿$c$。标准的解决方法便是LM，通过在当前位姿附近处迭代线性解来实现。线性求解系统的构建首先将变量$c$堆成一个向量$x$，以及误差函数堆成$v$。接着定义：

构成的LM系统为：
$$(H+\lambda diagH)\Delta x=J^T\Lambda e\text{\hspace{1em}(4)}$$
此处的$\lambda$是一个在梯度下降和牛顿-欧拉法之间正系数因子。梯度下降法更加鲁棒且不容易陷入局部极小，但是收敛比较慢；牛顿欧拉法刚好相反。
对每一次测量$h(c_i,c_j)$矩阵$H$会增加下面四个分量：

上式中$J_i$表示$e_{ij}$相对$c_i$的雅各比，上述分量都是33的矩阵块。等式的右边同时根据每个约束会增加31的矩阵块$J_{c_i}{\Lambda }_{ij}{e}_{ij}$和$J_{c_j}{\Lambda }_{ij}{e}_{ij}$。
求解完上述线性系统可以得到一个增量$\Delta x$, 将该增量更新到当前的$x$：
$$\begin{gathered} t_i=t_i+\Delta t \\ {\theta }_i={\theta }_i+\Delta {\theta }_i\text{\hspace{1em}(6)} \end{gathered}$$

4.3 误差雅各比

等式(4)中的测量函数$h$的雅各比见下：

4.4 稀疏性

实际大型系统中，位姿pose $c$可能一万多（甚至无限大）。目前的系统变量数量是3||c||， H大小是$||c|{|}^2$，或许会超过$10^8$个元素。对这么大的矩阵进行操作是非常消耗资源的。好在是，对于大部分场景，边构成的约束与poses的数量成线性增长关系，因此H非常稀疏。此处可以利用大型矩阵的稀疏性高效率解决线性系统的求解问题。
作者考虑使用CSparse package以稀疏结构解决等式(4)所构建的线性系统。该功能包包含一个高度优化集成的用于求解稀疏线性系统的乔里斯基分解求解器。当H比较大时，其使用了好几个策略去实现H的有效分解，包括逻辑排序，和一个近似最小度（approximate minimal degree, AMD）算法去重拍排变量。
一般情况下，乔里斯基分解的时间复杂度是$O(n^3)$, n表示变量的数量。对于稀疏矩阵，该复杂度依赖于乔里斯基因子的密度，该因子依赖于H矩阵的结构和变量的顺序。文献【19】的Mahon et al.分析过SLAM系统中乔里斯基因子与回环之间的函数关系。如果回环数量是常量，则乔里斯基因子密度是$O(n)$，对应的分解复杂度也是$O(n)$；如果回环的数量与变量的个数成线性正比例增长，则乔里斯基因子密度是$O(n^2)$，对应的分解复杂度为$O(n^3)$。

4.5 压缩列存储

LM算法的迭代有三个步骤：1）构建线性系统；2）分解H矩阵；3）通过倒转替换寻找$\Delta x$。构建此线性系统与约束的数量成线性关系（同时对于大多数图优化SLAM系统，与变量的数量也成线性关系）。大部分情况下，此过程可能是线性求解器最耗费资源的部分。作者使用一种有效的方法解决由约束生成的等式(5)构造稀疏H矩阵。
CSparse使用压缩列存储（compressed column storage，CCS）格式处理稀疏矩阵。下图列出里其思想：

数组里的每个非零元素被放在val向量里。非零元素按照列排序，再按照行排序。对所有的列，col_ptr对每个列只有一个入口，其数值为上一列数值加上上一个总的非零元素数量。col_ptr指向每一列的起始位置。最后由row_ind给出行号，实现对应行列元素的检索。
CCS格式是一种高效存储大型稀疏矩阵的方式，但是因为每个非零元素的插入某个列都会引起该列所有序列的移动，所以频繁插入时复杂度较高。效率最高也就是以智能列顺序创建稀疏矩阵，这也需要循环遍历约束||c||次。作者提出一种只需要遍历约束一次，并将每个33的矩阵块$J_i^T{\Lambda }_{ij}{J}_i$存储至一个特殊的指向性块数据结构中， 该数据结构与CCS格式基本一致。具体算法如表1所示，通过对约束的一次遍历将33的矩阵块存储进C++的std::map数据格式中，一个map表示一列。maps可以基于他们的键值实现有序高效插入，这便是行索引。一旦如步骤2中的数据结构已经创建完毕，则通过遍历此天然有序的maps转换为H矩阵的稀疏CCS格式。此处在数值插入时将列和行分开进行创建是因为在LM的一组迭代过程中列只需要被创建一次。【此处的操作过程主要是为了提高CCS格式的存储在多次迭代时的快速高效更新，否则同样会影响计算性能】
因为CSpase里的乔里斯基求解器只关注H的上三角元数，因此此处只存储H的上三角部分元素，并假设矩阵时是对称的。

上述表1中是创建稀疏矩阵的过程：
$H=CreatSparse(e,c_f)$
输入：一组约束$e_{ij}$，一组节点（变量）
输出：以CCS格式表示的稀疏上三角H矩阵
1）以$||c_f||$的大小创建该长度的std::map向量vector；每个map与H矩阵对应的列相关联。map的key是行索引号，里面的data是3*3的矩阵块。map[i, j]表示第i个map的第j个入口。
2）对每个约束$e_{ij}$，假设i < j：

1. 如果其不存在，则在下面的步骤中创建map的元素；
2. 如果$c_i$是自由的节点，则令$map[i,i]+=J_i^T{\Lambda }_{ij}{J}_i$；
3. 如果$c_j$是自由的节点，则令$map[j,j]+=J_j^T{\Lambda }_{ij}{J}_j$；
4. 如果$c_i,c_j$都是自由的节点，则令$map[j,i]+=J_i^T{\Lambda }_{ij}{J}_j$；（因为对称，只存储上三角）。

3）创建稀疏上三角矩阵H

1. 在下面的步骤中，忽略3*3map[i, i]中处于下三角区域的元素；
2. 按先列后行的顺序遍历map[]向量，通过其行和列分别对应创建CCS格式中的col_ptr和row_ind；
3. 再次遍历map向量，将其中的值插入到CCS格式中对应的val序列中。

4.6 可持续LM系统

LM算法如下表2所示，对于一组节点$c$和与其关联的测量进行一次流程操作。为了在迭代中间增加更多的节点，在增量式LM中允许运行单次迭代。算法过程是可持续的，$\lambda$在迭代之间是存储的，这样在下一次迭代时可以基于结果调整$\lambda$。内含的思想依据是增加少量的节点和测量不会对系统造成太大改变，因此$\lambda$的值能够体现梯度下降法和牛顿欧拉法的状态。当出现回环时，系统可能无法找到比较好的最小值，此时会在下几次迭代时提高$\lambda$启动系统传向比较好的搜索路径。
对于$\lambda$的调整有许多方法，作者选择了了一种较为简单的方式。系统启动时$\lambda$设置的较小，为$10^{-4}$。如果一次更新之后比上一次的误差更小，则$\lambda$折半；如果误差相同或者更大，则$\lambda$翻倍。这在增量式优化过程中表现得很不错。只要当增加节点时，误差下降，则$\lambda$同样减小，系统保持在牛顿欧拉法得区域里。当一个连接约束得加入导致系统出现比较大不能矫正的干扰时，$\lambda$则增大，系统退回到更加鲁棒的梯度下降法。

上述表2是LM方法单次迭代的过程：
输入：节点$c$和约束$e$，对角线增量$\lambda$
输出：更新后的的节点$c$
1）如果$\lambda$=0， 则将$\lambda$设定为上次运行时的值
2）使用$CreatSparse(e,c-c_0)$构建新的新的稀疏矩阵H，其中$c_0$是固定的pose, 也就是上次优化后的pose，$c-c_0$是新增加的节点；
3）使用基于AMD（Approximate Minimal Degree）的稀疏乔里斯基分解求解线性系统$(H+\lambda diagH)\Delta x=J^T\Lambda e$
4) 使用等式（6）更新新增加的节点$c-c_0$的估计值；
5）如果误差减小，则将$\lambda$折半并存储，并返回更新后的$c-c_0$的poses;
6) 如果误差增加，则将$\lambda$翻倍并存储，并返回更新前的$c-c_0$的poses。

5. 扫描匹配（Scan Matching）

SPA要求从激光扫描的匹配中得到精度估计（协方差的逆）。现有的好几种扫描匹配（scan-match）方法均可以提供此信息，比如Gutmann等使用激光点去匹配在参考扫描帧中提取出来的线从而得到一个误差的高斯估计。还有Konolige and Chou提出的相关匹配（correlation method）方法, Olson[22]对其进行了拓展，该方法基于给定的一帧扫描可以得到最优的全局匹配结果，同时返回精确的协方差。该方法也允许单帧扫描和多帧对齐的扫描之间相互进行匹配。SRI的建图系统$Karto$[12]在序列扫描的局部匹配中和多帧扫描的回环匹配中均使用了此方法。
为了产生实际场景的数据用于实验，作者使用63个不同大小的离线存储的机器人数据运行$Karto$，使用该方法的扫描匹配和优化器建图并产生约束，包括回环。该图被离线存储，并在实验部分对所有的对比方法作为输入进行比较。

6. 实验

作者将提出的SPA方法与当时比较先进的方法在63个真实场景的数据和一组比较大的仿真数据中进行比较。其他对比方法如下：

• 信息滤波器：DSIF[7]
• 随机梯度下降：TORO[10]
• 分解非线性系统：Treemap[8]
• 稀疏位姿优化：SPA，包含 a) 稀疏直接乔里斯基求解器， 和 b) 可迭代PCG(Preconditioned Conjugate Gradient, 预调节共轭梯度法)[15]

作者使用上述相同的“可持续LM”步骤作为SPA更新了PCG中的实现。唯一的不同是线性求解器。预调节器（preconditioner）是一个不完善的乔里斯基方法，而且以稀疏矩阵的格式实现了共轭梯度。
作者同样评估了一个稠密乔里斯基求解器，但是计算复杂度和内存消耗都是其他方法的好几倍。比如，1600个约束和800个节点使用一次稠密乔里斯基分解一次迭代需要2.1秒，其他方法只需要几毫秒。所有使用均在2.67GHz的Inter Core i7-920上运行。

6.1 精度测量

对于室内数据集，没有真值。一个位姿约束系统测量的好坏可以使用约束的权重协方差平方误差，或者使用${\chi }^2$来进行体验。如果扫描的匹配是精确的，则${\chi }^2$会更小，表示扫描结果对齐的更好。下图3展示了实际数据中${\chi }^2$的不同在对齐上的体现：

6.2 离线优化

因为离线优化强烈依赖于初始值，作者使用了当时两种常用的初始化策略进行：

• 里程计方法：图中的节点使用增量的约束进行初始化。这是在大部分图优化中使用的标准方法。
• 旋转树方法：旋转树基于图的广度优先遍历进行构建。树的根节点是图中的第一个节点。新节点的位置初始化根据所构建的旋转树深度优先遍历实现。节点孩子的位置根据连接约束设置为转换后的双亲节点位置。在作者的实验中，此初始化方法表现最好。
  测试结果如下：
  
  此处的离线数据测试，两种初始化方法没有太明显的差异。左图中PCG和SPA重合，可见SPA在保证优化精度的情况下，计算复杂度最小。

6.3 在线测试

在线实验中， SPA同样在保证最优优化精度的情况下，计算时间消耗远小于其他方法，且随时间变化更加稳定。

6.4 大的对称结构环境测试

上图可以看出在大型复杂环境中，基于旋转树初始化新节点的PCG和SPA均可以较好的完成优化过程，SPA的时间复杂度更小。使用里程计方式初始化的PCG和SPA均不能较好地完成优化。

7. 结论

作者提出和验证了一种用于2D位姿图优化的有效非线性优化方法，称为SPA（Sparse Pose Adjustment）。SPA依赖于高效的线性矩阵构建和稀疏非迭代乔里斯基分解去有效表示和求解大型稀疏位姿图。大的全局地图优化需要几秒，但是实时的运行计算最糟糕的时候也可以达到10ms/节点。
与其他方法相比，SPA更快且收敛更好。唯一的缺点是初始化参数不好时会导致失败，但是可以与旋转树初始化方式进行配合处理和优化，甚至可以在大型复杂包好对称结构的环境中运行。与一个扫描匹配前端进行配合，SPA同样可以实现实时的地图探索和构建。同时，SPA也可以进行地图拼接和删减，可以用来维护长期的建图过程。