cartographer代码学习-扫描匹配（暴力搜索）

在学习栅格地图的时候，我们知道在栅格更新前会先进行扫描匹配获取当前机器人最有可能所在的位姿：

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

  if (pose_estimate_2d == nullptr) {
   
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }

这个函数同样在LocalTrajectoryBuilder2D里面，它主要执行了两个操作：包里匹配以及基于ceres的LM匹配。这里我们首先看一下暴力匹配部分。

参数配置

在扫描匹配中，是否启用相关性扫描匹配（暴力匹配）根据参数use_online_correlative_scan_matching决定：

  // 根据参数决定是否 使用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);
  }

其决定了是否启动该扫描搜索器。同时，如果要使用的话，还需要配置下述一些参数：

real_time_correlative_scan_matcher = {
   
    linear_search_window = 0.1,             -- 线性搜索窗口的大小
    angular_search_window = math.rad(20.),  -- 角度搜索窗口的大小
    translation_delta_cost_weight = 1e-1,   -- 用于计算各部分score的权重
    rotation_delta_cost_weight = 1e-1,
  },

进一步，我们看一下match这个函数的具体实现。

函数实现

1、函数输入与输出

对于相关性扫描匹配函数而言，其输入输出主要包含了以下几个：

/**
 * @param[in] initial_pose_estimate 预测出来的先验位姿
 * @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
 * @param[in] grid 用于匹配的栅格地图
 * @param[out] pose_estimate 校正后的位姿
 * @return double 匹配得分
 */

在调用相关性扫描匹配函数时，前面我们看到它传递了一些参数：

const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);

上述参数中，第一项pose_prediction为位姿推测器计算出来的预测位姿，作为当前先验的存在；filtered_gravity_aligned_point_cloud是用于扫描匹配的当前点云。注意到这里的点云是位于local坐标系原点的点云。在cartographer中，似乎它的位姿跟随不是跟随的全局坐标系而是一直跟随的local坐标系，所以前期处理点云的时候都是将点云转换到local坐标系原点然后用于后期匹配以及插入；*matching_submap->grid()则是一个指针，我们知道在cartographer中地图都是采用智能指针的形式处理的，所以这里也就是相当于使用了第一张激活的地图进行匹配（cartographer中正常运行时存在两张激活的子图，它们之间差90帧）。而最后面的initial_ceres_pose则是一个根据相关性扫描匹配函数得到的新的位姿。

2、函数具体流程

看完输入输出，接下来展开看看这个函数的具体内容，它主要包含了以下几个部分：

2.1、激光数据的位姿旋转

前面我们知道，输入的点云是位于local坐标系原点的，但是传入的初始姿态一版都不是原点，我们的搜索是基于给定姿态进行前后左右的搜索的，