子图间约束-cartographer
Kong Liangqian Lv6
 2021-12-07 18:38:29  

之前说到ComputeConstraintsForNode，完成了初始化全局submap的位姿，和子图间内约束。

ComputeConstraintsForNode

在文件WorkItem::Result PoseGraph2D::ComputeConstraintsForNode中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Step: 当前节点与所有已经完成的子图进行约束的计算---实际上就是回环检测
for (const auto& submap_id : finished_submap_ids) {
  // 计算旧的submap和新的节点间的约束
  ComputeConstraint(node_id, submap_id);
}
// Step: 计算所有节点与刚完成子图间的约束---实际上就是回环检测
if (newly_finished_submap) {
  const SubmapId newly_finished_submap_id = submap_ids.front();
  // We have a new completed submap, so we look into adding constraints for
  // old nodes.
  for (const auto& node_id_data : optimization_problem_->node_data()) {
    const NodeId& node_id = node_id_data.id;
    // 刚结束的子图内部的节点, 不再与这个子图进行约束的计算
    if (newly_finished_submap_node_ids.count(node_id) == 0) {
      // 计算新的submap和旧的节点间的约束
      ComputeConstraint(node_id, newly_finished_submap_id);
    }
  }

本文详细说明一下子图间的约束，即点和子图的约束.

ComputeConstraint

计算子图间约束(回环检测)。传入的内容是节点的ID和submap的ID.mapping/internal/2d/pose_graph_2d.cc

1
2
3
4
5
6
7
8
9
10
11
12
/**
 * @brief 进行子图间约束计算, 也可以说成是回环检测
 * 
 * @param[in] node_id 节点的id
 * @param[in] submap_id submap的id
 */
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;

如果地图未完成状态，则立马退出

1
2
3
4
5
6
// 如果是未完成状态的地图不进行约束计算
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;
}

比较此node和submap中最近一次添加的node时间做对比

1
2
// 获取该 node 和该 submap 中的 node 中较新的时间
    const common::Time node_time = GetLatestNodeTime(node_id, submap_id);

如果在建图的过程中，此if一直为true，则设置添加局部约束为true

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// 两个轨迹的最后连接时间
const common::Time last_connection_time =
    data_.trajectory_connectivity_state.LastConnectionTime(
        node_id.trajectory_id, submap_id.trajectory_id);

// 如果节点和子图属于同一轨迹, 或者时间小于阈值
// 则只需进行 局部搜索窗口 的约束计算(对局部子图进行回环检测)
if (node_id.trajectory_id == submap_id.trajectory_id ||
    node_time <
        last_connection_time +
            common::FromSeconds(
                options_.global_constraint_search_after_n_seconds())) {
  // If the node and the submap belong to the same trajectory or if there
  // has been a recent global constraint that ties that node's trajectory to
  // the submap's trajectory, it suffices to do a match constrained to a
  // local search window.

  maybe_add_local_constraint = true;
}

在重定位的过程中，可能if会不成立，则设置添加全局约束

1
2
3
else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
  maybe_add_global_constraint = true;
}

获取节点信息数据与地图数据

1
2
3
constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
    submap = static_cast<const Submap2D*>(
        data_.submap_data.at(submap_id).submap.get());

通过局部搜索进行回环检测，调用MaybeAddConstraint添加约束

1
2
3
4
5
6
7
8
9
10
11
12
13
// 建图时只会执行这块, 通过局部搜索进行回环检测
if (maybe_add_local_constraint) {
  // 计算约束的先验估计值
  // submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点指向节点的坐标变换
  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);
}

定位时才有可能执行这块，调用MaybeAddGlobalConstraint添加约束

1
2
3
4
5
6
// 定位时才有可能执行这块
else if (maybe_add_global_constraint) {
  // 全局搜索窗口 的约束计算 (对整体子图进行回环检测)
  constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,
                                               constant_data);
}

MaybeAddConstraint

此函数添加的是节点和子图的约束, mapping/internal/contraints/contraint_builder_2d.cc

