cartographer_real_time_correlative_scan_matcher_2d

0.引言

不错的参考文章

实时相关性扫描匹配：

暴力匹配

。

暴力搜索三层 for 循环
预先投影搜索把暴力搜索中的三层 for 循环
$n_x n_y n_θ n x n y n θ 降为 n θ n_θ n θ 。能极大的加速算法的运行速度.$

主要实现在
real_time_correlative_scan_matcher_2d.h
与
correlative_scan_matcher_2d.h
中，接口函数核心函数为
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 {

1.SearchParameters

step 2 根据配置参数初始化 SearchParameters

  const SearchParameters search_parameters(
    options_.linear_search_window(), options_.angular_search_window(),
    rotated_point_cloud, grid.limits().resolution());

位于
correlative_scan_matcher_2d.h
中。

参考

。

角度步长angular_step_size的计算：

请添加图片描述

余弦定理：

请添加图片描述

  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);
  }

  // 根据论文里的公式 求得角度分辨率 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)));

根据

参数角度搜索的范围窗口大小

以及

角度搜索步长

求取角度搜索窗口的步数：

  // 范围除以分辨率得到个数
  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;

所以，参数设置的其实是搜索角度的一半。

x,y搜索栅格数的计算

  // XY方向的搜索范围, 单位是多少个栅格
  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);

  // linear_bounds 的作用是确定每一个点云的最大最小边界
  // 类似于 八连通遍历的 -1 0 +1 数组作用
  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});
  }

所有的参数进行了初始化：

  int num_angular_perturbations;            // 角度步数
  double angular_perturbation_step_size;    // 角度每步的度数
  double resolution;
  int num_scans;                            // 实际角度步数，旋转后的点云集合的个数？
  std::vector<LinearBounds> linear_bounds;  // 确定每一个点云的最大最小边界// 类似于 四连通遍历的 -1 0 +1 数组

2.生成所有的候选解

step 3 生成按照不同角度旋转后的点云集合

  const std::vector<sensor::PointCloud> rotated_scans =
    GenerateRotatedScans(rotated_point_cloud, search_parameters);

输入参数为待匹配的点云，以及搜索参数
search_parameters
, 生成 num_scans 个旋转后的点云，代码很直观：

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

Step: 4 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引

const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
    grid.limits(), rotated_scans,
    Eigen::Translation2f(initial_pose_estimate.translation().x(),
                         initial_pose_estimate.translation().y()));

前面 step 1 中只进行了旋转调整了姿态，没有进行平移。这步就对 step 3 按照步长旋转得到的各个候选点云进行平移。

待匹配点云一帧 –> 按步长旋转生成候选帧 num_scans 帧 –> 对 num_scans 帧进行平移

这步中，返回的就直接是 num_scans 帧各个点云在栅格地图中的索引。

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

Step: 5 生成所有的候选解
```
std::vector<Candidate2D> candidates =
   GenerateExhaustiveSearchCandidates(search_parameters);
```
把所有 num_scans 候选帧的旋转平移计算出来。

注意这里使用
scan_index
标记来自于哪一

个候选帧。

3.获取最优解

step 6 计算所有候选解的加权得分

ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

请添加图片描述

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

Step: 7 获取最优解

const Candidate2D& best_candidate =
    *std::max_element(candidates.begin(), candidates.end());

std::max_element需要有比较函数，在 Candidate2D结构体中有实现：

  bool operator<(const Candidate2D& other) const { return score < other.score; }
  bool operator>(const Candidate2D& other) const { return score > other.score; }

Step: 8 将计算出的偏差量加上原始位姿获得校正后的位姿

*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));

原文链接：https://blog.csdn.net/fb_941219/article/details/125296092