多分辨率候选解-cartographer
Kong Liangqian Lv6
 2021-12-09 18:38:29  

之前是生成了多分辨率地图，下面介绍如何利用多分辨率地图来进行匹配

在计算子图间约束的过程中，会通过匹配来获取约束

上次总览：

计算子图间，节点和非本子图的约束，分为局部约束和全局约束，

局部约束控制在15米以内的约束添加，通过函数ConstraintBuilder2D::MaybeAddConstraint 进行添加

在MaybeAddConstraint中，为子图新建了匹配器scan_matcher，并且通过匹配器来计算约束

1
2
3
4
5
6
// 生成个计算约束的任务
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
  ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
                    constant_data, initial_relative_pose, *scan_matcher,
                    constraint);

ConstraintBuilder2D::ComputeConstraint

根据match_full_submap 布尔值，来决定是否进行全局匹配，

首先使用分支定界算法匹配器进行粗匹配，局部地图匹配，输入的是initial_pose, 即Node在local下的坐标，点云数据，

输出的内容为pose_estimate

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
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
void ConstraintBuilder2D::ComputeConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, bool match_full_submap,
    const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose,
    const SubmapScanMatcher& submap_scan_matcher,
    std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
    // Step:1 得到节点在local frame下的坐标
  const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;
  // Compute 'pose_estimate' in three stages:
  // 1. Fast estimate using the fast correlative scan matcher.
  // 2. Prune if the score is too low.
  // 3. Refine.
  // param: global_localization_min_score 对整体子图进行回环检测时的最低分数阈值
  // param: min_score 对局部子图进行回环检测时的最低分数阈值

  // Step:2 使用基于分支定界算法的匹配器进行粗匹配
if (match_full_submap) {
    // 节点与全地图进行匹配
    kGlobalConstraintsSearchedMetric->Increment();
    if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
            constant_data->filtered_gravity_aligned_point_cloud,
            options_.global_localization_min_score(), &score, &pose_estimate)) {
      CHECK_GT(score, options_.global_localization_min_score());
      CHECK_GE(node_id.trajectory_id, 0);
      CHECK_GE(submap_id.trajectory_id, 0);
      kGlobalConstraintsFoundMetric->Increment();
      kGlobalConstraintScoresMetric->Observe(score);
    } else {
      // 计算失败了就退出
      return;
    }
  } 
  else {
    // 节点与局部地图进行匹配
    kConstraintsSearchedMetric->Increment();
    if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
            initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
            options_.min_score(), &score, &pose_estimate)) {
      // We've reported a successful local match.
      CHECK_GT(score, options_.min_score());
      kConstraintsFoundMetric->Increment();
      kConstraintScoresMetric->Observe(score);
    } else {
      return;
    }
  }

然后通过ceres进行精匹配

1
2
3
4
5
6
// Step:3 使用ceres进行精匹配, 就是前端扫描匹配使用的函数
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                          constant_data->filtered_gravity_aligned_point_cloud,
                          *submap_scan_matcher.grid, &pose_estimate,
                          &unused_summary);

重新获取submap和Node之间的约束(变换），然后进行返回

1
2
3
4
5
6
7
8
9
10
11
// Step:4 获取节点到submap坐标系原点间的坐标变换
// pose_estimate 是 节点在 loacl frame 下的坐标
const transform::Rigid2d constraint_transform =
    ComputeSubmapPose(*submap).inverse() * pose_estimate;
// Step:5 返回计算后的约束
constraint->reset(new Constraint{submap_id,
                                 node_id,
                                 {transform::Embed3D(constraint_transform),
                                  options_.loop_closure_translation_weight(),
                                  options_.loop_closure_rotation_weight()},
                                 Constraint::INTER_SUBMAP});

候选解的生成

生成SearchParameters，根据先验的位姿，在进行进一步匹配

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/**
 * @brief 进行局部搜索窗口的约束计算(对局部子图进行回环检测)
 * 
 * @param[in] initial_pose_estimate 先验位姿
 * @param[in] point_cloud 原点位于local坐标系原点处的点云
 * @param[in] min_score 最小阈值, 低于这个分数会返回失败
 * @param[out] score 匹配后的得分
 * @param[out] pose_estimate 匹配后得到的位姿
 * @return true 匹配成功, 反之匹配失败
 */
 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 {
  // param: linear_search_window angular_search_window 
  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

此点云是在在原点，并且位姿为也为（0，0，0）的点云，因此要对点云先旋转后平移

1
2
3
4
5
6
// Step: 将原点处的点云先旋转到预测的方向上
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())));

为了生成较优候选解，生成按照不同角度旋转后的点云集合

根据SearchParameters 中的 angular_search_window，按照角度为[-angular_search_window, angular_search_window]，角度差为角度分辨率，生成num_scans 个不同角度的旋转点云

1
2
3
// Step: 生成按照不同角度旋转后的点云集合
const std::vector<sensor::PointCloud> rotated_scans =
    GenerateRotatedScans(rotated_point_cloud, search_parameters);

平移，对上面生成的所有的点云都进行平移，并且获取平移后的点在地图中的索引

1
2
3
4
5
6
// Step: 将旋转后的点云集合按照预测出的平移量进行平移, 获取平移后的点在地图中的索引
// 这里的离散激光点是在最细的分辨率的地图上面
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
    limits_, rotated_scans,
    Eigen::Translation2f(initial_pose_estimate.translation().x(),
                         initial_pose_estimate.translation().y()));

现在得到了多个经过平移旋转年之后的点云，注意只是旋转不同，平移距离都一样

减小搜索框

1
2
// 缩小搜索窗口的大小, 计算每一帧点云在保证最后一个点能在地图范围内时的最大移动范围
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());

然后计算最低分辨率的候选解，其流程为

  • 遍历所有的旋转程度不同的点云，把每一个点云搜索框内的x和y都作为一个偏移量，作为一个新点云
  • 对所有的新点云进行遍历打分
  • 按照分数进行排序

vector里面包含的是所有的点云，每一个Candidate2D 都包含这自己的分数

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

基于分支定界算法获得最优解

获得一个Candidate2D

1
2
3
4
// Step: 进行基于分支定界算法的搜索, 获取最优解
const Candidate2D best_candidate = BranchAndBound(
    discrete_scans, search_parameters, lowest_resolution_candidates,
    precomputation_grid_stack_->max_depth(), min_score); // param: max_depth

对计算出的偏移量进行校准

1
2
3
4
5
6
7
8
9
10
11
// 检查最优解的值, 如果大于指定阈值min_score就认为匹配成功,否则认为不匹配返回失败
if (best_candidate.score > min_score) {
  *score = best_candidate.score;
  // Step: 根据计算出的偏移量对位姿进行校准
  *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;
 Comments