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

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

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

一、前言

在上一篇博客中，大致对扫描匹配暴力求解的原理进行了讲解，同时列举了比较形象的例子。虽然说原理不是很复杂，但是代码实现起来还是存在一定难度的。首先在上一篇博客中提到 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。为了方便后需的理解，本人做了如下图示，首先在配置文件中可以找到类似如下配置：

  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.15, --遍历区域的边长
    angular_search_window = math.rad(1.), --遍历角度，弧度制
    translation_delta_cost_weight = 1e-1, --位姿偏移权重
    rotation_delta_cost_weight = 1e-1, --位姿旋转权重
  },

先看下面的图示，后续依照该图示进行讲解：
在这里插入图片描述

图1

1、白色方格 → 其尺寸为物理单位，这里是一个映射关系，把Cartographe地图映射到栅格地图上。
2、紫色方格 → 需要遍历的区域
3、紫色多边形 → 代表机器人，当然也可以理解为雷达传感器原点，或者点云数据的原点。
4、黄色圆形 → 点云数据。
5、蓝色圆形 → 最远的点云
6、蓝色正方形区域→需要被遍历的区域
7、max_scan_range → 最远点云相对于机器人的距离
8、resolution → 地图分辨率
9、红色线段 → 与max_scan_range一致，长度表示最远点云相对于机器人的距离
10、蓝色角度 → angular_perturbation_step_size，角分辨率

二、核心被调函数

上面调用的 real_time_correlative_scan_matcher_.Match 函数为 src/cartographer/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc 文件中的 RealTimeCorrelativeScanMatcher2D::Match() 函数，这里大致看一下逻辑即可，后续会对其进行十分细致的分析。

$\color{blue}(1)$ 对点云进行处理，原本的点云是相对于重力坐标系(Z轴与重力平行，原点与local系相同)，这里记 point_cloud= $points^{gravity}$ ，首先需要把点云数据变换到机器人坐标系下，变换之后记记 rotated_point_cloud= $points^{tracking}$ ，基于重力系机器人初始位姿记 initial_pose_estimate= $\mathbf {Robot}^{tracking}$ ，那么可得数学公式如下：
$\color{Green} \tag{01}points^{tracking}=\mathbf {Robot}^{tracking}_{gravity}*points^{gravity}$

$\color{blue}(2)$ 首先需要计算出角分辨率，也就是上图中的 angular_perturbation_step_size。源码中使用余弦求角公式 $\color{Green} \tag{02} cosA=\frac{b^2+c^2-a^2}{2bc}$ 令最远的点云距离 max_scan_range $= b = c$ ，分辨率 resolution=a，这样就可以计算出 cosA，然后再调用 arccos 函数，即可获得角分辨率的度数。

$\color{blue}(3)$ 角度遍历→对点云rotated_point_cloud= $points^{tracking}$ 进行旋转，每次旋转的角度为角分辨 angular_perturbation_step_size，旋转的次数由参数 angular_search_window 进行控制。每次旋转之后的点云都添加到集合 std::vector<sensor::PointCloud> rotated_scans 之中。

$\color{blue}(4)$ 区域遍历→把点云集合 rotated_scans 进行平移，平移到上图中正方形区域的各个方格，然后每个方格的位置都会对 rotated_scans(平移过后) 进行匹配，这里的匹配理解为打分即可。然后获得多个评分不一样的候选解，这里的候选解是以偏差的形式存储的。

$\color{blue}(5)$ 计算所有候选解的加权得分，将计算出的偏差量加上原始位姿获得校正后的位姿。

关于 RealTimeCorrelativeScanMatcher2D::Match() 函数的粗略注释如下，大致看一下即可，对基本流程了解之后，后面就是对细节的分析了。

/**
 * @brief 相关性扫描匹配 - 计算量很大
 * 
 * @param[in] initial_pose_estimate 预测出来的先验位姿
 * @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
 * @param[in] grid 用于匹配的栅格地图
 * @param[out] pose_estimate 校正后的位姿
 * @return double 匹配得分
 */
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  CHECK(pose_estimate != nullptr);

  // Step: 1 将点云旋转到机器人坐标系下
  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));

  // 根据配置参数初始化 SearchParameters
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());

  // Step: 2 生成按照不同角度旋转后的点云集合
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  
  // Step: 3 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  
  // Step: 4 生成所有的候选解
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  
  // Step: 5 计算所有候选解的加权得分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

  // Step: 6 获取最优解
  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  
  // Step: 7 将计算出的偏差量加上原始位姿获得校正后的位姿
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y},
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
  return best_candidate.score;
}

三、SearchParameters 类

在上面的函数中，其首先是根据公式(1)→ $points^{tracking}=\mathbf {Robot}^{tracking}_{gravity}*points^{gravity}$ 把local系下的点云数据point_cloud变换到机器人坐标系下，赋值给变量rotated_point_cloud。虽然可以看到如下代码：

  // 根据配置参数初始化 SearchParameters
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());

SearchParameters 的主要功能是根据配置文件的参数，计算出暴力匹配过程中需要的参数，以及以缩小搜索范围的函数，先分析一下 correlative_scan_matcher_2d.h 文件

