Cartographer 代码逻辑

先在此声明一下：这篇文章是在泡泡机器人的公众号文章Cartographer理论及实现浅析的基础上进行学习的，所以必须先感谢一下泡泡机器人对这一块的贡献！

---

整体理论概述

还是先放一张图：

大家看一下这张图，里面有scan与submap的匹配，这部分的匹配方法是有scan_matching 中的RealTimeCorrelationScanMatcher实现的。还有一个匹配是在完成闭环检测的全局位姿优化的时候会涉及到点集的匹配（后台程序），这个部分的点集匹配算法使用的是FastCorrelationScanMatcher，两个部分都会涉及到优化算法，优化算法都是使用CeresScanMatcher来实现的。
以上就是整个的系统概括了。这其中还有很多细节问题需要学习。（BTW:到现在我对于整个系统还是有点懵的，只能继续钻代码了！我觉得我这个智商来学习机器人简直拉低了整个队伍的智商….我有罪….）

---

Cartographer的代码结构

1、整体文件说明

这个部分还是在泡泡机器人（真的很良心的公众号！！）的文章的基础上进行学习的。

---

• common：定义了基本数据结构以及一些工具的使用接口。
• groundtruth: 这部分是系统入口（???似乎是这样的，反正包含了main函数，但是其实cartographer只是为了cartographer_ros提供接口而已，所以那只是一个测试的吧。我是这么理解的。 有大神指出这部分其实最开始的开源是没有的，具体的参见评论。目前关于cartographer的研究因为项目原因暂停一段时间了，有时间希望还能继续！ ）
• io：这部分就是基本的输入输出函数
• kalman filter:主要通过kalman滤波器完成对IMU、里程计及基于雷达数据的估计位姿的融合，进而估计新进的laser scan的位姿。(需要注意一下，虽然论文中说了没用particle filter，但是用了kalman filter，这两者是有区别的，后者的kalman filter是采用加权求平均的方法估计位姿).
• mapping:定义了上层应用的调用接口以及局部submap构建和基于闭环检测的位姿优化等的接口。
• mapping_2d(3d省略)：对mapping接口的不同实现（其实就是最终画出的图是2d的还是3d的）。
• sensor：定义了雷达数据及点集等相关的数据结构。

• transform：定义了位姿的数据结构及其相关的转换。

  ---

  以上部分很多在别人看来都是多余的（比如io，groundtruth，），所以有人做了简化版的cartographer，见hitcm大神的blog
  接下来就是要读源码的时间了，表示内心很是崩溃呀！无从下手呀！

  ---

2、mapping_2d代码逻辑结构

关于里面有很多的proto文件，这儿进行一些说明。这里的这些proto文件呢是google的Protocol Buffers，它是Google出品并开源的语言和平台均中立的数据序列化和反序列化工具。详情参见这个blog 其实就是一些序列号，反序列化的通信文件。其实也是一些接口文件。

2.1 cartographer::mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder类主要实现了接收处理上层应用传递的传感器数据的主要接口：

• （1）AddImuData用于接收处理上层应用传递的IMU数据。
• （2）AddOdometerPose用于接收处理上层应用传递的里程计数据。
• （3）AddHorizontalLaserFan用于接收处理上层应用传递的雷达数据。

其中包含重要的对象成员：

• （1）cartographer::mapping_2d::LocalTrajectoryBuilder类的对象local_trajectory_builder_用于完成局部submap的构建。
• （2）cartographer::mapping_2d::SparsePoseGraph类的对象sparse_pose_graph_用于完成闭环检测及全局位姿优化。

在AddImuData和AddOdometerPose函数的实现中会将接收的相应传感器数据传递给local_trajectory_builder_对象处理。在AddHorizontalLaserFan函数的实现中则将新进的laser fan传递给local_trajector_builder_对象用于局部submap构建，如果该laser fan被成功插入到某个submap，那么该laser fan被插入后的相关信息则被传递给sparse_pose_graph_对象用于基于闭环检测的全局位姿优化。

2.2 mapping_2d::local_trajectory_builder

