cartographer 代码思想解读（15）- ConstraintBuilder2D约束构造器

约束构建器主要是完成创建和维护节点与submap之间的位置关系，其中闭环检测则是由ConstraintBuilder2D完成，即submap与历史节点的约束，或者节点与历史submap之间的约束。

ConstraintBuilder2D成员

    // 约束器配置内容
  const constraints::proto::ConstraintBuilderOptions options_;   
  // 资源池
  common::ThreadPoolInterface* thread_pool_;
  // 线程锁
  absl::Mutex mutex_;

  // 'callback' set by WhenDone().
  std::unique_ptr<std::function<void(const Result&)>> when_done_
      GUARDED_BY(mutex_);

  // 目前添加的节点个数
  int num_started_nodes_ GUARDED_BY(mutex_) = 0;

    // 目前完成约束的节点个数
  int num_finished_nodes_ GUARDED_BY(mutex_) = 0;
  std::unique_ptr<common::Task> finish_node_task_ GUARDED_BY(mutex_);
  std::unique_ptr<common::Task> when_done_task_ GUARDED_BY(mutex_);
  // 约束的双端队列
  std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
  // 每个submap对应的匹配器
  std::map<SubmapId, SubmapScanMatcher> submap_scan_matchers_
      GUARDED_BY(mutex_);

    // 采样器
  common::FixedRatioSampler sampler_;
  // cere优化匹配器
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;

  // Histogram of scan matcher scores.
  // 匹配评分统计
  common::Histogram score_histogram_ GUARDED_BY(mutex_);

其中scan_matching::CeresScanMatcher2D则用于闭环检测的优化匹配器。constraints_则为一个队列，缓存所有的约束关系。sampler_为采样器，基于一定周期才会进行增加约束。

ConstraintBuilder2D构造

//构造函数
ConstraintBuilder2D::ConstraintBuilder2D(
    const constraints::proto::ConstraintBuilderOptions& options,
    common::ThreadPoolInterface* const thread_pool)
    : options_(options),
      thread_pool_(thread_pool),
      finish_node_task_(absl::make_unique<common::Task>()),
      when_done_task_(absl::make_unique<common::Task>()),
      sampler_(options.sampling_ratio()),
      ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}

ConstraintBuilder2D::~ConstraintBuilder2D() {
  absl::MutexLock locker(&mutex_);
  CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
  CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
  CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
  CHECK_EQ(num_started_nodes_, num_finished_nodes_);
  CHECK(when_done_ == nullptr);
}

构造函数较为简单，主要功能是将参数赋值到类中的全局变量中和部分变量的构造。

构建地图匹配器DispatchScanMatcherConstruction

// 构建一个地图匹配器，input 包括id和grid
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                     const Grid2D* const grid)

针对某一个submapid的submap构建一个扫描匹配器，用于约束计算时获取精确的约束。

  // 如果全局匹配器里已经存在，则直接返回对应id的匹配器
  if (submap_scan_matchers_.count(submap_id) != 0) {
    return &submap_scan_matchers_.at(submap_id);
  }

每个submapid都会有一个对应的匹配器，因此计算前可查看是否已经存在，如果存在可直接返回。

 // submap_scan_matchers_新增加一个 key，
 auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
 // 赋值grid
 submap_scan_matcher.grid = grid;
 // 采用快速scan matcher参数
 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);

如果不能存在容器中增加此submapid的匹配器。匹配器中的grid则为该submap的grid；同时采用快速相关匹配方法，详看分支定界快速相关匹配。
最后将扫描匹配器放入到一个线程task后台进行运行，有线程池管理。

添加一个约束MaybeAddConstraint

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)

添加约束接口，包括submapid和对应的submap， nodeid和对应节点数据，同时也包括了一个初始的相对位置。接口对应了约束的意义，即node在submap中的相对位置。

  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance()) {
    return;
  }
  // 采样周期内，无需考虑
  if (!sampler_.Pulse()) return;

约束并非无限制距离，距离若超过一定值时，无需考虑其约束关系，即太远无需考虑。同时两次间隔时间太短也无需考虑。

  // 添加一个约束元素
  constraints_.emplace_back();
  // 设置约束的个数
  kQueueLengthMetric->Set(constraints_.size());
  // 获取新添加约束的地址，用于下面赋值
  auto* const constraint = &constraints_.back();
  // 定义一个scan match
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());
  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);
  });

constraints_是个vector，可先添加一个空元素，然后再对最后一个元素进行赋值，其实为添加一个新元素（cartographer中很多都是如此做法，目的可减省一个变量空间和赋值时间）。scan_matcher是创建的扫描匹配器，其创建过程下面有有详细分析。为提高效率，后端大多都在多线程工作，即创建一个task，用于完成计算出准确的约束，即相对位置。ComputeConstraint则是根据初始相对位置、地图、scan等信息进行一次匹配获取更加精确的约束。

  constraint_task->AddDependency(scan_matcher->creation_task_handle);
  auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));
  finish_node_task_->AddDependency(constraint_task_handle);