1、参数分析

首先其包含了结构体 LinearBounds，如下：

  struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };

其单位是像素，表示的是像素偏移，可以理解为一个矩形框，与机器人所在的位置共同确定一个搜索或者说遍历范围。然后还存在如下变量；

  int num_angular_perturbations;            // 每次角度遍历的次数
  double angular_perturbation_step_size;    // 角度分辨率
  double resolution;
  int num_scans;                            // 旋转后的点云集合的个数
  std::vector<LinearBounds> linear_bounds;  // Per rotated scans.

2、构造函数

其存在两个构造函数，第一个是根据配置文件的参数计算出 correlative scan matcher 所需的参数，第二个构造函数则是需要传递已经计算好的参数，如角分辨率，遍历区域偏移值等。第二个构造函数较为简单，所以这里就不进行讲解了，只对第一个构造函数进行分析，位于 src/cartographer/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc 文件中

// 构造函数
SearchParameters::SearchParameters(const double linear_search_window,
                                   const double angular_search_window,
                                   const sensor::PointCloud& point_cloud,
                                   const double resolution)
    : resolution(resolution) {

首先其需要传递4个参数，linear_search_window 与 angular_search_window 在配置文件中可以进行设置，其分别用于间接控制搜索区域(遍历区域)的偏移值，与搜索的角分辨率。point_cloud 为点云数据，resolution 表示地图分辨率。首先运行了如下代码：

  // We set this value to something on the order of resolution to make sure that
  // the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;

  // 求得 point_cloud 中雷达数据的 最大的值（最远点的距离）
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }

该段代码是为了计算出最远点云数据的距离 max_scan_range ，其与图1中的 max_scan_range 是一致的。且该代码保证了 max_scan_range 最小值必须大于3.f * resolution。这样是为了点云较近的时候搜索角度过大。再看如下代码：

  // 根据论文里的公式 求得角度分辨率 angular_perturbation_step_size
  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));

该部分代码就与公式(2) $cosA=\frac{b^2+c^2-a^2}{2bc}$ 对应起来了，其 max_scan_range=b=c，resolution=a。再调用 std::acos 函数求解角分辨率。其上的 kSafetyMargin 是为了限制角度超过边界。那么再接着往下分析：

  // angular_search_window 除以分辨率得到遍历该角度需要搜索的次数。
  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;

这里需要注意一个点，配置文件中的参数 angular_perturbation_step_size 表示单个角度方向搜索的角度，所以乘以2才表示左右两个方向的角度。这里的+1，可以理解为中间位置(平行y轴)时的搜索。也就是说最终 num_scans 表示搜索完 angular_search_window 角度的范围，需要旋转变换 num_scans 次，每次旋转的角度为角分辨率 angular_perturbation_step_size。求得角度搜索范围之后，就是求得x,y(图1的矩形)区域偏移值了，如下所示：

  // XY方向的搜索范围, 单位是多少个栅格
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
      
  linear_bounds.reserve(num_scans);
  // 每一次角度的旋转，需要对所有区域进行遍历
  for (int i = 0; i != num_scans; ++i) {  
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations});
  }

这里是把每个搜索角度需要遍历矩形框偏移值先计算出来。根据前面的分析，先毛估一下(回忆下面的类容)：

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)

猜测其是按行进行遍历的，假设其在角度为 -35 的时候，遍历完所有的方格，然后向右旋转5度，再遍历完所有的方格，依此循环。当然，这里仅仅是猜测，后续分析再做最终确认。这样就把 SearchParameters 函数的构造函数分析完成了。

3、ShrinkToFit()

该函数比较奇怪，在构造函数中的变量 linear_bounds 只是包含了一小块需要遍历的区域，而该函数对 linear_bounds 进行了扩大，把所有的点云都包含在这个范围之内了，代码注释如下：

// 计算每一帧点云 在保证最后一个点能在地图范围内时 的最大移动范围
void SearchParameters::ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                                   const CellLimits& cell_limits) {
  CHECK_EQ(scans.size(), num_scans);
  CHECK_EQ(linear_bounds.size(), num_scans);

  // 遍历生成的旋转后的很多scan
  for (int i = 0; i != num_scans; ++i) {
    Eigen::Array2i min_bound = Eigen::Array2i::Zero();
    Eigen::Array2i max_bound = Eigen::Array2i::Zero();

    // 对点云的每一个点进行遍历, 确定这帧点云的最大最小的坐标索引
    for (const Eigen::Array2i& xy_index : scans[i]) {
      // Array2i.min的作用是 获取对应元素的最小值组成新的Array2i
      min_bound = min_bound.min(-xy_index);
      max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1,
                                               cell_limits.num_y_cells - 1) -
                                xy_index);
    }

    //调整linear_bounds，使点云被包含在其区全域内
    linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x());
    linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x());
    linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y());
    linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y());
  }
}

四、结语

对于 SearchParameters::ShrinkToFit() 函数，本人存在一些疑问，不过在 src/cartographer/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc 文件中，似乎并没有调用该函数，所以本人也没有深究了，有兴趣的朋友可以深入了解一下。