沉默杀手HomeArchivesTagsCategories
后端 5 计算约束
|激光SLAMCartographer源码解读|
Word count: 873|Reading time: 3 min

我的理解是，求inter约束也是一个 scan to map 的过程，找到和点云最相似的不同时间的子图，也就是找回环。其实是和lidar_localization的后端用NDT找关键帧的匹配是类似的，但是cartographer是点云和栅格地图匹配，不像点云匹配那样直观，分支定界的score就像NDT匹配的score，不过前者越大越好，后者越小越好。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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)
{
  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
  /*  node在local坐标系的位姿 = 子图在local坐标系中的位姿 * node在子图的位姿*/
  const transform::Rigid2d  initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

  float score = 0.;
  transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

整个函数是为了计算constraint_transform(节点 j 和子图 i的关系) ，需要的参数有：

  • 节点 j 的filtered_gravity_aligned_point_cloud
  • 分支定界的初值 initial_pose (节点 j 在local map坐标系的坐标)
  • Match() 的结果 pose_estimate (节点 j 在local map坐标系的坐标).
  • ComputeSubmapPose()函数 (local map坐标系转到子图 i 坐标系)

计算pose_estimate的三步:

  1. 使用 fast correlative scan matcher 做 Fast estimate
  2. Prune if the score is too low.
  3. ceres Refine

匹配所有子图 或 局部子图

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
// 匹配所有子图，对应 MaybeAddGlobalConstraint
if (match_full_submap)
{
  kGlobalConstraintsSearchedMetric->Increment();
  if( submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
          constant_data->filtered_gravity_aligned_point_cloud,
          // 参数为 global_localization_min_score
          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,
          // 参数为 min_score
          options_.min_score(), 
          &score, &pose_estimate)  )
  {
    CHECK_GT(score, options_.min_score());
    kConstraintsFoundMetric->Increment();
    kConstraintScoresMetric->Observe(score);
  }
  else    return;
}
{
  absl::MutexLock locker(&mutex_);
  score_histogram_.Add(score);
}

分枝定界求出的位姿被称为 初始位姿 3，这个初始位姿3及其携带的点云作为输入，用于ceres与此子图进行优化匹配

在建立全局约束的时候，直接在一个超大的范围内进行分枝定界搜索，并不需要计算一个特殊的初始位姿2，而直接把初始位姿设置为地图limits的中心点，可以理解为map的中心点。 而且打分的参数也不同了。

全局约束的搜索窗口范围： [1e6 * limits_.resolution(), M_PI]，角度其实是±180°

ceres refine

ceres优化匹配，得到更加准确的优化位置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Use the CSM estimate as both the initial and previous pose. 
// This has the effect that, in the absence of better information, 
// we prefer the original  CSM estimate.
ceres::Solver::Summary  unused_summary;
// ceres更新pose_estimate，获得节点在local map中的最优位姿
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                 constant_data->filtered_gravity_aligned_point_cloud,
                *submap_scan_matcher.grid,  &pose_estimate,
                &unused_summary);
// 计算得到node相对子图的位姿
const transform::Rigid2d constraint_transform = 
    ComputeSubmapPose(*submap).inverse() * pose_estimate;
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} );

对于局部约束，constraint_transform并不是回环边，其实就是子图和节点的普通约束。 全局约束才构造回环边

日志

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
if (options_.log_matches() )
{
    std::ostringstream info;
    info << "Node " << node_id << " with "
         << constant_data->filtered_gravity_aligned_point_cloud.size()
         << " points on submap " << submap_id << std::fixed;
    if (match_full_submap)
         info << " matches";
    else
    {
      const transform::Rigid2d difference =
          initial_pose.inverse() * pose_estimate;
      info << " differs by translation " << std::setprecision(2)
           << difference.translation().norm() << " rotation "
           << std::setprecision(3) << std::abs(difference.normalized_angle());
    }
    info << " with score " << std::setprecision(1) << 100. * score << "%.";
    LOG(INFO) << info.str();
}