沉默杀手HomeArchivesTagsCategories
后端 6 快速相关性匹配
|激光SLAMCartographer源码解读|
Word count: 1.3k|Reading time: 5 min

如果当前的scan和所有已创建完成的submap中的某个scan的位姿在距离上足够近，那么通过某种 scan match策略就会找到该闭环。Fast Correlative scan match 就是回环检测

FastCorrelativeScanMatcher2D的构造函数

1
2
3
4
5
6
7
FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options)
    : options_(options),
      limits_(grid.limits()),
      precomputation_grid_stack_(
    absl::make_unique<PrecomputationGridStack2D>(grid, options) ) {}

再来看PrecomputationGridStack2D的构造函数，引入预算图的概念(precomputed grids)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
// 存储同一个地图但分辨率不同，低分辨率地图value，采用对应高分辨地图中子格中最高分辨率
PrecomputationGridStack2D::PrecomputationGridStack2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options)
{
  CHECK_GE(options.branch_and_bound_depth(), 1);
  // 确定最粗的分辨率，也就是64个栅格合成一个格子
  // 注意不是7次方，而是2的6次方，64
  const int max_width = 1 << (options.branch_and_bound_depth() - 1);

  // vector<PrecomputationGrid2D>  precomputation_grids_;
  precomputation_grids_.reserve(options.branch_and_bound_depth());
  std::vector<float> reusable_intermediate_grid;
  const CellLimits limits = grid.limits().cell_limits();
  // vector 大小为，应该是每层存储的的grid，空间开辟意义不大，每层都会再次resize
  reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
                                     limits.num_y_cells);
  for (int i = 0; i != options.branch_and_bound_depth(); ++i)
  {
    /*  precomputation_grids_插入的仍然是7个
    从 2^0(0.05)  到  2^6
    队列中最前的为分辨率最高的地图，需要对原图进行扩展，而width是扩展和偏移量
    emplace_back会生成一个临时的变量，会调用PrecomputationGrid2D的构造函数
     */
    const int width = 1 << i;
    precomputation_grids_.emplace_back(grid, limits, width,
                          &reusable_intermediate_grid);
  }
}

传入地图grid为原分辨率地图，即为最高分辨地图。预处理地图堆栈则保存了n张不同分辨率的栅格地图，存储压缩2,4,8,16倍等等地图。首先是原分辨率地图，最后存储最粗分辨率的地图。不同层的地图，目的是后续相关匹配在不同分辨率地图下匹配，即为分支定界中的层。为保证上边界正确性，高层中的评分一定高于其底层节点的评分。压缩的地图并非直接从原图固定间隔采样，而是将固定间隔中所有坐标概率值最大值作为低分辨率地图，以此类推完成整个地图栈预处理。

在低分率下的地图匹配其相关性一定较高，如果分辨率继续降低，则极限为概率为1。

原始地图一直是不变的，例如每一层的分辨率都是在原始地图上生成的。即4 * origin_resolution分辨率地图是在origin_resolution分辨率地图上生成，而不是在2 * origin_resolution分辨率地图上生成的.

Match函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/*
input:
当前帧估计位置（里程计等提供的初始位置）
当前帧点云（即以激光雷达为坐标系的点云）
最小置信度
（grid在构造函数已经传递）

output：
置信度清单
匹配后输出位置
 */
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

开始部分同前端的相关性匹配相同

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
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(score != nullptr);
  CHECK(pose_estimate != nullptr);
   
  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())));
  // 根据将角度窗口按照一定分辨率划分，并根据每一个旋转角度将点云旋转，生成N个点云
  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()));



中间部分其实还是和前端那里类似

1
2
3
4
5
// 修复下所有点云的大小在空间的大小，即边界
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
//获取低分辨率的量化列表（和标准相关方法对比），并且计算匹配评分结果，并进行了排序
const std::vector<Candidate2D> lowest_resolution_candidates =
    ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
1
2
3
4
5
6
7
8
9
10
11
12
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生成候选的最粗分辨率像素偏移集,，要注意的是

1
2
3
4
5
6
7
// 注意不是7次方，而是2的6次方，64
const int linear_step_size = 1 << precomputation_grid_stack_->max_depth();
			......
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;

这里和前端的处理稍微不同，是为了处理最低分辨率的地图，搜索空间也与最低分辨率一致，最后所有位置及其对应评分放入集合中。

ScoreCandidates函数计算评分，并按照评分从高到低排序，最后返回分数从大到小排列的像素偏移集.

剩下的就是分支定界，然后把最佳候选的得分和最小得分比较，获得位姿

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 分支边界搜索最佳匹配
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;
}