(02)Cartographer源码无死角解析-(64) 2D后端优化→ConstraintBuilder2D、MaybeAddConstraint、MaybeAddGlobalConstraint

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

在上一篇博客中，主要对 PoseGraph2D::ComputeConstraint() 函数进行了分析。其主要是根据 maybe_add_local_constraint 与 maybe_add_global_constraint 参数，分别调用了如下代码：

    // 进行局部搜索窗口 的约束计算 (对局部子图进行回环检测)
    constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);

    // 全局搜索窗口 的约束计算 (对整体子图进行回环检测)
    constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,constant_data);

这两个函数都想实现于 src/cartographer/cartographer/mapping/internal/constraints/constraint_builder_2d.cc 文件中。且在上一篇博客的末尾部分对 ConstraintBuilder2D 实例的创建过程进行了解读。下面就来揭露一下 ConstraintBuilder2D 的真面目，在这之前，先简单的看一下 constraint_builder_2d.h 文件，其内容一眼看过去还是比较杂的，可以找到如下代码以及对应的英文注释：

  // Schedules exploring a new constraint between 'submap' identified by
  // 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
  // 'initial_relative_pose' is relative to the 'submap'.
  //
  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
  // all computations are finished.
  void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
                          const NodeId& node_id,
                          const TrajectoryNode::Data* const constant_data,
                          const transform::Rigid2d& initial_relative_pose);

  // Schedules exploring a new constraint between 'submap' identified by
  // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
  // This performs full-submap matching.
  //
  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
  // all computations are finished.
  void MaybeAddGlobalConstraint(
      const SubmapId& submap_id, const Submap2D* submap, const NodeId& node_id,
      const TrajectoryNode::Data* const constant_data);

大致的意思就是说，调用这两个函数的时候，需要保证其执行期间 submap 与 constant_data 这两个指针指向内容的有效性。其主要功能是建立子图 submap 与节点数据 constant_data 之间的约束。

二、MaybeAddConstraint()

$\color{blue} （1）：$ 这里以 MaybeAddConstraint() 为突破口进行讲解，首先来看该函数的输入：

/**
 * @brief 进行局部搜索窗口的约束计算(对局部子图进行回环检测)
 * 
 * @param[in] submap_id submap的id
 * @param[in] submap 单个submap
 * @param[in] node_id 节点的id
 * @param[in] constant_data 节点的数据
 * @param[in] initial_relative_pose 约束的初值
 */
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) {
    
    
	......
	......
}

看起来没有太多需要解释的地方，只需要注意一下 initial_relative_pose 表示节点 node 在子图 submap 中的位姿即可。

$\color{blue} （2）：$ 接着可以看到如下代码：

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

原理还是很简单的，判断一下节点距到子图原点距离是否超过 options_.max_constraint_distance()，超过则不计算约束。这里要提及一下，根据前面的分析 ComputeConstraint() 是计算子图间约束，所以 node_id 对应的节点是不被 submap 包含的，且此时的 submap 肯定为完成状态。

$\color{blue} （3）：$ 顺着往下分析，代码段如下：

  absl::MutexLock locker(&mutex_);
  // 当when_done_正在处理任务时调用本函数, 报个警告
  if (when_done_) {
    
    
    LOG(WARNING)
        << "MaybeAddConstraint was called while WhenDone was scheduled.";
  }

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

其先判断一下 when_done_ 是否被设置，如果被设置说明 when_done_ 已经被安排任务或者处理任务了，则打印一个警告信息。随后，其往 constraints_ 中放置了一个空的约束，且获得在这个约束的引用。

$\color{blue} （4）：$ 下面执行了两端十分重要的代码如下所示：

  // 为子图新建一个匹配器
  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);
  });

其会为子图创建扫描匹配器 scan_matcher，DispatchScanMatcherConstruction() 函数中存在条件限制，同一子图并不会重复创建，该函数的具体细节后面再进行分析，获得子图对应的 scan_matcher 之后，则会往线程池中添加一个计算约束的任务，也就是调用了 ConstraintBuilder2D::ComputeConstraint() 函数，注意，该函数与 PoseGraph2D::ComputeConstraint() 重名了，前者是后者的核心实现。具体细节同样后面再进行分析，

$\color{blue} （5）：$ 剩下的部分代码就比较简单了：

  // 等匹配器之后初始化才能进行约束的计算
  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);

其会为计算约束的任务添加一个依赖任务，scan_matcher->creation_task_handle，该任务的主要操作就是初始化 FastCorrelativeScanMatcher2D。具体细节后面分析，这里先放一下。同样，constraint_task_handle 也会被当作依赖传递添加到 finish_node_task_ 任务之中。那么，finish_node_task_ 又是什么呢？先看 MaybeAddGlobalConstraint() 函数，接着来分析他，因为其中也使用到 finish_node_task_ 。

三、MaybeAddGlobalConstraint()

/**
 * @brief 进行全局搜索窗口的约束计算(对整体子图进行回环检测)
 * 
 * @param[in] submap_id submap的id
 * @param[in] submap 单个submap
 * @param[in] node_id 节点的id
 * @param[in] constant_data 节点的数据
 */
