Cartographer源码阅读

---

（一） 定义的各种坐标系间的关系
1. map_frame = “map”,cartographer中使用的全局坐标系，最好保持默认，否则ROS的Rviz不认识其它的定义，导致无法可视化建图过程。
2.tracking_frame=”base_link”,机器人中心坐标系，很重要，其它传感器数据都是以这个为基础进行插入的。cartographer_ros里面有个tf_bridge的类就是专门用来查询其它坐标系到此坐标系的转换关系的。
2. published_frame = “base_link”
3. odom_frame = “odom” ，3与4是配合使用的，如果参数provide_odom_frame = true 那么最后可视化时，发布的转换消息是从 published_frame->odom_frame->map_frame, 也即cartographer内部计算出了未经回环检测的局部图坐标到优化后的全局图坐标之间的转换关系并发布了出来。在跑官网的二维背包例子时，在map坐标周围一直跳动的odom就是这个玩意。

---

（二） 关键代码说明
cartographer_ros中的node_main.cc中的run()是整个工程的入口，里面完成了一系列外部消息的订阅，并将之与回调函数绑定。通过SensorBridge的HandleLaserScanMessage（还有类似几个回调函数）将数据传输到cartographer内部进行处理。

cartographer中的几个关键类：
TrajectoryBuilder是一个虚基类，CollatedTrajectoryBuilder是它唯一的继承者，GlobalTrajectoryBuilderInterface也是一个虚基类，它的继承者分别是二维和三维实现中的mapping2d::GlobalTrajectoryBuilder和mapping3d::GlobalTrajectoryBuilder.以二维为例。

所以外部（SensorBridge的HandleLaserScanMessage）在调用AddData方法时都是实际上在调用CollatedTrajectoryBuilder的具体实现，在CollatedTrajectoryBuilder类的构造函数中有一个回调函数的绑定，即该类内部维护了一个数据预处理的辅助类sensor::Collator，通过预处理后的数据再被送入CollatedTrajectoryBuilder类的HandleCollatedSensorData函数中，该函数再利用成员类变量：由mapping2d::GlobalTrajectoryBuilder实例化的GlobalTrajectoryBuilderInterface类实际上完成数据的添加。

此时数据已经传送到了GlobalTrajectoryBuilder中，而该类的内部则一方面调用LocalTrajectoryBuilder进行局部图（未经回环检测的图）的构建，另一方面将数据送入其SparsePoseGraph中进行回环检测，添加约束，整体优化轨迹位姿。

LocalTrajectoryBuilder：Predict()函数接收IMU数据进行进行一个位置的初始估计，然后将之交给ScanMatch函数调用real_time_correlative_scan_matcher或者ceres_scan_matcher进行位置和姿态的进一步修正。

real_time_correlative_scan_matcher需要提供一个大致的位姿，然后在之附近开一个参数搜索窗口，通过计算不同参数下投影后的数据与概率图的匹配度（累加投影点的概率值，思想很简单，如若该组参数好，那么投影后吻合最好，概率值应该最大）得到最后的参数。ceres_scan_matcher则是利用梯度下降直接修正了，比较依赖初始位姿。

除了上述的用于局部位姿解算的scan-match方法，在scan_matching文件夹下还有一个用于scan-matching的重要类FastCorrelativeScanMatcher，该类主要用在回环检测上，其有两个接口一个用于基于submap的位姿求解，一个用于给定初值的位姿求解，两个函数的内部实际上都是利用分支定界法（BranchAndBound）进行递归求解位姿参数，貌似是个树的递归搜索方法，所以速度上贼快，大概是个以空间换时间的思路，得先建好概率图金字塔。（后面看回环部分再详细补充吧）

---

2017-07-10更新
新放出的代码更新了pure-localization和long-term-slam，目前的观察来看，long-term-slam和pure-localization沿用了以前的机制：
long-term-slam: 在开始更新新的trajectory时，首先load上一次建图保存的pose-graph(submap_pose+trajectory_node_pose)、submap(probability_grid)，然后将它锁死不进行更新，只用作回环检测添加约束用。然后新建一条trajectory，并沿用之前的建图模式，前端线程更新最新的submap，后端线程进行新trajectory
里的所有submap位姿和测站（trajectory_node）位姿回环优化。

pure-localization: 类似long-term-slam，先load缓存的上一次建图数据并锁死不进行更新，新建的trajectory保留3张最新的submap作匹配定位。

附：
long-term-slam的运行方法，设置pure-localization=false