这个类主要的任务是完成了submap子图的构建。它是基于proto中的LocalTrajectoryBuilderOptions这个接口实现的。（这个里面涉及几个问题，一是子图的构建什么时候结束？二是在优化位姿的时候是否修改了IMU的位姿信息？）

---

cartographer::mapping_2d::LocalTrajectoryBuilder类主要完成局部submap的构建。其提供了接收处理传感器数据的public函数：

• （1）AddImuData用于处理IMU数据。
• （2）AddOdometerPose用于处理里程计数据。
• （3）AddHorizontalLaserFan用于处理雷达数据。
• （4）一个public的struct InsertionResult。

以及包含了一些重要的private成员：

• （1）ScanMatch成员函数基于submap已有的laser fan估计当前laser fan在submap中的位置。被AddHorizontalLaserFan函数调用。
• （2）cartographer::kalman_filter::PoseTracker类的对象pose_tracker_用于融合基于雷达数据的laser fan的局部估计位姿、IMU数据以及里程计数据，进而估计出较优的laser fan的位姿。

在AddImuData和AddOdometerPose函数中会将IMU数据和里程计数据传递给pose_tracker_进行处理。pose_tracker通过UKF不断融合IMU和里程计数据进而更新当前位姿，因此通过pose_tracker可以获取当前laser fan的估计位姿的一个较好的初始化值。进一步的，在AddHorizontalLaserFan函数中会调用ScanMatch，ScanMatch函数中通过在submap中局部匹配（匹配的算法是采用Ceres方法进行优化的）得到的当前laser fan的估计位姿被pose_tracker_用来调整（就是优化的过程）该laser fan的初始化值（这个初始化值是由IMU和里程计数据得到的）。这样pose_tracker_通过融合多传感器数据，进而能够估计出较优的laser fan的位姿。
主要是看LocalTrajectoryBuilder.h文件里面都声明了哪些变量和函数。

整个LocalTrajectoryBuilder的关系调用图：

---

在以上的代码中，我还是没找到对于submap的子图的匹配是在什么时候结束的？只是在论文中有说：闭环检测是在一个新的scan点集被添加进入地图之前进行闭环检测。实现这个采用的是分支定界以及每个子图对应几个预处理过的网格的方法。

2.3 mapping_2d::sparse_pose_graph

这个算法实现的是为了优化所有点集和子图的位姿。这个优化的位姿位置怎么得到的呢？这个位置会被存储起来用于之后的loop closing优化。
cartographer::mapping_2d::SparsePoseGraph类主要完成基于闭环检测的全局位姿优化。其提供了接收处理新进被插入到submap的laser fan相关信息的public函数：

• （1）AddScan 对新进的laser fan进行闭环检测及在适当的时候进行全局优化。

以及一些重要的私有成员：

• （1）ComputeConstraintsForScan对新近laser fan信息进行处理并启动闭环检测scan
  match以及计算其约束，进而将约束添加到位姿优化目标中。
• （2）AddWorkItem将laser fan与ComputeConstraintsForScan绑定，并将任务加入到队列中。
• （3）HandleScanQueue依此调度队列中的任务。
• （4）sparse_pose_graph::ConstraintBuilder constraint_builder_ 用于完成laser
  fan的scan match以及约束计算。
• （5）RunOptimization优化目标。

在AddScan函数中会将laser fan相关信息与ComputeConstraintsForScan函数绑定，并将绑定好的任务通过AddWorkItem函数加入到队列中。HandleScanQueue函数则依次调度队列中的任务。第一次调用AddWorkItem时会直接启动ComputeConstraintsForScan任务，且在第一次ComputeConstraintsForScan任务时启动HandleScanQueue调度。在ComputeConstrainsScan中，通过constraint_builder_对象完成闭环检测的scan match以及约束计算。当所有约束计算完成时，则会进行RunOptimization优化目标。

2.4 ScanMatch

LocalTrajectoryBuilder中的scan match策略与SparsePoseGraph中的scan match策略是不同的。前者使用scan_matching::RealTimeCorrelativeScanMatcher，后者则使用scan_matching::FastCorrelativeScanMatcher。二者的目标优化均是由scan_matching::CeresScanMatcher完成。

晕呀晕呀晕呀！