cartographer详解（三）—— Real Time Correlative Scan Matcher（2d模式）

cartographer详解（三）—— Real Time Correlative Scan Matcher（2d模式）

摘要：关于real time 匹配的主要原理、代码详解，上一章：cartographer详解（二）—— Real Time Correlative Scan Matcher（3d模式）做了详细的介绍。这一章主要分析2d模式下的做法。
主要区别：
3d: 暴力搜索
2d: 预投影法：先通过角度切片，然后在在xy方向上暴力搜索，为了减少计算量。
减少计算量的原理：

一、主要工作逻辑

• 以初始位置（角度）为基础，角度搜索范围内，按照搜索步长进行旋转当前scan。将这一系列的离散化的scan，按照旋转顺序存储起来。也就是切片
• 在每个切片内，以初始位置（xy）为基础，xy搜索搜索范围内，按照搜索步长，进行离散，生成候选位置。
• 求得每个候选位置的score，score最高的，就为最佳位置。

二、代码详解

下面通过代码，展示具体实现细节。代码里关键的地方有注释，顺着代码应该可以比较清楚的理清具体工作逻辑。

• 代码总入口Match

double RealTimeCorrelativeScanMatcher::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud,
    const ProbabilityGrid& probability_grid,
    transform::Rigid2d* pose_estimate) const {
  CHECK_NOTNULL(pose_estimate);

  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, probability_grid.limits().resolution());
  //切片旋转,按照搜索角度/步长，以初始值为基础，生成一些列的旋转后的点云，按顺序存放在vector里
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  
  //将旋转点云，从旋转点云坐标中，转化到地图坐标中去。根据坐标，获得 cell index
  const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
      probability_grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  //生成所有候选位置
  std::vector<Candidate> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  //对每个候选位置进行打分
  ScoreCandidates(probability_grid, discrete_scans, search_parameters,
                  &candidates);
  //找到score最高的候选位置，就为所求位置。
  const Candidate& 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;
}

• 切片旋转GenerateRotatedScans

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud& point_cloud,
    const SearchParameters& search_parameters) {
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans); //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) {
    rotated_scans.push_back(
      //将点云point_cloud 按照旋转角度delta_theta 进行旋转
      sensor::TransformPointCloud(
      point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));
  }
  return rotated_scans;
}

• 离散化点云DiscretizeScans

std::vector<DiscreteScan> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {
  std::vector<DiscreteScan> discrete_scans;
  // typedef std::vector<Eigen::Array2i> DiscreteScan;
  discrete_scans.reserve(scans.size());
  for (const sensor::PointCloud& scan : scans) {
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());
    for (const Eigen::Vector3f& point : scan) {
      const Eigen::Vector2f translated_point =   //从旋转点云坐标中，转化到地图坐标中去
          Eigen::Affine2f(initial_translation) * point.head<2>();
      discrete_scans.back().push_back(
        //获得该点，对应的栅格索引
          map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(),
                                                     translated_point.y()));
    }
  }
  return discrete_scans;
}

• 生成所有候选位置GenerateExhaustiveSearchCandidates

std::vector<Candidate>
RealTimeCorrelativeScanMatcher::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<Candidate> 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

void RealTimeCorrelativeScanMatcher::ScoreCandidates(
    const ProbabilityGrid& probability_grid,
    const std::vector<DiscreteScan>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate>* const candidates) const {
  for (Candidate& candidate : *candidates) {
    candidate.score = 0.f;
    for (const Eigen::Array2i& xy_index :
         discrete_scans[candidate.scan_index]) {
      const Eigen::Array2i proposed_xy_index(
          xy_index.x() + candidate.x_index_offset,
          xy_index.y() + candidate.y_index_offset);
      const float probability =
          probability_grid.GetProbability(proposed_xy_index);
      candidate.score += probability;
    }
    candidate.score /=
        static_cast<float>(discrete_scans[candidate.scan_index].size());
    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()));
    CHECK_GT(candidate.score, 0.f);
  }
}