Cartographer源码阅读1

在agv公司实习，需学习cartographer算法。

参照大佬帖子Cartographer源码阅读_预准备 - 知乎

本贴仅作为记录自己学习cartographer源码的过程，笔记，感想。

cartographer代码主要包括两部分 cartographer_ros，cartographer，以及一个进行非线性优化的库ceres-solver。cartographer_ros是基于ros的通信机制，获取传感器的数据并转换成cartographer内定义的格式传递给cartographer处理。可以理解为基于cartographer的上层应用，对cartographer进行了一层ROS接口的封装。

代码整体框架：

算法大致分为三个部分，local slam（前端），Global SLAM（后端），回环检测LoopClosure。

Local SLAM部分任务是建立子图Submaps，但前端建图会有部分建图误差，且随着时间累计。该部分的参数定义见/src/cartographer/configuration_files/trajectory_builder_2d.lua和/src/cartographer/configuration_files/trajectory_builder_3d.lua中。

Global SLAM部分主要任务是进行回环检测，采用BBA，Branch-and-Bound Approach （BBA）的方法来解决。具体方法见论文Real-Time Loop Closure in 2D LIDAR SLAM。在3Dladar中，该部分还根据IMU惯导的数据找到重力的方向。对BBA算法的理解：搜索窗口表达成一棵树的形式，树上每一个树枝就是这颗树干节点所有解可能性的划分，而树干对每个树枝都有一个上限得分，当有一个解比子节点（树枝）的得分高时，说明这个树枝不是最优解，就剪掉这个树枝，从而大大加快解题速度。并且在实际问题中，大部分分支的得分是非常低的，通过阈值就剪掉了很多分支。该种方法拿存储空间换时间效率，因为需要预先计算并存储子图每个pixel的得分值，当输入一个节点时，直接寻找即可，所以这个程序很吃内存。

总而言之，前端负责生成较好的子图，而后端将不同的子图以最匹配的位姿合在一起成为总图。

cartographer运行后系统ROS节点，topic，service等情况如下图：

cartographer_node订阅传感器数据，运行算法，生成submap_list, 而cartographer_occupancy_grid_node则订阅了submap_list，并发布栅格地图。

cartographer_node
   |===@订阅的Topic
   |---------scan (sensors_msgs/LaserScan):   解释：传感器数据
   |---------echoes (sensors_msgs/Mu