(02)Cartographer源码无死角解析-(69) 2D后端优化→线程池深入追踪、问题、困惑梳理，知识点小结

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

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

一、前言

通过前面的博客，已经对 FastCorrelativeScanMatcher2D -分支定界算法(BranchAndBound) 有了十分深入的了解。但是依旧有很多的疑问，比如：

$\color{red}疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。

目前为止，并没有得到很好的答案，另外还有上一篇博客最后提到的：

 bool FastCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const float min_score, float* score,
    transform::Rigid2d* pose_estimate)

bool FastCorrelativeScanMatcher2D::MatchFullSubmap(
    const sensor::PointCloud& point_cloud, float min_score, float* score,
    transform::Rigid2d* pose_estimate)

这两个函数区别，被调方式与区别，如何做一个整体的理解。先来梳理一下，这段时间接触过的代码流程梳理一下。

二、流程梳理

首先回到 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 中文件，前端匹配获得位姿节点之后，PoseGraph2D::AddNode() 函数会被调用，这也是我们分析代码的突破口：

NodeId PoseGraph2D::AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data,const int trajectory_id,const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
    // 向节点 PoseGraph2D::data_.trajectory_nodes 与 PoseGraph2D::data_.submap_data 插入节点，且返回节点node_id 
    const NodeId node_id = AppendNode(constant_data, trajectory_id,insertion_submaps, optimized_pose);
    // 为节点建立约束，包含子图内约束及子图间约束
  	AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    
    return ComputeConstraintsForNode(node_id, insertion_submaps,newly_finished_submap);});

可以看到，就这样，就与线程池关联起来，简单的说，就是给出节点数据、与子图数据然派发任务给线程池，让其计算约束。PoseGraph2D::ComputeConstraintsForNode() 函数的调用流程如下：

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( const NodeId& node_id,std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,const bool newly_finished_submap)
	// 该函数再为新子图分配 submap_id 时，还会还会根据子图的全局位姿计算新子图的全局位姿
	// 且新的子图都会添加到 data_.submap_data 之中 与  optimization_problem_ 之中。
	submap_ids = InitializeGlobalSubmapPoses(node_id.trajectory_id, constant_data->time, insertion_submaps);CHECK_EQ(submap_ids.size(), insertion_submaps.size());

	// 把该节点的信息加入到OptimizationProblem中，后面优化需要使用到的
    optimization_problem_->AddTrajectoryNode(matching_id.trajectory_id,optimization::NodeSpec2D{
    
    constant_data->time, local_pose_2d,global_pose_2d, constant_data->gravity_alignment});

     // 把节点阶段与两个活跃子图的约束添加到 data_.constraints这，这里是子图内约束
     data_.constraints.push_back(Constraint{
    
    submap_id,node_id,{
    
    transform::Embed3D(constraint_transform),options_.matcher_translation_weight(),options_.matcher_rotation_weight()},Constraint::INTRA_SUBMAP});

	// 1、当前节点与所有已经完成的子图进行约束的计算---实际上就是回环检测
	// 2、计算所有节点与刚完成子图间的约束---实际上就是回环检测
    ComputeConstraint(node_id, submap_id);
	
	//通知 constraints::ConstraintBuilder2D constraint_builder_; 实例关于节点的约束已经计算完成
    constraint_builder_.NotifyEndOfNode();

至于为什么 ComputeConstraint(node_id, submap_id) 计算子图间实际上就是回环检测，这个在后面再进行详细的讲解。PoseGraph2D::ComputeConstraintsForNode() 返回的结果为 WorkItem::Result::kRunOptimization 或者 WorkItem::Result::kDoNotRunOptimization。当节点数目累计到一定量的时候，就行执行优化。

