Cartographer论文+源码初步解析

引言

最近因为需要,初步探索了cartographer的一些内容. 感觉Google的代码有些难读,因为库文件过于完备导致阅读较为困难.加之没有接触过的.lua等文件类型,开始时无从下手. 现在一点点记录下来.

主要是论文和代码,分开说, 中间可能互有穿插.

论文

主体的论文是《Real-Time Loop Closure in 2D LIDAR SLAM》，重点部分是第四部分和第五部分。

第四部分 ，局部 2D SLAM

在这一部分，主要介绍了三个内容。

1.Scans
介绍了scan 和　submap　两个坐标系的转换关系，公式

2.Submaps
这里，对激光雷达光束扫到的栅格进行了一个评分。每个激光束都分为集中（hit）和未击中（miss），若hit，将这个最近的网格点加入hit集合。若未击中，则将起经过的点加入miss集（不插入已经在hit集中的）。若有更新，则利用公式（2）和（3）进行更新。

示意图如下，打叉的是hit，阴影部分（除了打叉的）是miss。

3.Ceres scan matching

在 scan 插入 submap 之前，利用 ceses 的 scan matching 进行匹配优化，是一个非线性最小二乘问题。
M函数，论文中使用的是双三次插值法（bicubic interpolation）。

第五部分 ，回环检测部分（全局SLAM）

这一部分中，主要是优化问题和分支界定匹配的方法。优化所有点集和submap位置的方法使用的是Sparse Pose Adjustment（SPA）

1.Optimization problem
闭环的优化也被表示成非线性最小二乘问题。利用残差来纳入数据。每隔几秒利用ceres计算SPA的解决方案。

2.Branch-and-bound scan matching

这一部分先介绍了一个没有实际应用的算法，忽略它。 实际用的是算法2和算法3.

节点选择采用的是DFS，对于分支算法，采用的是算法2。

算法3是将节点选择和分支结合到一起之后的算法。

小总结

与该论文相关的还有两篇论文，《Real-Time Correlative Scan Matching 》和《Efficient Sparse Pose Adjustment for 2D Mapping》，即BBS和SPA（前端和后端） 。
BBS主要贡献是在GPU上完成了sacn match，使得运算效率有了极大的提高。其先低分辨率后高分辨率的方法，也在一定程度上提高了速度。

在scan到submap的scan_matching，用的RealTimeCorrelationScanMatcher
在submap到loop closing的scan_matching，用的FastCorrelationScanMatcher

论文看的比较粗略，大概说一下代码。（更加粗略）

代码

Cartographer的代码包共由三个部分，
1.cartographer_ros
2.cartographer
3.ceres-solver
因为代码比较难读，因此介绍中不按整个的逻辑关系去介绍，而是选择分模块进行简介。

首先介绍.lua文件

demo_backpack_2d_localization.launch

urdf文件:backpack_2d

lua文件:
backpack_2d_localization.lua
在这个文件中,首先设置pure_localization
并设置每n个点进行一次位姿优化(此处,可能会导致全局SLAM太慢而无法实时定位,因此通常减少global_sampling_ratio 和 constraint_builder.sampling_ratio)

backpack_2d.lua:对各类参数进行设置,主要是进行各个坐标系的frame设置.并在此设置是否使用里程计等传感器.

map_build.lua
参数use_trajectory_builder_2d在backpac_2d.lua设置为true

trajectory_builder.lua
是否设置为pure_localization模式

trajectory_builder_2d.lua主要设置local SLAM(前端)的参数
包括雷达扫描范围,可用高度,体素大小等参数

pose_graph.lua主要设置全局 SLAM(后端)的参数
在pose_graph.lua中,可以设置采样率, slam_pose和odom_pose的权重, 全局优化迭代次数等.

P.S.
1 pure_localization和mapping相比, 只是参数设置上TRAJECTORY_BUILDER.pure_localization = true(false→true) POSE_GRAPH.optimize_every_n_nodes = 20(90→20).

整体逻辑

1.node_main.cc:
run() 中,node.StartTrajectoryWithDefaultTopics(trajectory_options),开始设置.跳转到node.cc

2.node.cc:
在函数StartTrajectoryWithDefaultTopics中,调用AddTrajectory(options, DefaultSensorTopics());设置一大堆参数.然后利用LaunchSubscribers开始设置订阅的话题.包括laserscan, MultiEchoLaserScan, PointCloud2, Imu, Odometry, NavSatFix, LandmarkList.
这里有laserscan,先看一下node::handlelaserscanmessage.直接调用了map_builder_bridge,到了sensorbrideg的handlelaserscanmessage.然后利用ToPointCloudWithIntensities把 sensor_msgs::LaserScan::ConstPtr 转换成了carto::sensor::PointCloudWithIntensities.并利用handlelaserscan(pointcloud)处理. 这个我就看不懂了…大概判断时间戳,然后到HandleRangefinder.再到addsensordata. 最后应该是在collator.cc中实现的.

3.local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::ScanMatch// 输出预测的位姿
LocalTrajectoryBuilder2D::AddRangeData// 增加距离数据(也有IMU等)
LocalTrajectoryBuilder2D::InitializeExtrapolator// 初始化位姿外推器

4.real_time_clrrelative_scan_match.cc
RealTimeCorrelativeScanMatcher2D::Match// 输出位姿的最高得分
RealTimeCorrelativeScanMatcher2D::ScoreCandidates// 对候选点进行评分

5.global_trajectory_builder.cc