Cartographer代码阅读笔记：回环检测

有关文件：cartographer/mapping/2d/scan_matching/fast_correlative_scan_matcher_2d.cpp

Cartographer有关其他分析文章：回到目录

接下来探讨一下每个函数的具体实现过程：

---

Match函数分析

/** 
 * @brief 给定初始位姿时的匹配
 * @param initial_pose_estimate 初始位姿估计
 * @param point_cloud 将要考察激光点云数据
 * @param min_score 回环检测的最小得分，当做阈值使用
 * @param score 成功匹配后返回匹配得分
 * @param score 成功匹配后返回位姿估计
 * @return 如果闭环检测成功则返回ture，否则返回false
 */
bool FastCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, 
    const float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {

  // 计算搜索窗口
  const SearchParameters search_parameters(options_.linear_search_window(),
                                           options_.angular_search_window(),
                                           point_cloud, limits_.resolution());
  // 实现闭环检测
  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
                                   point_cloud, min_score, score,
                                   pose_estimate);
}

MatchWithSearchParameters函数分析

/** 
 * @brief 完成扫描匹配，进而实现闭环检测
 * @param search_parameters 搜索窗口的参数
 * @param initial_pose_estimate 机器人初始位姿
 * @param point_cloud 激光点云数据
 * @param min_score 回环检测匹配最小得分
 * @param score 记录回环检测匹配成功的得分
 * @param pose_estimate 记录回环检测匹配成功的位姿
 * @return 如果闭环检测成功则返回ture，否则返回false
 */
bool FastCorrelativeScanMatcher2D::MatchWithSearchParameters(
    SearchParameters search_parameters,
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {
  CHECK_NOTNULL(score);
  CHECK_NOTNULL(pose_estimate);

  // 获取初始位姿估计的方向角，将激光点云中的点都绕Z轴转动相应的角度。
  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 std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

  // 完成对旋转后的点云数据离散化操作，即将浮点类型的点云数据转换成整型的栅格单元索引。      
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      limits_, rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));

  // 尽可能的缩小搜索窗口大小，以减少搜索空间，提高搜索效率。
  search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());

  // 完成对搜索空间的第一次分割，得到初始子空间节点集合。
  const std::vector<Candidate2D> lowest_resolution_candidates =
      ComputeLowestResolutionCandidates(discrete_scans, search_parameters);

  // 完成分支定界搜索，搜索结果放在best_candidate中。
  const Candidate2D best_candidate = BranchAndBound(
      discrete_scans, search_parameters, lowest_resolution_candidates,
      precomputation_grid_stack_->max_depth(), min_score);

  // 检测最优解的值，如果大于指定阈值min_score就认为匹配成功，修改输入参数指针score和pose_estimate所指对象。      
  if (best_candidate.score > min_score) {
    *score = best_candidate.score;
    *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 true;
  }
  return false;
}

ComputeLowestResolutionCandidates函数分析

/** 
 * @brief 完成第一次分支
 * @param discrete_scans 离散化之后的各个搜索方向上的点云数据
 * @param search_parameters 搜索窗口的参数
 */
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters) const {

  // 完成对搜索空间的初始化分割（生成最低分辨率的所有候选解）。
  std::vector<Candidate2D> lowest_resolution_candidates =
      GenerateLowestResolutionCandidates(search_parameters);

  // 计算各个候选点的评分并排序。
  ScoreCandidates(
      precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
      discrete_scans, search_parameters, &lowest_resolution_candidates);
  return lowest_resolution_candidates;
}

GenerateLowestResolutionCandidates函数分析

/** 
 * @brief 生成最低分辨率的候选解
 * @param search_parameters 搜索窗口的参数
 */
