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类举例解释。

构造局部轨迹生成类 LocalTrajectoryBuilder2D
使用局部轨迹生成类，构造全局轨迹生成类 GlobalTrajectoryBuilder
使用全局轨迹生成类，构造轨迹生成管理类指针 CollatedTrajectoryBuilder
将轨迹生成管理类指针保存在一个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代码的整体流程过了一遍，其原理不难，但是源码过于复杂，其中还有许许多多的细节，需要花费大量时间仔细阅读才能真正熟悉它。

cartographer 代码分析相关推荐

20145236《网络攻防》Exp4 恶意代码分析
20145236<网络攻防>Exp4 恶意代码分析一.基础问题回答如果在工作中怀疑一台主机上有恶意代码,但只是猜想,所有想监控下系统一天天的到底在干些什么.请设计下你想监控的操作有哪些 ...
C#中类的继承 override virtual new的作用以及代码分析
继承中override virtual new的作用 virtual 父类中需要注明允许重写的方法: override 子类中必须显示声明该方法是重写的父类中的方法: new 子类中忽略父类的已存在的 ...
2017.4.18 静态代码分析工具sonarqube+sonar-runner的安装配置及使用
配置成功后的代码分析页面: 可以看到对复杂度.语法使用.重复度等等都做了分析,具体到了每一个方法和每一句代码. 四种使用方式: sonarqube + sonar-runner sonarqube + ...
lighttpd1.4.18代码分析
lighttpd1.4.18代码分析(八)--状态机(2)CON_STATE_READ状态 posted @ 2008-09-24 10:50 那谁阅读(2225) | 评论 (1) 编辑 lig ...
Device Tree（三）：代码分析
2019独角兽企业重金招聘Python工程师标准>>> 一.前言 Device Tree总共有三篇,分别是: 1.为何要引入Device Tree,这个机制是用来解决什么问题的?(请 ...
使用Hadoop和ELK进行业务代码分析！分分钟捉到Bug!
大数据是计算领域的新高地,它有望提供一种方法来应对二十一世纪不断增长的数据生成.越来越多的大数据爱好者正在涌现,越来越多的公司正在采用各种大数据平台,并希望提出以客户为中心的解决方案,帮助他们在竞争激 ...
20145328 《网络对抗技术》恶意代码分析
20145328 <网络对抗技术>恶意代码分析 ------看到这句话说明还没写完-------- 实践内容: 使用schtasks指令监控系统运行使用sysmon工具监控系统运行使用 ...
starGAN原理代码分析
下载: git clone https://github.com/yunjey/StarGAN.git 1 cd StarGAN/ 1 下载celebA训练数据: bash download.sh 1 ...
tensorflow笔记：多层CNN代码分析
tensorflow笔记系列: (一) tensorflow笔记:流程,概念和简单代码注释 (二) tensorflow笔记:多层CNN代码分析 (三) tensorflow笔记:多层LSTM代 ...

cartographer 代码分析