由于scan_matcher也为一个线程里完成，因此需要增加其依赖项，才可实现约束计算。

添加全局局部约束MaybeAddGlobalConstraint

与局部约束仅有一点不同，即全局约束无需初始位置，主要在大闭环中进行检测，将遍历整个地图空间进行扫描匹配。因此匹配器采用的全局匹配参数。具体可看下面ComputeConstraint分析。

约束计算ComputeConstraint

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) 

输入十分清楚，即node和submap，初始位置和扫描匹配器，与前端处理基本一致。最后输出约束，即经扫描匹配后的相对位姿。其中match_full_submap表示计算是局部约束还是全局约束。

  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
  // 转换为绝对位置，
  const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;
  float score = 0.;
  // 初始转换矩阵 为单位矩阵
  transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

将初始的相对位置，根据submap的位置计算全局位置，初始化一个单位矩阵为初始估计矩阵，表明初始位置无任何变换。

  // 全局所有submap进行匹配
  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;
    }

计算全局约束，则需要扫描匹配器将对整个地图空间进行扫描遍历，详细可看分支定界快速相关匹配。经过匹配后可获取最佳的估计转移矩阵和匹配置信度。如果置信度低于配置值则认为不可信，即无法找到约束，可直接抛弃。最后记录下全局约束的次数和统计置信度。

    kConstraintsSearchedMetric->Increment();
    // 匹配分数达到未一定值时则直接抛弃，获取优化值pose_estimate
    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 库进行一次优化匹配
  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);

经过快速相关匹配后，再经过ceres优化匹配，得到更加准确的优化位置。

  // 转换回node相对于submap的位置
  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});

最后将计算的位置还原成相对位置，并输出约束结果，显然此种约束类型应为INTER_SUBMAP，即所谓的非原子图类型。

其他辅助的接口

//完成了一次约束的添加进行通知
void ConstraintBuilder2D::NotifyEndOfNode() {
  absl::MutexLock locker(&mutex_);
  CHECK(finish_node_task_ != nullptr);
  // 表明完成添加了一个后端节点
  finish_node_task_->SetWorkItem([this] {
    absl::MutexLock locker(&mutex_);
    ++num_finished_nodes_;
  });
  // 重新开启添加节点任务
  auto finish_node_task_handle =
      thread_pool_->Schedule(std::move(finish_node_task_));
  finish_node_task_ = absl::make_unique<common::Task>();
  when_done_task_->AddDependency(finish_node_task_handle);
  ++num_started_nodes_;
}

每次完成一个node节点约束计算时均会进行调用。目的是完成finish_node_task_，同时开启一个新的task用于下一次约束任务。

WhenDone

// 当闭环结束后调用的回调
void ConstraintBuilder2D::WhenDone(
    const std::function<void(const ConstraintBuilder2D::Result&)>& callback) {
  absl::MutexLock locker(&mutex_);
  CHECK(when_done_ == nullptr);
  // TODO(gaschler): Consider using just std::function, it can also be empty.
  when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
  CHECK(when_done_task_ != nullptr);
  when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); });
  thread_pool_->Schedule(std::move(when_done_task_));
  //新建立一个任务
  when_done_task_ = absl::make_unique<common::Task>();
}

在线程池中，每当完成一次闭环检测时，即增加了约束后会执行WhenDone，即表明完成一次闭环约束计算，其目的是将约束的结果以回调的方式传输出去。

结果回调RunWhenDoneCallback

// 当whendown结束时，的回调函数
void ConstraintBuilder2D::RunWhenDoneCallback() {
  Result result;
  std::unique_ptr<std::function<void(const Result&)>> callback;
  {
    absl::MutexLock locker(&mutex_);
    CHECK(when_done_ != nullptr);
    // 将约束结果结果放入result中
    for (const std::unique_ptr<Constraint>& constraint : constraints_) {
      if (constraint == nullptr) continue;
      result.push_back(*constraint);
    }
    if (options_.log_matches()) {
      LOG(INFO) << constraints_.size() << " computations resulted in "
                << result.size() << " additional constraints.";
      LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10);
    }
    // 清除所有约束
    constraints_.clear();
    callback = std::move(when_done_);
    when_done_.reset();
    kQueueLengthMetric->Set(constraints_.size());
  }
  // 回调结果
  (*callback)(result);
}

总结

约束构建器目的是为了计算节点与submap之间的相对位置，并采用快速相关匹配方法和ceres优化匹配方法，得到更加精确的相对位置。包括两类，从局部进行匹配和全局进行匹配，其目的应该为闭环检测。由于闭环检测可不用实时，因此约束的计算采用线程池进行后台运算，并将结果通过回调进行回传。