(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()

该函数主要的功能是对传入的点云数据做一个平移，其需要传递三个函数：
①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;
}