cartographer源码解析

Posted 2020-12-22 carbo-t

cartographer为Google提供的激光SLAM开源库，通常通过其提供的ROS平台封装进行使用，该库结构清晰，模块完整，值得深入研究。

项目官网：https://google-cartographer.readthedocs.io/en/latest/

项目Github：https://github.com/cartographer-project/cartographer

环境安装与配置具体参考：https://www.cnblogs.com/lvchaoshun/p/9824528.html

安装过程中可能会碰到一些版本冲突之类的问题，耐心上网寻求解决方法。

温馨提示：本系列主要介绍源码内容，不介绍环境搭建，仅作为自己学习过程记录，可能会比较乱，有疑问欢迎留言讨论。

下面进入正题，下图是cartographer官网提供的软件架构图，本系列将通过cartographer_ros封装库中调用顺序，对程序主线进行介绍，暂未使用到的或次要的信息以后有空再分析。

 

---

 首先我们找到cartographer_ros中程序入口，即cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc，其中的main函数为cartographer_ros节点启动的程序入口。

 其他都是ros平台初始化相关的函数，只用看98行Run方法。

可以看到该函数内主要是些初始化工作，创建map_builder传给node进行初始化、读取pbstream地图文件，并调用StartTrajectoryWithDefaultTopics函数，该函数定义在node.cc中。

 我们会发现AddTrajectory是核心功能入口。××该函数需重点关注，之后我们还会回来一个个分析其中调用的关键函数。

 

 该函数功能比较多，我们一步步看。首先363行创建一个set保存传感器话题的编号，用于之后订阅器订阅。366行调用map_builder_bridge_实例的AddTrajectory函数，在cartographer_ros中类名包含“bridge”的类基本上都是作为“桥梁”调用cartographer源码的类。我们可以看到map_builder_bridge_为map_builder_bridge.cc中定义的MapBuilderBridge的实例，此时关注map_builder_bridge.cc中定义的AddTrajectory函数内容。

 根据cartographer全局配置参数和最初Run方法中定义的map_builder实例类型，我们了解到map_builder_为cartographer/mapping/map_builder.cc中定义的MapBuilder类的实例。上图函数内容分为两块，一块调用map_builder_的AddTrajectoryBuilder函数，创建trajectory_builder并设置结果回调，返回一个路径id作为当前轨迹的编号，详情下篇再介绍；另一块主要初始化一个SensorBridge实例，并放入sensor_birdges_这个map中，以及保存该路径id对应的配置参数。

接下来我们回到node.cc中核心功能入口函数。368与369行分别创建与当前路径编号对应的位姿预测器(PoseExtrapolator)与传感器采样器(TrajectorySensorSamplers)，根据配置参数创建实例没啥好说的。371行启动ros话题订阅器，并设置回调函数。这之后的创建定时器和记录话题编号也很简单。我们主要关注话题订阅的回调函数。

上图截取了LaunchSubsribers函数的部分代码，通过其回调函数，我们发现最终会调用sensor_bridge.cc中定义的HandleLaserScan函数。此处仅仅举个例子，该函数仅处理激光雷达数据(普通LaserScan类型消息、多回波、点云等)，其他传感器数据(如IMU、里程计等)会调用不同的回调函数。

我们观察HandleLaserScan函数实现。

 首先对点云细分，默认num_subdivisions_per_laser_scan_=10。points对象的类型为cartographer_ros自定义类型，其中保存了每个点的光强、位置与时间增量。time_to_subdivision_end保存了最后一个点的时间增量，即一块细分点云间时间间隔(第一个点之前的时刻到最后一个点之后的时刻之间的时间间隔)。之后做一些时间戳的运算，或许是诸如(..., -0.06, -0.04, -0.02, 0)的一组时间戳。最后调用HandleRangefinder将数据添加到全局路径构造器，如下图所示。

此处trajectory_builder_会是GlobalTrajectoryBuilder类的实例，是由于map_builder_的AddTrajectoryBuilder函数中调用了对应的构造函数，此处已涉及cartographer源码。

 本节主要介绍了cartographer_ros封装是如何跳转到cartographer源码中的，主要包括ros节点、map_builder与trajectory_builder的创建，pose_extrapolator与trajectory_sensor_samplers的初始化以及传感器话题订阅与处理。从下一节开始，将根据调用顺序介绍cartographer源码。