(02)Cartographer源码无死角解析-(48) 2D点云扫描匹配→扫描匹配基本原理讲解，代码总体框架梳理AddAccumulatedRangeData()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言(Scan match原理)

通过前面你的一系列博客，已经知道 Cartographer 中的概率栅格图是如何建立的了。不过需要注意的一点是该地图并不仅仅是保存下来给来看的，其还会被点云扫描匹配使用到，点云扫描匹配目的是估算位姿(该部分内容后面会详细讲解)。在 slam 中分为 Scan match 与 Point cloud match，通常情况下前者指的就是2D扫描匹配，后者指的是3D点云匹配。为了方便后续代码的理解，这里简单介绍一下扫描匹配的原理，先来看下图(理想位姿)：

图一：理想位姿

1、方格 → 所有的栅格组成。
2、黑色方格 → 障碍物，光子会被阻碍，形成点云数据。
3、白色方格 → 空闲区域，表示没有障碍物，光子可以直接穿过。
4、紫色多边形 → 代表机器人，当然也可以理解为雷达传感器原点，或者点云数据的原点。
5、黄色圆形 → 点云数据。

从上图来看，可见所有的点云数据都与障碍物重叠了，说明此时估算出来的位姿十分正确，这里称为理想位置。但是系统在运行的过程中并非是这样的。这里假设我们由传感器数据(Imu、GPS、里程计) 等估算出来的位姿如下所示：

图二：初始位姿 容易看出上图估算出来的位姿是存在偏差的，假设传感器估算出来的初始位姿，是不太准确的，那么问题就来了，如何根据初始位姿求解出理想位姿呢？首先，通常来说理想位姿需要在初始位姿的一定范围之内(也就是说，传感器的误差不能太大了)，基于这个前提，再来分析上图。

根据图示可以看出初始位姿与理想位姿在位置与姿态上都存在差异，也就是说由初始位姿变换到理想位姿，其平移与旋转可能都需要发生改变。基于这个原理，最简单的一种扫描方式就是暴力匹配，也就是在位移与角度上维度上进行遍历，流程如下：

1、设定初始位姿
2、位移扫描匹配遍历
3、角度扫描匹配遍历。
4、评分标准

初始位姿可以直接由传感器估算出来，先来看下图，

图三：位姿遍历

绿色区域 1-6 表示需要遍历的区域，也就是理想位姿的平移处于该区域内，也就是位移扫描匹配需要遍历绿色区域1-6。蓝色的箭头表示角度遍历的方向。该箭头的起点就是角度开始遍历的起点，终点就是角度遍历的终点。

如上图所示，假设从从1号方格开始第一次遍历，Robot角度为-35°(假设平行y轴为0°)，记为 1_(-35)。需要主意的是，Robot 与点云需要一直保持相对禁止，Robot位姿如何变换，点云则需做同样的变换。在 1_(-35) 的位置上，我们希望所有的点云都能够与黑色方格(障碍物)完全匹配，但是显然是不行的，所以改变一下角度，这里假设角分辨率为5°，那么下次利用 1_(-30) 的 Robot 与点云位姿与黑色方格进行匹配，依此一下，进行位姿和角度上的遍历，如下：

1_(-35)   1_(-30)   1_(-25) ......  1_(30)   1_(35)
2_(-35)   2_(-30)   2_(-25) ......  2_(30)   2_(35)
......
6_(-35)   6_(-30)   6_(-25) ......  6_(30)   2_(35)

其可以按行遍历，也可以按列遍历，每一个上述 Robot 与点云位姿与黑色方格进行匹配之后，我们都需要进行一次评分，在这个例子中会匹配6x(70/5)=6x14=84次，也就是说这里需要进行84次评分。那么问题来了，如何进行评分，才能体现出匹配结果的优劣呢？

