Cartographer源码阅读2D-前端暴力匹配-RealTimeCorrelativeScanMatcher2D

Cartographer源码阅读2D-前端暴力匹配-RealTimeCorrelativeScanMatcher2D

使用暴力匹配的思想，解决当前激光帧和submap匹配的问题：

假定在SLAM的过程中，车体的运动是连续有界的，即运动速度是不可能达到无穷大，根据车体的运动速度，可以预测当前帧和上一帧之间的最大相对运动大小是多少，假设我们已经知道上一帧经过CSM解算过的位姿，我们将所有可能的位姿和当前激光帧组合到一起，生成可能解。然后，遍历所有可能解，把所有的可能解投射到submap上：计算当前激光帧通过其位姿转换后，得到这些hit点落在submap上的位置，并计算这些位置处的占据概率之和，如果占据概率之和越大，则表示该可能解的位姿越可靠。通过暴力匹配，得到最高得分的解即为可能解。

RealTimeCorrelativeScanMatcher2D类

class RealTimeCorrelativeScanMatcher2D {
 public:
  explicit RealTimeCorrelativeScanMatcher2D(
      const proto::RealTimeCorrelativeScanMatcherOptions& options);

  RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) =
      delete;   
  RealTimeCorrelativeScanMatcher2D& operator=(
      const RealTimeCorrelativeScanMatcher2D&) = delete;

  // Aligns 'point_cloud' within the 'grid' given an
  // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
  // returns the score.
  double Match(const transform::Rigid2d& initial_pose_estimate,
               const sensor::PointCloud& point_cloud, const Grid2D& grid,
               transform::Rigid2d* pose_estimate) const;

  // Computes the score for each Candidate2D in a collection. The cost is
  // computed as the sum of probabilities or normalized TSD values, different
  // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html
  //
  // Visible for testing.
  void ScoreCandidates(const Grid2D& grid,
                       const std::vector<DiscreteScan2D>& discrete_scans,
                       const SearchParameters& search_parameters,
                       std::vector<Candidate2D>* candidates) const;

 private:
  // 根据预设的窗口大小，生成Candidate2D数据
  std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
      const SearchParameters& search_parameters) const;

  const proto::RealTimeCorrelativeScanMatcherOptions options_;
};

入口函数:Match()

根据调用接口可以知道，scan匹配的是前端的active_submaps的第一个submap。进入real_time_correlative_scan_match_2d.cc

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

  // 点云旋转
  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())));
  // 构建搜索框参数 
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());

  // 根据搜索框参数生成多个旋转后的点云
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  // 将每个点云加上平移后投影到网格中
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  // 根据搜索框，生成candidates，即为候选解
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  // 对candidates打分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  *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::SearchParameters(const double linear_search_window,
                                   const double angular_search_window,
                                   const sensor::PointCloud& point_cloud,
                                   const double resolution)
    : resolution(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;
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }
  // 计算角度增长step
  const double kSafetyMargin = 1. - 1e-3;
  // 角度非常小，计算角分辨率的方法在论文中有体现，推导方法如下：
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  
  // 计算角度有多少个增长的step
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  
  // 数量为2*num_angular_perturbations + 1，因为在根据角度生成点云时，
  // 是从-angular_search_window到+angular_search_window生成的。
  num_scans = 2 * num_angular_perturbations + 1;

  // 计算线性搜索框有多少个step，step的size是resolution
  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});
  }

参考链接: lihttps://blog.csdn.net/yeluohanchan/article/details/108716938nk.