(02)Cartographer源码无死角解析-(50) 2D点云扫描匹配→相关性暴力匹配2：RealTimeCorrelativeScanMatcher2D

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

上一篇博客中对类 SearchParameters 进行了详细的介绍，同时对 src/cartographer/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc 文件中的 RealTimeCorrelativeScanMatcher2D::Match() 进行了大致的讲解。下面对 RealTimeCorrelativeScanMatcher2D 的各个函数进行一个具体的分析。

其给构造函数十分简单，就不单独讲解，主要就是获取如下配置参数然后赋值给成员变量 options_：

  -- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息
  -- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果
  use_online_correlative_scan_matching = false,
  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,
  },

不过再分析 RealTimeCorrelativeScanMatcher2D 的各个函数之前，需要先把 correlative_scan_matcher_2d.cc 文件中的如下两个函数进行讲解：

// 生成按照不同角度旋转后的点云集合
std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud& point_cloud,const SearchParameters& search_parameters) {
    
    ......}

// 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
std::vector<DiscreteScan2D> DiscretizeScans(const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,const Eigen::Translation2f& initial_translation) {
    
    ......}

为了方便后续的讲解，这里把上一篇博客的图示粘贴一下：
在这里插入图片描述

图1

二、GenerateRotatedScans()

该函数需要传递两个参数：①point_cloud→点云数据；②search_parameters→已经计算好的搜索配置参数。该函数的逻辑比较简单，就是对点云数据做多次旋转，每次旋转度数都是在上一次旋转的基础上再增加角分辨率度数。总的旋转次数为 search_parameters.num_scans，该参数在上一篇博客中提到过，如下：

  // 范围除以分辨率得到个数
  num_angular_perturbations =std::ceil(angular_search_window / angular_perturbation_step_size);
  // num_scans是要生成旋转点云的个数, 将 num_angular_perturbations 扩大了2倍
  num_scans = 2 * num_angular_perturbations + 1;

需要注意的是，这里的 point_cloud 点云数据是相对于机器人的，且已经进行过重力矫正，所以对该点云的旋转只需要绕z轴即可，记 scan_index=i 次旋转之后的点云为 $points^{tracking}_{scan\_i}$ ，初始点云 $point\_ cloud=points^{tracking}_{init}$ ，那么使用数学公式表示如下：
$\color{Green} \tag{01} points^{tracking}_{scan\_i}=\mathbf R^{scan\_i}_{init}* points^{tracking}_{init}$
其上的 $\mathbf R^{scan\_i}_{init}$ 等价于源码中的 transform::Rigid3f::Rotation(Eigen::AngleAxisf(delta_theta, Eigen::Vector3f::UnitZ()))。每次变换之后的结果都存储于 rotated_scans 中，遍历完成之后返回该变量，源码注释如下：

// 生成按照不同角度旋转后的点云集合
std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud& point_cloud,
    const SearchParameters& search_parameters) {
    
    
  std::vector<sensor::PointCloud> rotated_scans;
  // 生成 num_scans 个旋转后的点云
  rotated_scans.reserve(search_parameters.num_scans);
  // 起始角度
  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  // 进行遍历，生成旋转不同角度后的点云集合
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
           delta_theta += search_parameters.angular_perturbation_step_size) {
    
    
    // 将 point_cloud 绕Z轴旋转了delta_theta
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));
  }
  return rotated_scans;
}

三、DiscretizeScans()

该函数主要的功能是对传入的点云数据做一个平移，实际上调用该函数，是把点云数据变换到 local 坐标系虚下，其需要传递三个函数：
①map_limits→用于获取点云数据
②scans→通常情况下就是上一函数 GenerateRotatedScans() 的返回结果，存储 num_scans 帧不同角度的点云数据；
③initial_translation→所有点云数据需要平移的数量参数

源码中会进行两层遍历，第一层遍历for循环，其会获得每个角度的所有点云数据 scan；第二层遍历for循环，对单个角度下的每个点云数据进行处理，其处理角较为简单，就是进行简单的平移，并且计算出平移之后点云数据在栅格地图中的二维索引(坐标)，存储于变量 discrete_scans 中返回。源码注释如下：

// 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
std::vector<DiscreteScan2D> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {
    
    
  // discrete_scans的size 为 旋转的点云的个数
  std::vector<DiscreteScan2D> discrete_scans;
  discrete_scans.reserve(scans.size());

  for (const sensor::PointCloud& scan : scans) {
    
    
    // discrete_scans中的每一个 DiscreteScan2D 的size设置为这一帧点云中所有点的个数
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());

    // 点云中的每一个点进行平移, 获取平移后的栅格索引
    for (const sensor::RangefinderPoint& point : scan) {
    
    
      // 对scan中的每个点进行平移
      const Eigen::Vector2f translated_point =
          Eigen::Affine2f(initial_translation) * point.position.head<2>();

      // 将旋转后的点 对应的栅格的索引放入discrete_scans
      discrete_scans.back().push_back(
          map_limits.GetCellIndex(translated_point));
    }
  }
  return discrete_scans;
}

四、GenerateExhaustiveSearchCandidates()

对 correlative_scan_matcher_2d.cc 文件中的两个函数分析完之后，来看看类 RealTimeCorrelativeScanMatcher2D 中的函数，首先要讲解的就是 GenerateExhaustiveSearchCandidates()。从函数命名来看，表示使用穷举的方式生成候选者。那么，下面就来看看其具体是如何实现的。