std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
    const SearchParameters& search_parameters) const {

  // 根据预算图的层数计算初始分割的粒度z^(depth)    
  const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();

  // 遍历所有搜索方向，累计各个方向下空间的分割数量，得到搜索空间初始分割的子空间数量。
  int num_candidates = 0;
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    const int num_lowest_resolution_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + linear_step_size) /
        linear_step_size;

    const int num_lowest_resolution_linear_y_candidates =
        (search_parameters.linear_bounds[scan_index].max_y -
         search_parameters.linear_bounds[scan_index].min_y + linear_step_size) /
        linear_step_size;

    num_candidates += num_lowest_resolution_linear_x_candidates *
                      num_lowest_resolution_linear_y_candidates;
  }

  // 构建各个候选点
  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 += linear_step_size) {
      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 += linear_step_size) {
        candidates.emplace_back(scan_index, x_index_offset, y_index_offset, search_parameters);
      }
    }
  }
  CHECK_EQ(candidates.size(), num_candidates);
  return candidates;
}

ScoreCandidates函数分析

/** 
 * @brief 计算当前层的候选解与对应分辨率地图中的匹配得分，并按照得分从大到小的进行排序
 * @param precomputation_grid 当前层对应的预计算图
 * @param discrete_scans 待匹配的点云数据
 * @param search_parameters 搜索窗口的参数
 * @param candidates 当前层对应的候选解
 */
void FastCorrelativeScanMatcher2D::ScoreCandidates(
    const PrecomputationGrid2D& precomputation_grid,
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {

  // 遍历候选解
  for (Candidate2D& candidate : *candidates) {
    int sum = 0;
    // 遍历所有激光点
    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);
      //计算累计占用概率log_odd
      sum += precomputation_grid.GetValue(proposed_xy_index);
    }
    // 求平均并转换为概率。
    candidate.score = precomputation_grid.ToScore(
        sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
  }
  
  // 对评分进行降序排序
  std::sort(candidates->begin(), candidates->end(),
            std::greater<Candidate2D>());
}

BranchAndBound函数分析

/** 
 * @brief 分支定界方法实现
 * @param discrete_scans 离散化的不同搜索角度下的激光点云数据
 * @param search_parameters 搜索窗口的配置参数
 * @param candidates 候选点集合
 * @param candidate_depth 搜索树高度
 * @param min_score 候选点最小评分
 * @return 返回当前分支中的最优解（得分最高且大于min_score）
 */
Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    const std::vector<Candidate2D>& candidates, const int candidate_depth,
    float min_score) const {

  // 如果搜索树高度为0，意味着搜索到叶子节点。      
  if (candidate_depth == 0) {
    // 由于每次迭代过程中对新扩展的候选点进行了降序排序，所以认为队首的这个节点就是最优解，直接返回即可。
    return *candidates.begin();
  }

  // 创建一个临时的候选点对象，并为之赋予最小的评分。
  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);
  best_high_resolution_candidate.score = min_score;

  // 遍历所有候选点，
  for (const Candidate2D& candidate : candidates) { 
    // 如果遇到一个候选点的评分很低，意味着该分支之后的候选点中也没有合适的解。直接跳出循环，说明没有构成回环。
    if (candidate.score <= min_score) {  
      break;
    }
    
    // 进行分支， 分解为4个子候选解
    std::vector<Candidate2D> higher_resolution_candidates;
    const int half_width = 1 << (candidate_depth - 1);
    for (int x_offset : {0, half_width}) {
      if (candidate.x_index_offset + x_offset >
          search_parameters.linear_bounds[candidate.scan_index].max_x) {
        break;
      }
      for (int y_offset : {0, half_width}) {
        if (candidate.y_index_offset + y_offset >
            search_parameters.linear_bounds[candidate.scan_index].max_y) {
          break;
        }
        higher_resolution_candidates.emplace_back(
            candidate.scan_index, candidate.x_index_offset + x_offset,
            candidate.y_index_offset + y_offset, search_parameters);
      }
    }

    // 对新扩展的候选解评分（也称为定界）同时对评分进行排序。
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);

    // 递归调用BranchAndBound对新扩展候选解进一步执行分支、定界等搜索过程。                    
    best_high_resolution_candidate = std::max(
        best_high_resolution_candidate,
        BranchAndBound(discrete_scans, search_parameters,
                       higher_resolution_candidates, candidate_depth - 1,
                       best_high_resolution_candidate.score));
  }
  return best_high_resolution_candidate;
}

1.第一次分支的数量与所有子图总数相等，之后的每次分支数量都是4。

返回顶部 or 回到目录