cartographer 代码分析

相关注释代码链接为：cartographer代码注释

代码主要分为两个部分，其一为cartographer的核心实现，另一个为cartographer的ros封装壳。首先介绍其ros封装，可以看到大概的调用流程，然后再深入源码去剖析其实现过程。但是其代码可以说十分的繁琐且复杂，在只能大致理清楚其逻辑。

Cartographer-ROS

根据运行的命令 roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag可以知道，运行文件offline_backpack_2d.launch启动算法。

• 加载配置文件 backpack_2d.lua
• 调用launch文件 offline_node.launch

因此，深入文件 offline_node.launch

• 运行节点 rviz
• 运行节点 cartographer_occupancy_grid_node
• 运行节点 cartographer_offline_node

因此对于cartographer-ros来说，主要的节点就是这两个，其在如下文件中分别实现

• offine_node_main.cc
• occupancy_grid_node.cc

occupancy_grid_node

主要实现的功能有

• 新建一个定时器，定时发布全局地图信息
• 构造回调函数，在回调函数内处理子地图列表信息

其子列表信息回调函数流程如下：

• 设置所有之前的子地图为待删除子地图
• 在待删除子地图中去掉当前子地图列表中还存在的
• 获取新子地图信息 FetchSubmapTextures
  • 发布一个srv，获取压缩后的子地图栅格信息
  • 对栅格地图信息进行解压
• 转换子地图信息格式

而发布流程则为

• 将所有子地图构造成一个图片(调用cairo实现)
• 转换为ROS格式并发布

offine_node_main

在main函数中

• 调用函数CreateMapBuilder构造了一个MapBuilder类
• 调用RunOfflineNode函数，传入MapBuilder类指针

其中，RunOfflineNode函数则为离线运行节点的主要逻辑

可以从图中看到，函数主要工作为

• 调用AddOffineTrajectory()函数，实际上该函数主要调用的是map_builder类的AddTrajectoryBUilder函数。
• 创建ROS相关的发布以及订阅消息的处理，并定时发布可视化信息
• 构造SensorBridge类，并处理传感器数据,实际上则是调用TrajectoryBuilderInterface类的传感器数据处理
• 读取参数配置文件，保存地图信息等其他工作

从输入输出的角度来看整体的代码，我们需要弄清楚本节点发布的ROS数据具体有哪些，是如何获得的，并且是如何处理这些传感器数据。

输入

• 传递传感器数据到核心cartographer代码
  直接由SensorBridge类使用TrajectoryBuilderInterface类指针调用其传感器数据处理函数。

输出

• 发布消息到ROS
  • 定时发送轨迹、子地图列表、约束项等信息
  • 在请求时返回子地图的栅格地图数据,调用函数HandleSubmapQuery，实际上调用的是map_builder_->SubmapToProto函数获取压缩后的栅格信息

因此，整理如下

• 调用map_builder类的AddTrajectoryBuilder函数,进行初始化
• 调用TrajectoryBuilderInterface类相关的传感器处理函数处理传感器信息
• 调用map_builder_->SubmapToProto函数等获取处理结果

最终，我们可以看到实际上交互的内容并不多，在获取栅格地图信息上由于数据量较大进行了数据压缩，其他的都是直接通过指针获取得到数据。因此我们接下来看主体代码时只要集中在前面两点上即可。

Cartographer 主体

根据上文结论，接下来分为两个部分进行介绍。

调用map_builder类的AddTrajectoryBuilder函数

这里根据配置参数的选择，才有了2D激光数据和3D激光数据的区别，根据不同的传感器数据配置2D或者3D对应的类，由于在代码上没有太大的区别逻辑都是几乎一模一样的（在处理IMU数据上有一些区别，3D激光必须要IMU,2D可以不要）。因此后面都是以2D类举例解释。

1. 构造局部轨迹生成类 LocalTrajectoryBuilder2D
2. 使用局部轨迹生成类，构造全局轨迹生成类 GlobalTrajectoryBuilder
3. 使用全局轨迹生成类，构造轨迹生成管理类指针 CollatedTrajectoryBuilder
4. 将轨迹生成管理类指针保存在一个vector中

这里就非常的绕，需要仔细思考，所有的轨迹生成类均为TrajectoryBuilderInterface的子类，因此需要特别注意，使用TrajectoryBuilderInterface类指针是具体指向的是哪一个子类的实现。最后返回的是CollatedTrajectoryBuilder类的指针，因此下面调用的是该之类的传感器数据处理函数。

• 其他工作
  • 纯定位模式的配置
  • 初始位置的设置