PoseGraph2D 中添加到线程池的任务都是被 PoseGraph2D::HandleWorkQueue() 取出处理的，关于该函数后面或许还得深入讲解一下，其中调用了了函数 RunOptimization()，其会对初步计算出来得约束进行优化(当节点数目累计到一定量的时候)，另外其还会把计算出来得约束添插入到 PoseGraph2D::data_.constraints 之中。该函数的大致逻辑如下：

// 将计算完的约束结果进行保存, 并执行优化
void PoseGraph2D::HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)

	// Step: 把新计算出的约束信息添加到data_.constraints向量的末尾处
    data_.constraints.insert(data_.constraints.end(), result.begin(),result.end());

	 // Step: 执行优化
	 RunOptimization();

	// Step: 如果已经设置了全局优化的回调函数, 将数据传入回调函数
	if (global_slam_optimization_callback_) {
    
    ......}

    // 根据优化后的约束更新子图轨迹id与节点轨迹id的连接关系
    for (const Constraint& constraint : result) {
    
    UpdateTrajectoryConnectivity(constraint);}


	// 进行子图的裁剪, 如果没有裁剪器就不裁剪
	for (auto& trimmer : trimmers_) {
    
    trimmer->Trim(&trimming_handle); // PureLocalizationTrimmer::Trim()}

	 // Step: 优化执行结束了, 继续处理任务队列中的其他任务
	 DrainWorkQueue();

在博客:
(02)Cartographer源码无死角解析-(59) 2D后端优化→ 线程池: DrainWorkQueue()、AddWorkItem()
中有提到，类 PoseGraph2D 中添加的到线程池的任务，只会被一个线程执行，也就是说关于节点约束相关的计算都是一个线程完成的。因为他们都是把任务添加到 PoseGraph2D:: work_queue_ 这个队列。

到这里为止，可以说把这段时间的内容都回顾了一下，然后再来看几个细节的东西。

三、细节追踪ComputeConstraint()

在 PoseGraph2D::ComputeConstraintsForNode() 函数中，可以找到如下两个代码片段：

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

前者是计算节点与所有已经完成的子图进行约束，后者是子图恰好完成时，计算节点与该子图的约束。传递的参数都是 node_id 与 submap_id。进一步追踪，可以在 PoseGraph2D::ComputeConstraint() 看到如下代码段：

// 建图时会执行, 通过局部搜索进行回环检测
constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data,initial_relative_pose);

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

这两个函数的都用都是求得节点与子图之间的约束，其等价与节点到子图的坐标变换。前者传入一个初始参考位姿，在附近进行局部的搜索。后者不需要传入初始位姿，因为其是全局搜索。前者只在建图模式下会被调用，后则只在纯定位模式下被调用，且都存在有设置调用频率。这两个最函数最终都是调用 ConstraintBuilder2D::ComputeConstraint() ，传递的实参不一样，对比如下：

ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
                  constant_data, initial_relative_pose, *scan_matcher,
                  constraint);

ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
                  constant_data, transform::Rigid2d::Identity(),*scan_matcher,
                  constraint);

可以发现，其只有第四个参数，与第六个参数存在区别，第四个参数表示是否进行全局匹配，false 表示局部匹配，根据前面的分析传入一个初始参考位姿，在附近进行局部的搜索。如果设置为 true，表示全局匹配，则无需传递初始参考位姿，故传递了单位变换 transform::Rigid2d::Identity() 。无论全局匹配还是局部匹配，原理都是一样的，先使用 FastCorrelativeScanMatcher2D 进行节点与子图间的粗匹配，随后再利用 CeresScanMatcher2D 再粗匹配的结果上进行非线性优化。

四、线程细节-PoseGraph2D

src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中，可以看到如下函数：

PoseGraph2D::AppendNode()
PoseGraph2D::AddNode()
PoseGraph2D::AddImuData()
PoseGraph2D::AddOdometryData()
PoseGraph2D::AddFixedFramePoseData()
PoseGraph2D::AddLandmarkData()
PoseGraph2D::AddSubmapFromProto()
PoseGraph2D::AddNodeToSubmap()
PoseGraph2D::AddTrimmer()
PoseGraph2D::RunFinalOptimization()
PoseGraph2D::SetLandmarkPose()
......

