cartographer 代码思想解读（19）-算法思想总结

引入cartographer算法官方框架图。

从cartographer官方结构框图可看出，其算法整体结构遵从slam基本结构，包括前端处理和后端处理两部分。其中前端处理即cartographer中的Local SLAM，其主要功能是输入传感器信息通过scan-match获取帧间相对位姿关系，构建submap并获取laser-scan与submap相对位姿关系；而后端处理即为cartographer中的global slam。主要将位姿推移类传感器和前端结果按时间顺序进行串联起来形成一条轨迹，寻找闭环关系，最终采用优化求解获取位姿总体误差最小时的位姿轨迹即submap位姿。

前端处理整体思想

slam前端主要采用scan-match的相关技术，计算相邻激光点相对位姿，若已知初始位姿，则可获取所有scan节点全局绝对位姿。因其原理原因，会引入累计误差，但由于相对位姿较为精确，短时间内累加误差较小，cartographer采用一段时间内的scan构建submap而非整个map；每个submap相互独立，并且采用scan-to-map思想进行scan-match进一步降低累计误差，其结构如下所示。

从图中可看出N个连续scan构建submap0，然后接下来的N个连续scan构建新的submap1，以此类推。前端可获取每个scan的相对于对应submap的位姿，和submap在全局的位姿。

前端处理流程

1. 传感器预处理：主要将测距传感器即激光传感器信息转换为cartographer内部定义的统一类型：点云类型，而非ros中的range类型；
2. 激光点云数据经过两次体素滤波处理，即降采样；
3. odometry、imu及其scan-match精确位姿作为历史轨迹，构建位置估计器实时估计下刻位置；其中odometry和imu在2d slam非必要输入，可完全采用纯激光激光匹配估计。
4. 根据预估位置作为初始位姿，经相关匹配（可配置使能或禁能）和ceres优化匹配获取精确的位置；
5. 经过运动滤波器（即降采样）的scan节点作为slam轨迹节点；
6. 将轨迹节点scan即及其对应的点云进行维护和更新submap；
7. 当submap满足一定数量的激光帧时，完成此submap的创建，输出MatchingResult类型结果；
8. 重新累计新的scan node构建新的submap，以此循环；

后端处理

slam后端主要解决slam前端带来的累计误差，主要包括闭环检测和图优化求解两大部分。其中图优化求解目前一般都采用开源优化库，如google的ceres库和国内常用的g2o库；故目前slam质量的好坏，闭环检测成为了重中之重。如下给出cartographer进行后端优化位姿图结构拓扑，前端给出了相邻scan node的位姿约束，同时也给出了submap与组成其的scan node的约束关系，如下图在前端基础上增加了黑色线即为闭环检测提供的闭环约束。

在进行图优化求解前，构建位姿图。其中红色和绿色约束由前端匹配结果获取；而黑色线约束，即scannode 与历史的submap（非自身构建的submap）之间的相对位姿；或submap与历史的scan node（除构建自身的其他scannode）之间的相对位姿。

闭环检测

闭环检测本质仍为scan-match，只是采用当前scannode匹配历史的submap而非当前时刻的submap；或者采用当前完成的submap去匹配历史的scannode。当匹配的置信度（即可认为是吻合度）达到一定值时，可认为闭环存在，增加其约束关系。

由于闭环检测是检测当前位姿是否经过曾经经历的地方，和前端匹配本质区别，无法获取较为准确的初始位姿。因其搜索范围较大，则不适合直接使用real_time_correlative_scan_match。cartographer优化了相关匹配算法，提出了FastCorrelativeScanMatcher（即分支限界加速匹配法），实现快速搜索闭环可能性。
如果存在闭环，即匹配可信度超过设置阈值，将匹配出的位姿作为初始值送入ceres库获取更加精细的匹配结果，其结果即为闭环约束。

后端处理流程

1. 将scannode和submap作为优化节点，加入位姿图；
2. 从前端匹配结果中获取相邻scannode节点间约束和submap内部的submp与scannode约束；
3. 创建闭环匹配器进行闭环检测，增加闭环约束；
4. 构建位姿图优化器进行优化；
5. 将优化结果将旧值进行更新；

由于后端处理不需要实时处理，为保证前端的实时，后端采用线程池在后台处理；因此优化的结果替换时无法替换所有scan node值，需将优化后最新的位姿转移矩阵，去估计前端中未被优化的节点。否则可能会导致在地图最新时间附近错位。

总结

至此，完成了cartographer源码和slam原理分析；如果已有slam图优化基础，可不用完全阅读所有源码，主要了解和分析cartographer中的submap的构建和维护方法和分支限界加速匹配闭环检测两部分即可，也是cartographer区别于其他开源slam算法特别部分。

注：cartographer所有分析仅是个人理解，由于通常调用较多，源码修改较少，因此深层次原理并非完全理解。如理解不正确的，望指出，谢谢