沉默杀手
后端 4 计算约束的准备工作
|激光SLAMCartographer源码解读|
Word count: 1.2k|Reading time: 5 min

接上一篇，看两次计算约束的函数 ComputeConstraint

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
// 根据指定的 nodeid 和 submapid 计算其约束
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                    const SubmapId& submap_id)
{
  bool  maybe_add_local_constraint = false;
  bool  maybe_add_global_constraint = false;
  const  TrajectoryNode::Data*  constant_data;
  const  Submap2D* submap;
  {
    absl::MutexLock locker(&mutex_);
    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
    if (!data_.submap_data.at(submap_id).submap->insertion_finished() )
    {
      // Uplink server only receives grids when they are finished,
      // so skip constraint search before that.
      return;
    }
    // 获取两个id最新的那个时刻
    const common::Time  node_time = GetLatestNodeTime(node_id, submap_id);
    const common::Time  last_connection_time =
        data_.trajectory_connectivity_state.LastConnectionTime(
            node_id.trajectory_id,  submap_id.trajectory_id );
    // 如果节点与submap在同一轨迹内或者距离上次全局约束时间较短，则计算局部约束
    if (node_id.trajectory_id == submap_id.trajectory_id ||
        node_time <
            last_connection_time +
                common::FromSeconds(
                    options_.global_constraint_search_after_n_seconds()) )
    {
        maybe_add_local_constraint = true;
    }
    // 如果不在同一轨迹内，一定间隔计算全局约束
    else if (global_localization_samplers_[node_id.trajectory_id]->Pulse() )
    {
        maybe_add_global_constraint = true;
    }
    constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
    submap = static_cast<const Submap2D*>(
        data_.submap_data.at(submap_id).submap.get()  );
  }

  • 建图模式，节点与submap在同一轨迹内 或者 存在一个最近的全局约束把节点的轨迹和子图的轨迹连接起来时，使用local search window计算局部约束；

  • 纯定位模式，节点与submap不在同一轨迹，使用全局搜索窗口计算约束(对整体子图进行回环检测) 。纯定位进行慢，主要就是 global_constraint_search_after_n_seconds 较大导致，迟迟不能确认maybe_add_global_constraint为true

local约束在求解时，搜索窗口小，有初值;    global约束在求解时，搜索窗口大，没有初值。 记住二者都是计算 Constraint::INTER_SUBMAP

准备计算局部和全局约束

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
if (maybe_add_local_constraint)
{
  const transform::Rigid2d  initial_relative_pose =
      optimization_problem_->submap_data()
          .at(submap_id)
          .global_pose.inverse() *
      optimization_problem_->node_data().at(node_id).global_pose_2d;
  // 添加局部约束，进行闭环匹配，更新相对位置
  constraint_builder_.MaybeAddConstraint(
      submap_id, submap, node_id, constant_data, initial_relative_pose);
}
  // 添加全局约束
else if (maybe_add_global_constraint)
  constraint_builder_.MaybeAddGlobalConstraint(
      submap_id, submap, node_id, constant_data);

前端得到节点相对于世界的位姿，也可以得到某个子图的世界位姿，因此得到这个节点相对于这个子图的相对位姿，把这个位姿称为 初始位姿 1。 之所以要用世界坐标系作为桥梁，是因为子图和这个节点并不一定在在同一条轨迹坐标系中(local map坐标系)

这里主要是ConstraintBuilder2D类的两个函数: MaybeAddConstraint 和 MaybeAddGlobalConstraint，它们只有细微不同，前者的开头有这样两句：

1
2
3
if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance()  )
    return;

然后就是其中调用的ConstraintBuilder2D::ComputeConstraint不同，局部约束的是 ComputeConstraint(submap_id, submap, node_id, false, constant_data, initial_relative_pose, *scan_matcher, constraint);

全局约束的是 ComputeConstraint( submap_id, submap, node_id, true, constant_data, transform::Rigid2d::Identity(), *scan_matcher, constraint);，也就是 match full submap

局部约束

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
void ConstraintBuilder2D::MaybeAddConstraint(
    const SubmapId& submap_id,  const Submap2D* const submap,
    const NodeId& node_id, 
    const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose)
{
  // 参数  max_constraint_distance
  // 初始位姿 1 的模不能太大
  // 约束并非无限制距离，若子图和节点的距离太远，则无需考虑约束
  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance() )
        return;
  /* 这是 ConstraintBuilder2D类 唯一使用采样器的地方
  ratio 默认 0.3， 参数 sampling_ratio
  sampling_ratio越小， Pulse()返回的false越多，更容易return*/
  if (!sampler_.Pulse()) return;

  absl::MutexLock locker(&mutex_);
  // std::unique_ptr<std::function<void(const Result&)>>  when_done_;
  if (when_done_)
  {
    LOG(WARNING)
        << "MaybeAddConstraint was called while WhenDone was scheduled.";
  }
  constraints_.emplace_back();  // 添加空元素
  kQueueLengthMetric->Set(constraints_.size()  );
  // 指针指向，对最后一个元素赋值
  auto* const constraint = &constraints_.back();
  // fast time scan matcher 在这里
  const auto* scan_matcher = 
      DispatchScanMatcherConstruction(submap_id, submap->grid()  );
}

参数max_constraint_distance很重要，如果建图回到同一位置，但没有出现回环，可能是因为过程中的累计误差过大了，大于这个参数，导致没有求 inter 约束。

DispatchScanMatcherConstruction

针对某一个submap_id的submap构建一个扫描匹配器,先看返回类型

1
2
3
4
5
6
7
8
struct SubmapScanMatcher
{
  const Grid2D* grid = nullptr;
  std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
      fast_correlative_scan_matcher;
  // 线程池用的 Task
  std::weak_ptr<common::Task>  creation_task_handle;
};

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
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(
      const SubmapId& submap_id,  const Grid2D* const grid)
{
  CHECK(grid);
  if (submap_scan_matchers_.count(submap_id) != 0)
      return &submap_scan_matchers_.at(submap_id);

  // Map of dispatched or constructed scan matchers by 'submap_id'
  // std::map<SubmapId, SubmapScanMatcher>  submap_scan_matchers_
  auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
  // 下面都是成员赋值
  submap_scan_matcher.grid = grid;
  auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();

  auto scan_matcher_task = absl::make_unique<common::Task>();
  // 这里也是 work_item 机制
  scan_matcher_task->SetWorkItem(
      [&submap_scan_matcher, &scan_matcher_options]() {
        submap_scan_matcher.fast_correlative_scan_matcher =
            absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
                *submap_scan_matcher.grid, scan_matcher_options);
      } );

  submap_scan_matcher.creation_task_handle = 
      thread_pool_->Schedule(std::move(scan_matcher_task));
  return &submap_scan_matchers_.at(submap_id);
}

这里就是构造了scan_matcher_task的 work_item，返回 scan_matcher。

继续看MaybeAddConstraint

1
2
3
4
5
6
7
8
9
10
11
12
13
14
 // 线程池
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_)
{
  ComputeConstraint(submap_id, submap, node_id, false, /* match */
                    constant_data, initial_relative_pose, *scan_matcher,
                    constraint);
});
// constraint_task 依赖 scan_matcher_task
constraint_task->AddDependency(scan_matcher->creation_task_handle);
auto constraint_task_handle =
    thread_pool_->Schedule(std::move(constraint_task));
// finish_node_task_ 依赖 constraint_task
finish_node_task_->AddDependency(constraint_task_handle);

其实MaybeAddConstraint做的就是下面的工作，接下来的重点就是 ConstraintBuilder2D::ComputeConstraint