Cartographer源码阅读06-CSM-前端匹配 TSDF 似然域

CSM（相关性扫描匹配）是Cartographer实现帧间匹配的两个算法之一，简单来说是暴力搜索3层for（θ，x，y）循环，并且先搜索角度，来降低计算量，构造多分辨率地图，先在粗分辨率地图上找到位姿的最优得分，然后在细分辨率地图上搜索最优位姿。

代码在Cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc

1.RealTimeCorrelativeScanMatcher.h

本部分的RealTimeCorrelativeScanMatcher没有实现多分辨率地图搜索,只是对所有的位姿进行枚举.

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:
  //生成所有的候选解.
  std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
      const SearchParameters& search_parameters) const;

  const proto::RealTimeCorrelativeScanMatcherOptions options_;
};

2.RealTimeCorrelativeScanMatcher.cc

2.1产生候选解GenerateExhaustiveSearchCandidates

计算出来所有的要进行搜索的解。即得到三层for循环的所有的组合,在Match()里面被调用.

std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
    const SearchParameters& search_parameters) const
{
  int num_candidates = 0;
  //计算 一共有多少的candidates。即三层循环中一共要计算多少次score
  //相当于num_scans*num_linear_x_candidates*num_linear_y_candidates
  //这里的num_scans表示一共由多少的角度搜索次数
  //先对角度θ进行搜索，相比先对x,y进行搜索，再对θ进行搜索，大大降低了搜索次数
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index)
  {
    //计算x的候选解的数量
    const int num_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + 1);

    //计算y的候选解
    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;
  }

  //获得三层for循环的组合起来的所有的解　这里所有的点角度都是下标　xy都是grid_index
  std::vector<Candidate2D> 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;
}

2.2正式匹配-Match

实现的是RealTime CSM论文里面的方法:Computing 2D Slices.这里并没有进行多分辨率地图的构建　,因此这里实际上就是进行枚举而已。

枚举窗口中每一个位姿的得分，以选取最优位姿,并没有进行什么高级的加速策略,用来提供后续ceres_scan_match的初始位姿.

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

  //把激光雷达数据转换到地图坐标系中.
  //经过这个函数之后，所有的激光数据的原点都和世界坐标系重合。
  //而角度也都是在世界坐标系中描述的。
  //因此对于各个不同的x_offset y_offset只需要进行激光端点的平移就可以
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));

  //得到所有的搜索解.
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

  //为每一个解进行打分.
  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;
}

2.3给解打分-ScoreCandidates

void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
    const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const
{
  for (Candidate2D& candidate : *candidates)
  {
    //两种不同的打分函数,即两种不同的势场.
    switch (grid.GetGridType())
    {
		//似然域的打分方式，打分更加平滑
      case GridType::PROBABILITY_GRID:
        candidate.score = ComputeCandidateScore(
            static_cast<const ProbabilityGrid&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);
        break;
		//效果差不多
      case GridType::TSDF:
        candidate.score = ComputeCandidateScore(
            static_cast<const TSDF2D&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);
        break;
    }
    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()));
  }
}

2.4似然域打分+TSDF打分方式

TSDF的打分方式

float ComputeCandidateScore(const TSDF2D& tsdf,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
  float candidate_score = 0.f;
  float summed_weight = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset);
    const std::pair<float, float> tsd_and_weight =
        tsdf.GetTSDAndWeight(proposed_xy_index);
    const float normalized_tsd_score =
        (tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) /
        tsdf.GetMaxCorrespondenceCost();
    const float weight = tsd_and_weight.second;
    candidate_score += normalized_tsd_score * weight;
    summed_weight += weight;
  }
  if (summed_weight == 0.f) return 0.f;
  candidate_score /= summed_weight;
  CHECK_GE(candidate_score, 0.f);
  return candidate_score;
}

似然域的打分方式

float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
  float candidate_score = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    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());
  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

小节

本文对CSM(相关性匹配)中的RealTimeCorrelativeScanMatcher算法进行了阅读,不过让人失望的是,没有像多分辨地图的优化算法. 这部分的计算结果是为ceres扫描匹配进行初始值的优化.