调用TrajectoryBuilderInterface类函数处理传感器数据

由上面分析可知这里的TrajectoryBuilderInterface类是父类，而实际调用的是子类CollatedTrajectoryBuilder。

CollatedTrajectoryBuilder类

主要功能：

• 构造Collator类，管理所有的传感器数据，设置回调函数为HandleCollatedSensorData(),在Collator类内所有的传感器数据都被表达成通用的传感器数据结构。
• 调用Collator类处理传感器数据，在将传感器数据转换成通用数据后，调用回调函数HandleCollatedSensorData()
• 在回调函数中调用全局轨迹生成类GlobalTrajectoryBuilder对传感器数据进行处理

GlobalTrajectoryBuilder类

在全局轨迹生成类中将传感器数据进行了分类，其中激光雷达数据、IMU数据和里程计数据用于生成局部轨迹，其他的传感器数据如GPS信息、路标点信息等则直接被添加到了位姿图优化类PoseGraph2D类中。

• 针对里程计、IMU和激光信息，调用局部路径生成LocalTrajectoryBuilder2D类进行处理
• 处理结束后将其添加到PoseGraph2D类中进行优化
• 将其他传感器数据同样添加到PoseGraph2D类中进行优化

因此，具体的实现部分在LocalTrajectoryBuilder2D类中添加传感器数据部分，以及PoseGraph2D类中添加传感器数据作为节点部分。

LocalTrajectoryBuilder2D类

• 构造PoseExtrapolator位姿外推类，类似于卡尔曼滤波器的功能，利用之前的传感器信息和当前激光采集时间，预测当前位置和速度。
• 根据预测速度，对激光数据进行运动畸变矫正
• 调用CeresScanMatcher2D类，进行激光数据前端匹配的计算当前帧与当前子地图的位置关系。
• 激光达到一定距离则调用submap_2d类插入到子地图中
• 利用匹配结果更新PoseExtrapolator的估计，为下一次做准备

其中，CSM前端匹配算法是，子地图-激光帧的匹配。具体原理部分可参考原论文，这里的重点不是理论。另外submap_2d类插入激光数据到子地图中，对子地图进行了管理，具体的子地图管理策略为。

其中，激光数据插入地图调用的是probability_grid_range_data_inserter_2d类中的函数，其原理为占用栅格地图更新原理，可参考注释和相关资料理解，这里不再赘述。

PoseGraph2D类

• 在收到局部轨迹生成类得到的子地图后，首先添加到构造的优化问题OptimizationProblem2D类中。
• 然后调用fast_correlative_scan_matcher_2d.cc文件中的算法进行回环，主要就是利用分支定界算法在一定大小的窗口内进行搜索匹配。
• 回环检测结束后，无论是否成功都将优化求解问题添加到线程池中
• 其他传感器数据采集到也会将优化问题添加到线程池中进行求解

分支定界方法用于寻找回环约束的具体实现与原理较为复杂，可以参考论文和代码注释进行学习，这同样不是本文的重点。最后，将所有传感器的数据添加到OptimizationProblem2D类中构造了后端优化问题，接下来我们将求解这样一个最终的优化问题。

OptimizationProblem2D类

优化问题的求解调用函数OptimizationProblem2D::Solve，其主要流程和普通的Ceres优化流程没有什么区别，就是按照Ceres的套路来。其中最为关键问题在于图优化中的节点和边如何构造以及误差函数的计算，这里我们采用因子图的方式表达这样一个图模型。

最后我们看一下误差函数,定义在SpaCostFunction2D类中，非常非常的简单，就是优化结果不能和之前匹配得到的相对位置相差太多。

  bool operator()(const T* const start_pose, const T* const end_pose,
                  T* e) const {
    // 误差计算函数  
    // ScaleError 基于旋转和平移项不同的权重，保证收敛
    // ComputeUnscaledError 真正计算误差的函数
    const std::array<T, 3> error =
        ScaleError(ComputeUnscaledError(
                       transform::Project2D(observed_relative_pose_.zbar_ij),
                       start_pose, end_pose),
                   observed_relative_pose_.translation_weight,
                   observed_relative_pose_.rotation_weight);
	// 保存误差
    std::copy(std::begin(error), std::end(error), e);
    return true;
  }

这里里程计的误差函数和匹配的误差函数是相同的，可以认为结果是两个里程计的可变加权和，其他的误差函数如路标点等这里不再过多展开，都是一样的。

至此，我们将cartographer代码的整体流程过了一遍，其原理不难，但是源码过于复杂，其中还有许许多多的细节，需要花费大量时间仔细阅读才能真正熟悉它。