cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

// This is an implementation of the algorithm described in "Real-Time Correlative Scan Matching" by Olson.
// 这是Olson在“实时相关扫描匹配”中描述的算法的实现。
// It is similar to the RealTimeCorrelativeScanMatcher but has a different trade-off: Scan matching is faster because more effort is put into the precomputation done for a given map. However, this map is immutable after construction.
//它类似于RealTimeCorrelativeScanMatcher，但有一个不同的权衡：扫描匹配速度更快，因为在给定地图的预计算中投入了更多精力。但是，此地图在构造后是不可变的。

Match() 或 MatchFullSubmap() --> MatchWithSearchParameters() --> ComputeLowestResolutionCandidates() --> BranchAndBound()

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

bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate) const {
  // Compute a search window around the center of the submap that includes it
  // fully.
  // 围绕包含它的子图的中心计算搜索整个窗口
  const SearchParameters search_parameters(
      1e6 * limits_.resolution(),  // Linear search window, 1e6 cells/direction.
      M_PI,  // Angular search window, 180 degrees in both directions.
      point_cloud, limits_.resolution());
  // 计算搜索窗口的中点 把这个中点作为搜索的起点
  const transform::Rigid2d center = transform::Rigid2d::Translation(
      limits_.max() - 0.5 * limits_.resolution() *
                          Eigen::Vector2d(limits_.cell_limits().num_y_cells,
                                          limits_.cell_limits().num_x_cells));
  return MatchWithSearchParameters(search_parameters, center, point_cloud,
                                   min_score, score, pose_estimate);
}


// The actual implementation of the scan matcher, called by Match() and 
// MatchFullSubmap() with appropriate 'initial_pose_estimate' and 'search_parameters'.
// 扫描匹配器的实际实现，由Match（）和MatchFullSubmap（）调用，
// 并带有适当的“ initial_pose_estimate”和“ search_parameters”。
// 根据搜索窗口和初始位置进行scan-match来进行位姿的优化。
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);

  // 把激光数据旋转到世界坐标系中的0度的位置
  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())));

  // 生成一系列的rotated scans，各种不同的角度的scan，60度或者360度范围内的旋转
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

  // 把上面的rotated scans转换到世界坐标系中,这里进行转换的时候只需要进行平移就可以了
  // 这里的离散激光点是在最细的分辨率的地图上面   
  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());

  // 计算最低分辨率中的所有的候选解 最低分辨率是通过搜索树的层数、地图的分辨率计算出来的。
  // 对于地图坐标系来说 最低分辨率=1<<h h表示搜索树的总的层数
  // 这里不但对最低分辨率的所有候选解的得分进行了计算　同时还按照从大到小排列
  const std::vector<Candidate2D> lowest_resolution_candidates =
      ComputeLowestResolutionCandidates(discrete_scans, search_parameters);

  // 用分枝定界方法来计算最优的候选解
  const Candidate2D best_candidate = BranchAndBound(
      discrete_scans, search_parameters, lowest_resolution_candidates,
      precomputation_grid_stack_->max_depth(), min_score);

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

MatchWithSearchParameters() 调用了correlative_scan_matcher_2d.h 的　GenerateRotatedScans()、DiscretizeScans()方法。

扫描二维码关注公众号，回复： 10441527 查看本文章

struct SearchParameters {...} 也在correlative_scan_matcher_2d.h中声明：目的是　设置搜索窗口的大小

MatchFullSubmap()中给定了在搜索空间的大小，xy方向10的6次方×分辨率，角度的搜索空间为180度。

std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::ComputeLowestResolutionCandidates(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters) const {

　// 调用GenerateLowestResolutionCandidates() 生成最低分辨率层的所有可行解
  std::vector<Candidate2D> lowest_resolution_candidates =
      GenerateLowestResolutionCandidates(search_parameters);

  // 计算每个Candidates的得分 根据传入的地图在这个地图上进行搜索来计算得分
  // 按照匹配得分　从大到小　排序，返回排列好的candidates 
  ScoreCandidates(
      precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()),
      discrete_scans, search_parameters, &lowest_resolution_candidates);
  return lowest_resolution_candidates;
}

整个树结构按照默认参数分为了７层，最上一层的搜索步长最大，越向下，搜索步长越小，最后一层（叶子节点层）即为所有的可行解。

ComputeLowestResolutionCandidates　最低分辨率的候选解：指的是，最上面一层的可行解。层数为７情况下，最低分辨率为2的６次方，即size为64。

std::vector<Candidate2D>
FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates(
    const SearchParameters& search_parameters) const {
  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;
  }

  // 将所有可行解保存起来，可行解的结构为（对应角度，x偏移量，y偏移量，搜索参数）
  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: 计算当前层的候选解在对应分辨率地图中的匹配得分，并按照得分从大到小的顺序排序。即，最低分辨率（最上一层）这层候选解，对应在size为64的地图中的匹配得分，最下一层候选解对应着size为1的地图中的匹配。

void FastCorrelativeScanMatcher2D::ScoreCandidates(
    const PrecomputationGrid2D& precomputation_grid,  // 用来计算分数的地图
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {

  // 每个候选解 枚举所有的激光点 累计占用概率log-odd
  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);
      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()

// 
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 {
  
  // 检查是否是最底层(叶子层)，如果已经查找到最底层了，则返回分数最高的候选解，排序在第一位的
  if (candidate_depth == 0) {
    // Return the best candidate.
    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;
    }

    // 对其进行分枝，搜索步长减为上层的一半
    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);
      }
    }

    // 对这层的所有可行解进行打分，并按照分数从大到小进行排列,计算所有的候选解(4个)的得分
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);

    // 从此处开始迭代，对分数最高的节点继续进行分支，直到最底层，然后再返回倒数第二层再进行迭代
    // 如果倒数第二层的最高分没有上一个的最底层（叶子层）的分数高，则跳过，否则继续向下进行评分
    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;
}

搜索顺序如图所示：

此图引用于参考文献[3]

优先对每层中分数最高的节点进行分支，直到最底层，之后再返回到倒数第二层中的第二个节点继续进行迭代。

因此，这是一种深度优先的分支定界的搜索，也就是论文中的　DFS branch and bound scan matcher for (BBS)

最终返回一个得分最高的节点，计算此节点与起始点的坐标变换，求得此节点的位姿估计pose_estimate。之后再通过ceres对这个粗匹配进行迭代计算，求得一个更精确的　pose_estimate，即为约束。

普通约束和闭环约束的区别在与搜索空间的不同，闭环约束的搜索空间太大，所以carto通过分支定界法进行了加速搜索，从而实现了秒级的实时闭环检测。

references

1 https://blog.csdn.net/xiaoma_bk/article/details/83040559 --- cartographer 添加约束 /分支界定法

2 https://blog.csdn.net/u013620235/article/details/72956929 --- Cartographer中的branch and bound算法的理解

3 https://blog.csdn.net/weixin_36976685/article/details/84994701 --- carto论文解析，分支定界讲的不错

李太白lx

发布了30 篇原创文章 · 获赞 43 · 访问量 6万+

私信关注

cartographer探秘第四章之代码解析（五） --- 后端优化 --- 闭环约束

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

分支定界算法的实现　BranchAndBound()

references

猜你喜欢

cartographer探秘第四章之代码解析（五） --- 后端优化 --- 闭环约束

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

分支定界算法的实现 BranchAndBound()

references

猜你喜欢

分支定界算法的实现　BranchAndBound()