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 'probability_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 ProbabilityGrid& probability_grid,
               transform::Rigid2d* pose_estimate) const;

  // Computes the score for each Candidate2D in a collection. The cost is
  // computed as the sum of probabilities, different from the Ceres
  // CostFunctions: http://ceres-solver.org/modeling.html
  //
  // Visible for testing.
  void ScoreCandidates(const ProbabilityGrid& probability_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_;
};

根据调用接口可以知道，scan匹配的是前端的active_submaps的第一个submap。
入口函数：

double RealTimeCorrelativeScanMatcher2D::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());
      // 根据搜索框参数生成多个旋转后的点云
      const std::vector<sensor::PointCloud> rotated_scans =
         GenerateRotatedScans(rotated_point_cloud, search_parameters);
      // 将每个点云加上平移后投影到网格中
      const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
                        probability_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(probability_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 Eigen::Vector3f& point : point_cloud) {
    const float range = point.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});
  }
}
// 根据search_parameters生成旋转后的点云
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);
  // 按照-angular->+angular区间，生成2 * num_angular_perturbations + 1个点云
  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(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));
  }
  return rotated_scans;
}

std::vector<DiscreteScan2D> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {
  std::vector<DiscreteScan2D> discrete_scans;
  discrete_scans.reserve(scans.size());
  // 遍历每帧中的点云
  for (const sensor::PointCloud& scan : scans) {
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());
    // 点云中的每个点加上平移，转换到世界坐标系，并得到在地图中对应的id
    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.GetCellIndex(translated_point));
    }
  }
  return discrete_scans;
}


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)
    {
        // 获取x方向offset数量
        const int num_linear_x_candidates =
            (search_parameters.linear_bounds[scan_index].max_x -
             search_parameters.linear_bounds[scan_index].min_x + 1);
        // 获取y方向offset数量
        const int num_linear_y_candidates =
            (search_parameters.linear_bounds[scan_index].max_y -
             search_parameters.linear_bounds[scan_index].min_y + 1);
        // x*y为offset总量
        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)
    {
        // 生成candidates，一个offset（一个新的位姿）对应一个点云，所以有点云数量*x方向offset数量*y方向offset数量个可能解
        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;
}

// 运用暴力匹配的方法，
void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
    const ProbabilityGrid &probability_grid,
    const std::vector<DiscreteScan2D> &discrete_scans,
    const SearchParameters &search_parameters,
    std::vector<Candidate2D> *const candidates) const
{
    // 遍历所有的candidates可能解
    for (Candidate2D &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());
        // std::hypot：计算两点之间的距离
        // 最终的score=平均score*exp^(-x^2):x=距离*权重+角度*权重
        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);
    }
}

角分辨率公式推导：

在很远的地方，一个网格分辨率r和激光雷达中心连线，如下图所示：

该三角形可以看做是等腰三角形。由余弦定理：$\cos \theta =\frac{a^2+b^2-c^2}{2ab}$，对于本三角形有：$a=b=dmax,c=r$，带入公式有：$\cos \theta =\frac{2dma{x}^2-r^2}{2dma{x}^2}=1-\frac{r^2}{2dma{x}^2}$。

看一下可能解的定义：Candidate2D：

struct Candidate2D {
  Candidate2D(const int init_scan_index, const int init_x_index_offset,
              const int init_y_index_offset,
              const SearchParameters& search_parameters)
      : scan_index(init_scan_index),
        x_index_offset(init_x_index_offset),
        y_index_offset(init_y_index_offset),
        // 得到真实的xy数据
        x(-y_index_offset * search_parameters.resolution),
        y(-x_index_offset * search_parameters.resolution),
        // scan_index包含了角度信息，计算方式如下
        orientation((scan_index - search_parameters.num_angular_perturbations) *
                    search_parameters.angular_perturbation_step_size) {}

  // Index into the rotated scans vector.
  int scan_index = 0;

  // Linear offset from the initial pose.
  int x_index_offset = 0;
  int y_index_offset = 0;

  // Pose of this Candidate2D relative to the initial pose.
  double x = 0.;
  double y = 0.;
  double orientation = 0.;

  // Score, higher is better.
  float score = 0.f;
  // 排序方法
  bool operator<(const Candidate2D& other) const { return score < other.score; }
  bool operator>(const Candidate2D& other) const { return score > other.score; }
};