从上图可以很直观的看出点云与障碍物重叠度越高，则匹配效果越好。结合前面的内容，这里假设存在 $n$ 个点云，其每个点云对应的方格记为 $cel{l}_i$，$cel{l}_i$ 对应被占用的概率为 $p_i$，那么每次匹配的得分可以记为 $s_x=({\sum }_{i=1}^n{p}_i)/n$。然后认为所有匹配(84次)得分中最高的 $s_{max}$ 为最优匹配。也就是图一的理想匹配。

$注意$ 这里讲解的仅仅是一种十分简单的方式，实际工程的实现并非如此简单，需要考虑很多的东西，比如通常情况下，点云与黑色方格很难完全匹配，也就是说不存在精确解，那么只能通过迭代的方式求得最优解。除此之外，还有涉及到其他的很多技巧，后续会进行部分讲解。

Cartographer 中的扫描匹配主要是参考：A Flexible and Scalable SLAM System with Full 3D Motion Estimation 实现，有兴趣的朋友可以好好阅读一下。
 

二、框架梳理1→AddAccumulatedRangeData()

下面来看看源码中是如何进行扫描匹配的，该篇不可不对细节进行刨析，了解基本流程即可，下一篇博客再进行细节上的分析。首先找到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数：

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

  // Computes a gravity aligned pose prediction.
  // 进行位姿的预测, 先验位姿z
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);

该函数是获得点云数据time时刻机器人的位姿，其是先对于 local 坐标系的，这里记 non_gravity_aligned_pose_prediction 为 ${\mathbf{Robot}}_{local}^{tracking}$。表示 tracking_frame 在 local 系中的位姿。
 

2、机器人位姿重力校正

  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

gravity_alignment 在前面详细分析过，这里记为 ${\mathbf{GravityAlignment}}_{local}^{gravity}$，该矩阵的逆 $[{\mathbf{GravityAlignment}}_{local}^{gravity}{]}^{-1}={\mathbf{GravityAlignment}}_{local}^{gravity}$，可以对 local 系下的位姿或者点云数据进行重力校正，也就是把local系的数据变换到gravity系下，gravity坐标系原点与local坐标系重叠，但是gravity坐标系的 Z 轴是与重力方向垂直的，上述的的代码就是对推断器推断出来的位姿进行重力矫正，对应的数学公式如下：$${\mathbf{Robot}}_{gravity}^{tracking}={\mathbf{Robot}}_{local}^{tracking}\ast [{\mathbf{GravityAlignment}}_{local}^{gravity}{]}^{-1}\text{\hspace{1em}(01)}$$其上的 pose_prediction = ${\mathbf{Robot}}_{gravity}^{tracking}$ 表示重力gravity系下机器人的位姿。
 

3、重力对齐水平点云

  // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }

该处代码的 gravity_aligned_range_data.returns 点云数据已经进行过重力矫正矫正了，所以重新构建的点的filtered_gravity_aligned_point_cloud 同样表示经过重力矫正的点云。这里记为 $point{s}^{gravity}$。
 

4、扫描匹配校准位姿

  // local map frame <- gravity-aligned frame
  // 扫描匹配, 进行点云与submap的匹配
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

扫描匹配的本质是对先验位姿进行矫正，也就对 pose_prediction 位姿进行优化。优化之后的位姿 pose_estimate_2d 这里记为 ${Robo{t}^{\prime }}_{gravity}^{tracking}$。
 

5、机器人位姿恢复到 local 系

  // 将二维坐标旋转回之前的姿态
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 校准位姿估计器
  extrapolator_->AddPose(time, pose_estimate);

优化过的位姿 ${Robo{t}^{\prime }}_{gravity}^{tracking}$ 是基于 gravity 系的，我们需要把其恢复到 local 系下，恢复之后的位姿记为 ${Robo{t}^{\prime }}_{local}^{tracking}$，那么对应的数学公式如下所示：$${\mathbf{Robo}{\mathbf{t}}^{\prime }}_{local}^{tracking}={\mathbf{Robo}{\mathbf{t}}^{\prime }}_{gravity}^{tracking}\ast {\mathbf{GravityAlignment}}_{local}^{gravity}\text{\hspace{1em}(02)}$$