这些函数都有一个共同的特点，那就是都有调用 PoseGraph2D::AddWorkItem() 这个函数往 PoseGraph2D::work_queue_ 队列中添加函数。但是消耗这个队列的，只有一个任务，那就是 PoseGraph2D::DrainWorkQueue() 这个函数，但是根据 PoseGraph2D::AddWorkItem() 中如下代码

  if (work_queue_ == nullptr) {
    
    
    // work_queue_的初始化
    work_queue_ = absl::make_unique<WorkQueue>();
    // 将 执行一次DrainWorkQueue()的任务 放入线程池中等待计算
    auto task = absl::make_unique<common::Task>();
    task->SetWorkItem([this]() {
    
     DrainWorkQueue(); });
    thread_pool_->Schedule(std::move(task));
  }

可以知道，只有当 work_queue_ 被消耗完了置为 nullptr，才会添加一个 DrainWorkQueue() 任务到线程池，该任务会再次消耗 work_queue_ 队列。也就是，同一时刻，只有一个线程在消耗 work_queue_ 队列。有下面的代码可以知道：

// 在调用线程上执行工作队列中的待处理任务, 直到队列为空或需要优化时退出循环
void PoseGraph2D::DrainWorkQueue() {
    
    
  bool process_work_queue = true;
  size_t work_queue_size;

  // 循环一直执行, 直到队列为空或需要优化时退出循环
  while (process_work_queue) {
    
    
    std::function<WorkItem::Result()> work_item;
    {
    
    
      absl::MutexLock locker(&work_queue_mutex_);
      // 退出条件1 如果任务队列空了, 就将work_queue_的指针删除
      if (work_queue_->empty()) {
    
    
        work_queue_.reset();
        return;
      }
      // 取出第一个任务
      work_item = work_queue_->front().task;
      // 将取出的任务从任务队列中删掉
      work_queue_->pop_front();
      work_queue_size = work_queue_->size();
      kWorkQueueSizeMetric->Set(work_queue_size);
    }
    // 执行任务
    // 退出条件2 执行任务后的结果是需要优化, process_work_queue为false退出循环
    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  
  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
  // We have to optimize again.
  // 退出循环后, 首先等待计算约束中的任务执行完, 再执行HandleWorkQueue,进行优化
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
    
    
        HandleWorkQueue(result);
      });
}

队列消耗完了，work_queue_ 会被置为 nullptr，直到 PoseGraph2D::AddWorkItem() 函数被调用，发现 work_queue_ == nullptr，则调用 work_queue_ = absl::make_unique<WorkQueue>() 构建一个新的队列。

五、线程细节-PoseGraph2D

src/cartographer/cartographer/mapping/internal/constraints/constraint_builder_2d.cc 文件中搜索 thread_pool_->Schedule，可以看到其在多个地方被调用，如计算量最大的 ConstraintBuilder2D::MaybeAddGlobalConstraint() 与 ConstraintBuilder2D::MaybeAddConstraint() 分别存在如下代码段

  // 生成个计算全局约束的任务
  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));

  =======================================================================================================

  // 生成个计算约束的任务
  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);
  });

也就是说，他们都是直接添加到线程池的，可以由多个线程处理这些任务，那个线程先拿到就那个线程先处理。

六、PoseGraph2D::HandleWorkQueue()