void ConstraintBuilder2D::MaybeAddGlobalConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data) {
    
    
  absl::MutexLock locker(&mutex_);
  if (when_done_) {
    
    
    LOG(WARNING)
        << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
  }
  
  // note: 对整体子图进行回环检测时没有距离的限制

  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());
  auto* const constraint = &constraints_.back();
  // 为子图新建一个匹配器
  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, true, /* match_full_submap */
                      constant_data, transform::Rigid2d::Identity(),
                      *scan_matcher, constraint);
  });
  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);
}

可以看到该函数与 MaybeAddConstraint 基本差不多的，只不过因为其是计算全局的约束，少了一个距离判断，因为无论节点距离子图多元，都会计算他们之间的约束。

四、NotifyEndOfNode()

该函数在前面的博客分析中，其实已经被调用过了PoseGraph2D::ComputeConstraintsForNode() 函数中，在添加完所有计算约束(子图间或子图内)之后，有执行如下代码：

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

其主要目的就是告知 constraint_builder_，关于 node_id 节点的约束已经计算完成，可以进行下面的操作了。 ConstraintBuilder2D::NotifyEndOfNode() 的整体注释如下：

// 告诉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_;
}

首先为 finish_node_task_ 设置一个工作项，对 num_finished_nodes_ 进行 ++ 操作，主要同于记录目前完成约束结算的节点数目。然后把该任务进行分发，也就是添加到线程池之中，且把其作为一个依赖项添加到 when_done_task_ 之中。最后 num_started_nodes_ 执行 ++ 操作，.h 中该变量的注释如：

  // TODO(gaschler): Use atomics instead of mutex to access these counters.
  // Number of the node in reaction to which computations are currently
  // added. This is always the number of nodes seen so far, even when older
  // nodes are matched against a new submap.
  int num_started_nodes_ GUARDED_BY(mutex_) = 0;

该变量与 num_finished_nodes_ 有点相近，只是 num_finished_nodes_ 记录的是目前完成约束计算的节点数量，而 num_started_nodes_ 记录的是目前已经进行约束计算任务分配的节点数。

五、WhenDone()

顺着上面的逻辑下来，就是 when_done_task_ 这个任务是何时被添加分发至线程池，这里同样需要回顾一下前面的知识点，还记得 PoseGraph2D::DrainWorkQueue() 函数吗？其会持续处理 PoseGraph2D::work_queue_ 这个队列，直到处理完其其中的所有任务，最后调用了如下代码：

  // We have to optimize again.
  // 退出循环后, 首先等待计算约束中的任务执行完, 再执行HandleWorkQueue,进行优化
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
    
    
        HandleWorkQueue(result);
      });

其调用 ConstraintBuilder2D::WhenDone() 时传递了一个函数，该函数的内容就是执行 PoseGraph2D::HandleWorkQueue() 函数。该函数前面已有初步介绍，且其中会反过来调用 PoseGraph2D::DrainWorkQueue() ，这样共同形成了一个循环。关于 ConstraintBuilder2D::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_
  when_done_ = absl::make_unique<std::function<void(const Result&)>>(callback);
  CHECK(when_done_task_ != nullptr);

  // 生成 执行when_done_的任务
  when_done_task_->SetWorkItem([this] {
    
     RunWhenDoneCallback(); });
  // 将任务放入线程池中等待执行
  thread_pool_->Schedule(std::move(when_done_task_));

  // when_done_task_的重新初始化
  when_done_task_ = absl::make_unique<common::Task>();
}

该代码其实很简单的，本质上就是把 PoseGraph2D::DrainWorkQueue() 作为一个回调函数赋值给 ConstraintBuilder2D::when_done_，其会在执行 when_done_task_ 任任务，也就是 ConstraintBuilder2D::RunWhenDoneCallback() 中调用。需要注意 when_done_task_ 在前面时有添加依赖项的，需要先执行 finish_node_task_handle。

六、RunWhenDoneCallback()

// 将临时保存的所有约束数据传入回调函数, 并执行回调函数
void ConstraintBuilder2D::RunWhenDoneCallback() {
    
    
  Result result;
  std::unique_ptr<std::function<void(const Result&)>> callback;
  {
    
    
    absl::MutexLock locker(&mutex_);
    CHECK(when_done_ != nullptr);

    // 将计算完的约束进行保存
    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());
  }
  // 执行回调函数 HandleWorkQueue
  (*callback)(result);
}

该函数主要任务就是把计算完的约束保存到 Result result，且在调用回调函数 PoseGraph2D::HandleWorkQueue() 时作为形参传递。当然，还完成了一些置位操作。

六、总结

到目前为止，关于 ConstraintBuilder2D 的大部分成员函数与成员变量都进行了分析，不过还有两个重要的函数如下：

// 为每个子图新建一个匹配器
const ConstraintBuilder2D::SubmapScanMatcher* ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, const Grid2D* const grid) 

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)

这两个函数都是在 ConstraintBuilder2D::MaybeAddConstraint() 或 ConstraintBuilder2D::MaybeAddGlobalConstraint() 函数中被调用的，下篇博客会进行详细讲解。

这里需要提及到的一点是，完成所有约束计算后，回调 PoseGraph2D::HandleWorkQueue() 函数时，其中有调用 PoseGraph2D::RunOptimization() 函数，这是一个比较重要的函数，后续我们会进行重点讲解，因为其就是对计算出来的约束进行优化。