cartographer算法论文中的两个scan matching怎么理解？

cartographer算法论文中的两个scan matching怎么理解？

【导读】开源MIT Min cheetah机械狗设计第33篇，番外篇第9篇，本篇主要讲解Cartographer前端点云匹配。

我们都知道Cartographer适合于室内的地图构建，而不适合户外地图构建，为什么呢？就是因为数据匹配过于严格，户外的激光雷达点云比较复杂，而且可能受到复杂环境的各种干扰，导致匹配成功概率比较低，而室内的激光点云相对来说质量还是比较高的。

上一篇文章已经将激光点云传入了前端，本篇也就从前端激光点云匹配开始讲起。

从下图可以看出，点云数据，IMU和里程计数据都传入了前端，激光点云用来做做地图匹配，IMU/里程计用来提供点云匹配的初始位置。后端其实也需要一个初始位置，如果有GPS和Landmark，则不需要IMU/里程计，因为GPS和Landmark直接提供了绝对位置，比IMU/里程计推导出来的位姿要准多了。

先来看看雷达点云传入后发生了什么

前端点云匹配的函数如下：

可以看出传入的参数为：

话题名称：sensor_id ，比如/range，/IMU，等

点云数据：unsynchronized_data（直译就是未经过时间同步的点云数据）

何谓时间同步？

Cartographer虽然支持多个相同类型传感器数据传入，但会对多个传感器数据进行数据融合，好像多个传感器只有一帧数据一样。

假设我们有两个激光雷达传感器数据，则数据有两种情况：

1、一个传感器比另一个传感器数据来的快，且没有交集，则数据融合会将这两帧激光雷达点云数据进行合并输出。

2、一个传感器比另一个快，但存在时间上的交集，那么会将慢的那一帧进行截断，最后数据融合成一帧。

看看AddRangeData（）函数是怎么实现这两种情况的。

程序如下：

解读如下：

第一步调用了

函数内容如下：

功能就是前面描述的，对多个传感器数据进行融合为一帧

42-43：将传进来的timed_point_cloud_data点云数据中的强度值intensities全部置0

44-50：判断id_to_pending_data_中key值为sensor_id的个数是不是为0，不为0则进入判断，假设现在点云数据第一次到来，也就是第一次调用这个函数，所以这个判断条件不满足。

另外这里要说明一下id_to_pending_data_为map容器类型，而且不允许key值重复，所以每个sensor_id只有一个数据（也即会抛弃旧数据，使用新数据），而且只有相同类型的传感器数据全部到齐了才会进行下一步处理，比如，如果有两个激光雷达，一个数据快，一个数据慢，快的数据到达之后，还有继续等待慢的数据到来。

51：将收到的点云数据插入map中

52-54：判断expected_sensor_ids_的个数和id_to_pending_data_的个数是否相等，也就是激光点云话题个数是否和id_to_pending_data_的个数相等，也即等待所有此类型话题全部到齐，其中这个expected_sensor_ids_是在http://map_builder.cc中进行赋值的，代码如下：

因为现在id_to_pending_data_中只有一帧点云数据，所以满足expected_sensor_ids_的个数和id_to_pending_data_的个数不相等这个判断条件，因此直接return了，所以继续回到函数开始。

此时如果又来了另外一个传感器的点云数据，还是重复上述过程，这个时候id_to_pending_data_中的所有激光雷达点云数据就都到齐了（这里假设只有两个激光雷达传感器），这样程序就可以从55行开始执行。

55：将上一次的结束时间点current_end_赋值给current_start_作为这次的起始时间。

56-60：找到所有激光雷达点云中最早的一个时间点，然后赋值给 current_end_，也就是上图中Laser_1的时间戳，即current_end_，这样就可以将上图中第1个区间段的数据拿到进行处理，对于Laser_1数据全部拿下，对于Laser_2则截断了一部分，处理函数是CropAndMerge（），功能就是合并数据。

下面看一下CropAndMerge（）函数是如何合并多帧数据的

程序一开始就是对所有的激光雷达传感器数据进行了遍历

以其中一个传感器数据为例，首先取出数据：

之后的任务就是找出下图中第1个时间段内的数据。如何找到数据呢？找到数据其实就是找到current_start_和current_end_的时间点。当然目前我们程序是第一次运行到55行以后，所以也就只有第1个时间段的数据。

1）找到第一个小于current_start_的点，如果是Laser_1就是左边，如果是Laser_2则不存在，不过这时overlap_begin就指向了第2个区间的左边。

2）找到第一个小于current_end_的点，如果是Laser_1就是右边，如果是Laser_2则就是第2区间的右边。

3）把找到的这段数据塞进 sensor::TimedPointCloudOriginData result里，也就是上图中的Laser_1和Laser_2的一部分。

4）如果数据拿到手就删除id_to_pending_data_中的数据，也分几种情况，

a）数据全部使用了，比如上图中的Laser_1

b）只用了部分数据，比如上图中的Laser_2

c）一个点都没用到

对应的程序：

5）Laser_1和Laser_2的数据都塞进result，接着就是对每个点云的时间进行排序了，排完序返回result