总的来说一共做了一下内容

  • 计算量的过滤
  • 新建匹配器（构成七张地图的匹配器）送入线程池
  • 把计算节点和submap的约束的函数送入线程池，并添加相应的依赖

具体实现

超过范围的不进行约束的计算，此距离可以在配置文件中设置，pose_graph.lua文件中，constraint_builder的max_constraint_distance。对局部子图进行回环检测时能成为约束的最大距离（什么是局部子图）

1
2
3
4
5
max_constraint_distance  // 超过范围的不进行约束的计算
  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance()) { // param: max_constraint_distance
    return;
  }

在队列中新建一个指向Constraint数据的指针

1
2
3
4
// 在队列中新建一个指向Constraint数据的指针
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();

为子图新建一个匹配器，传入的是此submap的栅格地图

1
2
3
4

// 为子图新建一个匹配器
const auto* scan_matcher =
    DispatchScanMatcherConstruction(submap_id, submap->grid());

新建约束任务，进行添加, 注意此ComputeConstraint 非 之前的ComputeConstraint

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

为此任务添加依赖，注意，这是放入线程池因此不会直接运行，而是等待调度

1
2
3
4
5
6
7
// 等匹配器之后初始化才能进行约束的计算
  constraint_task->AddDependency(scan_matcher->creation_task_handle);
  // 将计算约束这个任务放入线程池等待执行
    auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));
  // 将计算约束这个任务 添加到 finish_node_task_的依赖项中
  finish_node_task_->AddDependency(constraint_task_handle);

finishnode_task 在每一次运行到这的时候都会添加一个依赖项，因此他可能会依赖上百个任务，来表示这个NODE到底有没有完成优化task

匹配器的建立

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
// 为每个子图新建一个匹配器
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                     const Grid2D* const grid) {
  CHECK(grid);
  // 如果匹配器里已经存在, 则直接返回对应id的匹配器
  if (submap_scan_matchers_.count(submap_id) != 0) {
    return &submap_scan_matchers_.at(submap_id);
  }
  // submap_scan_matchers_新增加一个 key
  auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
  kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
  // 保存栅格地图的指针
  submap_scan_matcher.grid = grid;

  auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
  auto scan_matcher_task = absl::make_unique<common::Task>();
  // 生成一个将初始化匹配器的任务, 初始化时会计算多分辨率地图, 比较耗时,把他放入线程池的原因是
  // 这里需要生成七张地图，比较费时
  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);
}

下面继续走

ComputeConstraintsForNode

在计算完上面的节点和子图之间的局部和全部约束之后，调用函数constraintbuilder.NotifyEndOfNode();来通知该节点的约束构建完毕

1
2
// 结束构建约束
constraint_builder_.NotifyEndOfNode();

NotifyEndOfNode

  • 添加finishnode_task的任务，因为之前都已经设置好了他的依赖了，所以依赖完成一个他加一个1
  • 添加whendone_task 任务
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// 告诉ConstraintBuilder2D的对象, 刚刚完成了一个节点的约束的计算
void ConstraintBuilder2D::NotifyEndOfNode() {
  absl::MutexLock locker(&mutex_);
  CHECK(finish_node_task_ != nullptr);
  
  // 生成个任务: 将num_finished_nodes_自加, 记录完成约束计算节点的总个数
  finish_node_task_->SetWorkItem([this] {
    absl::MutexLock locker(&mutex_);
    ++num_finished_nodes_;
  });

  // 将这个任务传入线程池中等待执行, 由于之前添加了依赖, 所以finish_node_task_一定会比计算约束更晚完成
  auto finish_node_task_handle =
      thread_pool_->Schedule(std::move(finish_node_task_));

  // move之后finish_node_task_就没有指向的地址了, 所以这里要重新初始化
  finish_node_task_ = absl::make_unique<common::Task>();
  // 设置when_done_task_依赖finish_node_task_handle
  when_done_task_->AddDependency(finish_node_task_handle);
  ++num_started_nodes_;
}
 Comments