Cartographer总结

Cartographer总结

由架构图知，Cartographer的建图过程是先获取scan输入，对其进行“体素过滤”下采样后再进行“自适应体素过滤”后进行scan matching(scan-to-submap)寻找scan在submap中的估计的最佳插入位置(本质是一个非线性最小二乘问题（使用双三次插值将二维的栅格像素位姿转为一维的像素概率得分）)，使用滑动窗口更新submap。由于非线性二乘初始值越好结果越好，因此可以融合IMU和Odom的数据提供更好的初始值。
在通过CS公式计算得出scan插入submap的最优的位姿后(即前端Local SLAM做的事)。如果没有误差，那SLAM已经做完了。虽然submap中scan之间的误差很小可以忽略，但随时间推移，后面的submap中的scan与之前的submap之间的累积误差会很大。因此，需要对其进行优化，采用的优化算法是SPA(稀疏位姿调整)，优化submap的全局位姿、scan的全局位姿、scan在submap中的插入位姿，约束是scan在submap中的位姿及两者间的协方差矩阵（实际的优化对象是Cartographer中的节点node（“scan的全局位姿”）。因为无论submap还是map地图，本质上都是激光scan在栅格化的地图中的观测结果，所以优化目标就是优化scan在全局坐标下的位姿，优化完所有的scan也就优化完了整个地图。顺便一提，Cartographer项目中使用自定义的容器MapById来存储需要优化的submaps和nodes）。如果检测到“回环约束”的话优化效果会更好，加速“回环检测”的方法是使用“预计算栅格像素约束得分”和“分支定界BBA”加速回环的检测，使用这两种方法加速的“回环检测”能够在当前scan插入submap之前就检测到回环而且做完对整个地图的全局优化，实现了实时的SLAM建图。这种高效率的回环检测方法就是Cartographer这个SLAM系统的最大创新之处，至于分支定界的“节点选择”(也就是某个scan全局位姿的选择)、“分支规则”、“上界计算”详见论文——“Real Time Loop Cluser in 2D Lidar SLAM”。
Cartographer性能：通过实验验证了Cartographer这种通过scan-to-submap并进行使用基于BBA和“栅格预计算”的回环检测，在检测到回环约束就进行Global SLAM全局优化的算法可以在较低的算力平台以及较低成本的传感器硬件上进行大场景的实时高精度SLAM建图定位。
在特征丰富的室内场景Cartographer可以有很好的实时高精度建图效果。但在特征稀疏的对称结构场景(eg.对称的长廊和办公室隔间)或室外场景性能退化严重（因为很容易出现“回环”的错误检测进行全局优化导致地图偏移）。之所以长廊办公室隔间等没有明显特征区别的环境导致Cartographer性能退化的原因是，在长廊或办公室隔间这种对称环境中行进时当前帧和之前的在长廊的帧都没有明显区别，所以只依赖scan会导致scan-to-submap匹配无法正确匹配也会导致错误的“回环检测”引发错误的全局优化，导致建图精度下降。

关键词：栅格占用概率&概率更新、CS(Scan-to-submap)、BBS(Loop Closer)、SPA(submap位姿、scan位姿、插入位姿、submap与scan之间协方差矩阵、Huber损失函数)