现在终于可以回到文章开始的LocalTrajectoryBuilder2D::AddRangeData（）函数继续分析

这个函数中数据时间同步已经解决了：

1）取点云获取的时间为基准初始化PoseExtrapolator

如果不使用IMU，就初始化位姿推断器，由于cartographer中只有激光雷达是必须的, 如果没有odom和IMU, 只用激光雷达推测出的Pose也是可以用位姿推测器的。显然这里我们是使用IMU的。

InitializeExtrapolator（）函数中主要就是调用AddPose（），这个前面已经介绍过。

接着计算第一个点：第一个点的采集时间(time_first_point)等于(time)加上第一个点记录的相对时间

如果该时间比PoseExtrapolator的最新时间还要早，说明在第一个点被采集时，PoseExtrapolator还没初始化，直接返回。

2）接下遍历每个点，针对每个点，用PoseExtrapolator推算机器人在该点的Pose，放入range_data_poses这个向量。range_data_poses用于记录零时刻时，各个扫描点所对应的、用于计算base_footprint-->odom坐标变化的位姿。它的长度等于点云中点数，而且索引一一对应，即range_data_poses的第一个位姿用于点云的第一个点，依次类推。

接着就是再次遍历synchronized_data所有点的数据，并进行处理：

①获得带时间戳的单个数据点的坐标

②坐标变换，将点云中的origins坐标（在机器人坐标系下，也即tracking frame）转换到Local （odom） 坐标系下。

origin_in_local（雷达原点在Local坐标系下的位置）

=range_data_poses（机器人在 local 坐标系下的位姿）*synchronized_data.origins（雷达原点在机器人坐标系下的位置）

③将点云hit点转换到Local（odom） 坐标系下

hit_in_local（点云数据在 local 坐标系下的坐标）

=range_data_poses（机器人在 local 坐标系下的位姿）*sensor::ToRangefinderPoint(hit) （表示点云在机器人坐标系下的位置）

上述坐标变换可以用下图表示如下：

④计算local下激光数据与激光原点之间的距离

⑤过滤数据

如果点云大于等于最近测量距离、且小于等于最远测量距离，那么则把该点云添加到 accumulated_range_data_.returns 变量中，如果超过范围，则放到 accumulated_range_data_.misses中。

到这一步，一帧点云数据就处理完了。每处理完成一次就执行++num_accumulated_;

当num_accumulated_大于配置文件中的options_.num_accumulated_range_data()时，就进行一次扫描匹配，默认options_.num_accumulated_range_data()数为1

①计算2次有效点云数据的的时间差，这里由于options_.num_accumulated_range_data()默认值为1，因此也就是两帧之间的时间差。

②获取机器人此时的姿态，也即朝向

③以最后一个点的时间戳估计出的坐标为这帧数据的原点，

之后调用函数AddAccumulatedRangeData（）开始进行数据矫正

此函数的输入参数为：

1）时间戳time

2）点云数据：将点云投影到水平面并进行滤波,然后返回过滤并矫正过的点云数据，功能由TransformToGravityAlignedFrameAndFilter（）函数实现。

3）机器人朝向gravity_alignmen

4）点云之间的时间差sensor_duration

先看TransformToGravityAlignedFrameAndFilter（）函数

功能：先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量

1）获得点云time时刻机器人位姿

2）重力坐标系下机器人的位姿

pose_prediction（重力gravity系下机器人的位姿）=

gravity_alignment.inverse()（重力向量在local下的逆）*non_gravity_aligned_pose_prediction（local系下的机器人位姿）

所谓的重力系就是：gravity坐标系原点与local坐标系重叠，但是gravity坐标系的 Z 轴是与重力方向垂直的，上述的代码就是对推断器推断出来的位姿进行重力矫正。

3）进行体素滤波

4）扫描匹配，将pose_prediction作为匹配的初始位姿，然后通过子图匹配，对pose_prediction进行优化矫正，得到矫正后的位姿pose_estimate_2d

5）再将机器人位姿恢复到 local 系

pose_estimate（恢复到 local 系下的机器人位姿）=

pose_estimate_2d（优化后的位姿，重力系下）*gravity_alignment（重力系在Local系下的转换）

6）将优化后的位姿送到位姿估计器进行矫正以往的位姿

7）原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云

8）将点云写入子图submap

至此，前端匹配就结束了，直接返回结果

到这里还有两个重要的函数没有说，一个是体素滤波，另一个就是匹配算法

先来看看体素滤波，函数是AdaptiveVoxelFilter（）

输入参数为点云数据point_cloud，自适应体素滤波相关参数options

自适应体素滤波相关参数

其中FilterByMaxRange（）函数是通过设置最大范围，筛选满足条件的点云

对点云进行自适应滤波

1）如果点云已经足够稀疏，则不处理，直接返回

2）执行体素滤波

传入参数为点云point_cloud和体素的分辨率resolution

其中核心的函数就是RandomizedVoxelFilterIndices（），用来标记过滤后的点。

传入的参数为：①需要处理的点数据；②体素滤波分辨率；③lambda表达式