最后我们再回到 PoseGraph2D::HandleWorkQueue() 这个函数，本人再调试的时候发现 global_slam_optimization_callback_ 并没有被设置，虽然如此，我们这里也简单介绍一下其作用，先看代码如下：

  // Step: 如果已经设置了全局优化的回调函数, 将数据传入回调函数
  if (global_slam_optimization_callback_) {
    
    
    std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
    std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
    {
    
    
      absl::MutexLock locker(&mutex_);
      const auto& submap_data = optimization_problem_->submap_data();
      const auto& node_data = optimization_problem_->node_data();
      for (const int trajectory_id : node_data.trajectory_ids()) {
    
    
        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
            submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
    
    
          continue;
        }
        // 计算约束后的最后一个节点与submap的id
        trajectory_id_to_last_optimized_node_id.emplace(
            trajectory_id,
            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
        trajectory_id_to_last_optimized_submap_id.emplace(
            trajectory_id,
            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
      }
    }
    // 将优化后最后一个节点的submap id 与 节点id 传入回调函数进行处理
    global_slam_optimization_callback_(
        trajectory_id_to_last_optimized_submap_id,
        trajectory_id_to_last_optimized_node_id);
  }

总的来说，就是可以通过 PoseGraph2D::SetGlobalSlamOptimizationCallback() 函数设置一个回调函数，注意该回调函数需要接收如下两个类型的参数：

const std::map<int /* trajectory_id */, SubmapId>&,
const std::map<int /* trajectory_id */, NodeId>&

不过上面并非把地图中所有子图还节点都传入了，其只传递了当前轨迹优化后最后一个节点与子图。通过自定义的回调函数，可以再进一步对优化后的数据进行处理。不过建图过程中并没ou有设置回调函数，所以没有做任何的后处理。

随后调用了 PoseGraph2D::UpdateTrajectoryConnectivity() 函数，该函数代码如下：

// 根据新计算出的约束更新子图轨迹id与节点轨迹id的连接关系
void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
    
    
  CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
  const common::Time time =
      GetLatestNodeTime(constraint.node_id, constraint.submap_id);
  data_.trajectory_connectivity_state.Connect(
      constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id,
      time);
}

其作用是根据计算出来的约束更新子图轨迹id与节点轨迹id的连接关系，且记录更新时对应的时间。身下还有如下代码：

   // 根据轨迹状态删除轨迹
    DeleteTrajectoriesIfNeeded();
    
    TrimmingHandle trimming_handle(this);
    // 进行子图的裁剪, 如果没有裁剪器就不裁剪
    for (auto& trimmer : trimmers_) {
    
    
      trimmer->Trim(&trimming_handle); // PureLocalizationTrimmer::Trim()
    }
    // 如果裁剪器处于完成状态, 就把裁剪器删除掉
    trimmers_.erase(
        // c++11: std::remove_if 如果回调函数函数返回真,则将当前所指向的参数移到尾部,返回值是被移动区域的首个元素
        std::remove_if(trimmers_.begin(), trimmers_.end(),
                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
    
    
                         return trimmer->IsFinished(); // 调用PureLocalizationTrimmer::IsFinished()
                       }),
        trimmers_.end());

    // 把这个变量置为0
    num_nodes_since_last_loop_closure_ = 0;

    // Update the gauges that count the current number of constraints.
    // 计算相同轨迹下与不同轨迹下 inter_constraints 的数量, 放入Metric中
    double inter_constraints_same_trajectory = 0;
    double inter_constraints_different_trajectory = 0;
    for (const auto& constraint : data_.constraints) {
    
    
      if (constraint.tag ==
          cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
    
    
        continue;
      }
      if (constraint.node_id.trajectory_id ==
          constraint.submap_id.trajectory_id) {
    
    
        ++inter_constraints_same_trajectory;
      } else {
    
    
        ++inter_constraints_different_trajectory;
      }
    }
    kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
    kConstraintsDifferentTrajectoryMetric->Set(
        inter_constraints_different_trajectory);

这些代码后面还会涉及到，到时候再进行详细的分析。

七、结语

接下来就是对 PoseGraph2D::HandleWorkQueue() 中调用的 PoseGraph2D::RunOptimization() 函数进行分析了，看看其在计算出子图间与子图内约束之后，是如何进行优化的。不过这里需要注意，不要把下面的疑问给忘记了：

$\color{red}疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。