$\color{blue}(1)$ 这里把角度遍历与范围遍历组合起来的结果，称为候选解，最终的目的就是从这些候选解中找到最优者。在这之前，该函数首先计算出候选解的个数，上一篇博客中，提到了 SearchParameters::linear_bounds 成员变量，该变量描述的是需要遍历的区域，也就是图1 中的蓝色正方形区域，其以像素(栅格)为单位。遍历区域的栅格数目为 num_linear_x_candidates*num_linear_y_candidates，其本质就是以像素(栅格)为单位，求面积。总的候选解还要乘以 search_parameters.num_scans，源码中使用加法的方式，应该也是一样的效果。

$\color{blue}(2)$ 创建一个保存候选解的变量 std::vector<Candidate2D> candidates，每个候选解都是 Candidate2D 类型，创建其实例对象需要参数： ①scan_index→角度遍历的索引；②x_index_offset, y_index_offset→偏移值，这里可以理解为确定搜索区域的原点之后，先对于该原点的偏移值，注意，其以像素(栅格)为单位。

$\color{blue}(3)$ 使用三个for循环进行遍历，外面的两个循环用于控制搜索(遍历)区域，最里面的循环用于控制角度搜索。这里就完成了图1 中蓝色正方形区域每个位置及其角度的搜寻，角度的范围由配置文件中的 angular_search_window 参数进行控制。

该函数所有候选解都存储于 candidates 变量中，让后返回，源码如下：

// 生成所有的候选解
std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
    const SearchParameters& search_parameters) const {
    
    
  int num_candidates = 0;
  // 计算候选解的个数
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    
    
    const int num_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + 1);
    const int num_linear_y_candidates =
        (search_parameters.linear_bounds[scan_index].max_y -
         search_parameters.linear_bounds[scan_index].min_y + 1);
    num_candidates += num_linear_x_candidates * num_linear_y_candidates;
  }

  std::vector<Candidate2D> candidates;
  candidates.reserve(num_candidates);

  // 生成候选解, 候选解是由像素坐标的偏差组成的
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    
    
    for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
         x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
         ++x_index_offset) {
    
    
      for (int y_index_offset =
               search_parameters.linear_bounds[scan_index].min_y;
           y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
           ++y_index_offset) {
    
    
        candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
                                search_parameters);
      }
    }
  }
  CHECK_EQ(candidates.size(), num_candidates);
  return candidates;
}

五、ScoreCandidates()

在对该函数进行讲解之前，需要提前知道一些东西，那就是 Cartographer 实现相关性暴力匹配匹配的时候，候选解的得分其与距离存在联系，先来看下图：
在这里插入图片描述

图2
假设矩形的中心表示通过传感器或者其他方式初步估算出来的位姿，会在附近进行移动旋转搜索，尝试找到最优的候选解，不过从源码的实现来看，其还是比较信任初步估算出来的位姿，因为在距离初始位姿越远，则其得分的权重越低，相对于初始位姿旋转角度越大，其得分的权重也越低(如上图所示，偏离绿色的角度越大)。首先来看 ScoreCandidates() 函数中的如下代码：

    // 对得分进行加权
    candidate.score *=
        std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
                                   options_.translation_delta_cost_weight() +
                               std::abs(candidate.orientation) *
                                   options_.rotation_delta_cost_weight()));

其上的 std::hypot(candidate.x, candidate.y) 就是计算与初始位姿的距离长度，candidate.orientation 表示相对于初始位姿的旋转。options_.translation_delta_cost_weight() 与 options_.rotation_delta_cost_weight() 可在配置文件中配置，默认都是权重都是1。

其上使用的是 std::exp 函数，可以很容易的看出，当距离初始位姿非常近，旋转角度非常少的时候，该函数的结果趋向于 $e^0=1$ ，其是符合原理的。另外在 ScoreCandidates() 函数中，根据 grid.GetGridType() 类型调用 ComputeCandidateScore() 函数，本人执行的代码是如下部分：

        candidate.score = ComputeCandidateScore(
            static_cast<const ProbabilityGrid&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);

六、ComputeCandidateScore()

该函数同样实现于 real_time_correlative_scan_matcher_2d.cc 文件中，非成员函数，其存在两个重载，这里只对第二个进行讲解，这里先给出代码注释：

// 计算点云在指定像素坐标位置下与ProbabilityGrid地图匹配的得分
float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
    
    
  float candidate_score = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    
    
    // 对每个点都加上像素坐标的offset, 相当于对点云进行平移
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset);
    // 获取占用的概率
    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    // 以概率为得分
    candidate_score += probability;
  }
  // 计算平均得分
  candidate_score /= static_cast<float>(discrete_scan.size());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

其传入得 discrete_scan 是基于 local 坐标系下一个角度的点云数据像素(栅格)坐标，对所有点云坐标进行遍历，根据该像素坐标获取其对应栅格的占用率，进行累加然后取平均值。

七、结语

对这些函数都分析完成之后，有必要的朋友可以再回去看一下上一篇博客中讲解的 RealTimeCorrelativeScanMatcher2D::Match() 函数，结合起来分析，相信对整体的把握就比较好了。那么，对于 LocalTrajectoryBuilder2D::ScanMatc() 函数中的如下部分可以说是讲解完成了：

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

initial_ceres_pose 经过初步矫正后存储于 pose_prediction 之中，score 表示该最优位置的分值。