6、点云数据姿恢复到 local 系

  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));

range_data_in_local 表示 local 系下的点云数据记为 $point{s}^{local}$，那么还需要把 gravity_aligned_range_data= $point{s}^{gravity}$，也就是 gravity 系下的点云数据，变换到 local 系下，那么对应的数学公式如下：
$$point{s}^{local}={\mathbf{Robot}}_{gravity}^{local}\ast point{s}^{gravity}\text{\hspace{1em}(03)}$$虽然代码中 gravity_aligned_range_data 是放在函数的左边，但是实际在计算的时候是按上面的公式计算的，也就是放在右边的。
 

7、计算cpu耗时

  // 计算cpu耗时
  const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
  if (last_thread_cpu_time_seconds_.has_value()) {
    const double thread_cpu_duration_seconds =
        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value()) {
      kLocalSlamCpuRealTimeRatio->Set(
          common::ToSeconds(sensor_duration.value()) /
          thread_cpu_duration_seconds);
    }
  }
  last_wall_time_ = wall_time;
  last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;

该部分的代码不涉及到算法，就不再进行讲解了，单纯的逻辑处理。
 

三、框架梳理2→LocalTrajectoryBuilder2D::ScanMatch()

上面的讲解 4、扫描匹配校准位姿，其调用了函数 LocalTrajectoryBuilder2D::ScanMatch()，该函数有两个形参①pose_prediction→先验位姿(待优化)；②filtered_gravity_aligned_point_cloud→经过重力矫正之后的点云数据。该函数的主体流程如下：

$(1)$ 从活跃的子图 active_submaps_ 中获得第一个子图，用于后续的扫描匹配。这里需要会议一下前面的内容，active_submaps_ 中最多保存两个子图。

$(2)$ 判断 use_online_correlative_scan_matching 参数，该参数在：

src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_3d.lua

文件中进行配置。如果配置为 true，则会在进行正式扫描匹配之前，进行一次相关性扫描匹配(correlative scan matching)，实际上就是暴力搜索匹配。十分的耗时，不过可以对先验进行初步的优化，令后续扫描匹配的结果更加精准，下篇博客会进行详细的介绍。

$(3)$ 使用ceres进行扫描匹配，该部分十分的重要，后续会进行详细介绍。

$(4)$ 使用 metrics::Histogram 把数据都记录下来，优化后位姿 pose_observation 与 优化前位姿 pose_prediction 的平移差，或者角度差，这些数据都可以保存下来进行度量，或者可视化。

代码注释如下所示：

/**
 * @brief 进行扫描匹配
 * 
 * @param[in] time 点云的时间
 * @param[in] pose_prediction 先验位姿
 * @param[in] filtered_gravity_aligned_point_cloud 匹配用的点云
 * @return std::unique_ptr<transform::Rigid2d> 匹配后的二维位姿
 */
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
  if (active_submaps_.submaps().empty()) {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  // 使用active_submaps_的第一个子图进行匹配
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
  if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  // 使用ceres进行扫描匹配
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  // 一些度量
  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  // 返回ceres计算后的位姿
  return pose_observation;
}

 

四、结语

通过该篇博客，知道了扫描匹配的总体调用流程，当然，还没有对其进行细致的讲解。根据上面的分析，可以知道，扫描匹配主调部分都集中在 LocalTrajectoryBuilder2D::ScanMatch() 函数中，其中有如下两个部分是十分重要的：

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
  if (options_.use_online_correlative_scan_matching()) {
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  // 使用ceres进行扫描匹配
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);

下一篇博客，我们来讲解一下 RealTimeCorrelativeScanMatcher2D real_time_correlative_scan_matcher_ ，相关性扫描匹配，其原理就是进行遍历的暴力匹配。