cartographer算法代码结合论文思路分析

cartographer代码入口：

global_trajectory_builder.cc文件下的AddHorizontalLaserFan函数，

其中分为两部分：前端AddHorizontalLaserFan{}

后端sparse_pose_graph_->AddScan{}

一、前端

所用帧间匹配算法：相关匹配算法优化初始位姿，再使用ceres库优化

代码流程：

scan数据直接应用于帧间匹配

 odom/imu数据   通过位姿外推器（松耦合）得  预测位姿

预测位姿作为初始位姿   加入 相关匹配方法（注在本阶段不需使用分支定界，因为在小范围枚举）

得到的位姿 再次传入 ceres优化得到最终位姿

在此位姿下实现激光数据帧插入submap中，至此前端较为重要的流程已经完成

（注：因为前段的位姿是通过增量式计算得到的，所以会产生累计误差，因此后端优化就由此产生以消除累计误差）

二、后端

后端回环包括激光数据帧和过去所有submap实现匹配的过程，匹配成功就计算相对位姿和得分，根据得分判断是否需要调用优化。

对于后端回环检测和优化理论不理解可以参考SPA优化算法详解：以Cartographer后端为例 - 知乎

后端的代码流程：

每加入一帧激光帧，调用ComputeConstraintsForScan（）先是约束计算，计算和当前和前一个submap的相对位姿实现约束加入（注：此时约束是用前端得到的是带有误差的位姿和submap的相对位姿）

然后是回环检测（触发条件：新scan和新完成submap）：                                                            在调用ComputeConstraintsForScan（）中调用MaybeAddGlobalConstraint（）枚举已经完成的submap，实现真实约束计算其实就是加入的scan和过去所有submap匹配过程。

for(所有submap)

//*********************************************

而这个匹配过程使用了多分辨率建立多个地图，实现分支定界加速（现在粗分辨率中找到激光点落在地图中的最高得分，再在最高得分的栅格上继续细分分辨率，一直到最细分辨率，由于精度受限于分辨率因此无法得到更精确的位姿，所以有优化方法补充）

最后使用优化的方法得到scan在global地图中的真实位姿（前端得到的是带有误差的位姿），再计算与当前submap的约束。

submap++；

//*****************************************

当节点到达设定数量时，调用优化。

误差函数：

真实位姿与submap的相对位姿  减去  前端得到的是带有误差的位姿和submap的相对位姿