1）创建一个hash map索引

VoxelKeyType：体素的索引，

std::pair<int, int>的第一个元素保存该voxel内部的点的个数, 第二个元素保存该voxel中选择的那一个点的序号

表示如下图所示：

2）遍历所有的点云point_cloud

3）获取体素的索引值VoxelKeyType

索引值VoxelKeyType使用一个64位的uint64_t类型进行存储，举个例子：

假设两个点云point的三维坐标为

则经过GetVoxelCellIndex之后，坐标变为：

圆整后两个点都为 ，也就是这两个点都同为一个体素里的，因此返回的VoxelKeyType是一样的，又由于flat_hash_map不允许key值重复，因此VoxelKeyType唯一，之后如果有相同VoxelKeyType值的，则

std::pair<int, int>的第一个int进行++递增，达到对体素里的点进行计数的功能。

4）对std::pair<int, int>的第二个值进行赋值

寻找一个点能够代表这个体素，如果体素里面只有一个点，则这个点的索引就int，如果体素里面有很多点，就随机产生一个数，这个数的范围是（1，所有点的个数），如果随机数等于此时体素里面点的个数，则这个点的索引就是int

5）找到每个代替体素的点之后，对点进行做标记

也就是找到原始点云中，那些可以代表体素的点，并对其进行标记，之后原始点就用这些标记的点进行替代，从而实现少量点云代替大量点云的功能。

现在再回到AdaptivelyVoxelFiltered（）这个函数，此时我们已经对点云完成了体素滤波

如果经过体素滤波后，发现点太少了，已经小于阈值options.min_num_points()了，那么说明我们体素的分辨率resolution给大了，得减小这个分辨率，增加点数，因此需要一个自适应体素滤波，也就是循环减小，直到达到需要得点，或者分辨率已经很小了，没办法了，则直接停止。

体素滤波讲完了，轮到最后一个扫描匹配了，函数为ScanMatch（）

函数传入得参数为：

时间戳：time ，预测位姿pose_prediction，体素滤波后得点云：filtered_gravity_aligned_point_cloud

1）取出活跃得子图（最多有两个活跃的子图），参与匹配

2）暴力搜索匹配

a，将点云旋转到预测的initial_pose_estimate位姿上

b，配置参数

也就是前面文章提出的位移搜索窗口，角度搜索窗口

c、不同角度的点云集合

d、不同位移平移后的点云在地图上的索引

e、获得所有的候选解

也就是 三个的组合解

f、计算加权得分

g、获得最优解

h、得到矫正后的位姿

上述暴力匹配完成后还没完，还要继续进行ceres扫描匹配，上述暴力匹配可以说是对initial_ceres_pose进行了一个矫正，接着还要进行更精细的匹配。

3）ceres扫描匹配

需要传入的参数：

预测位姿：pose_prediction.translation()

校正后的位姿：initial_pose_estimate

点云数据：point_cloud

栅格地图：grid

优化后的位姿：pose_estimate

可以看出整个位姿优化的一个过程，初始预测--->暴力相关性匹配--->ceres扫描匹配

关于ceres的使用可以参考官方资料，核心就是在于构建残差项

程序构建了三个AddResidualBlock

分别为地图残差

平移残差

旋转角度残差

对照下面残差的定义，可以观察上述三个AddResidualBlock的params均为ceres_pose_estimate，也就是我们要优化的目标，损失函数loss_function_1则都为nullptr，代价函数cost_function_1则都不同。

另外平移残差的参考位移为target_translation，也就是选择姿态估计器的位姿，ceres认为优化后的位姿，不会离姿态估计器的位姿太多；

旋转角度残差的参考角度设为了ceres_pose_estimate[2]，也即ceres认为优化后的位姿，不会离匹配后的位姿太多。

这里计算残差，都是基于使用的传感器预测的位姿很准的情况。

所以在调参的时候，如果传感器不是很好，位姿估计器不准确，可以适当的将地图的权重调的大于平移和旋转的权重。若是调参一直调不好，可以将计算平移和旋转的残差注释掉。（摘自文章：cartographer中的扫描匹配_ceres::bicubicinterpolator-CSDN博客）

其实上述比较重要的一个残差是地图残差，特别如果激光点云比较稀疏，直接匹配可能会有问题，因此将其进行平滑处理，如下图所示，另外地图的权重是这样处理的：地图的权重除以点云的数量，得到一个平均值，然后再进行传入，所以地图的权重就会被分散，当点云的数量越来越多时，地图的权重就会被分散的越来越小。

地图的处理过程如下：

先对点云的坐标进行转换，将点云 在机器人坐标系下的坐标，转换到地图局部坐标系下：

用图表示如下：

转换后对这些坐标点进行差值平滑处理：由离散点变平滑曲线

构造ceres优化函数：

核心代码在OccupiedSpaceCostFunction2D类中，得到平滑曲线后，计算在地图上的占用概率值，占用的概率越大得分越高。

4）参数保存

5）返回结果

自此，Cartographer算法的前端匹配就讲完了，下一篇将讲解后端匹配。