(02)Cartographer源码无死角解析-(61) 2D后端优化→

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面两篇博客，已经知道了如何使用 Cartographer 中的线程池，且为为线程池添加任务。在回顾代码之前，这里重复前面的两个疑问点：

$疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
$疑问2:$ 为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。

接着回顾一下 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 中的如下代码：

NodeId PoseGraph2D::AddNode(......, const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps){
    
    
	......
  // 向节点列表加入节点,并得到节点的id
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
  ......
  // 把计算约束的工作放入workitem中等待执行
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    
    
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });
  return node_id;
}

看起来还是比较简单的，就是构建一个工作项，该工作项的内容就是执行 ComputeConstraintsForNode() 函数。然后通过 AddWorkItem 把该工作项添加至 PoseGraph2D::work_queue_。需要注意的是，处理 PoseGraph2D::work_queue_ 队列的线程至多一个，这个线程主要处理关于 PoseGraph2D 的计算问题，如果 PoseGraph2D::work_queue_ 被消耗完之后，该线程会去执行其他的任务(线程并不是结束了)，如果后续 PoseGraph2D::work_queue_ 有了新的数据，又会分配一个线程对齐进行处理，直到 数据处理完。

存储于 PoseGraph2D::work_queue_ 中的工作项包含很多类型，如下：

PoseGraph2D::AddImuData(......)  // 把imu数据加入到优化问题中
PoseGraph2D::AddOdometryData(......) // 把里程计数据加入到优化问题中
PoseGraph2D::AddFixedFramePoseData() // 把gps数据加入到优化问题中
PoseGraph2D::AddLandmarkData() //把landmark数据加入到data_.landmark_nodes中 这个工作项放入到队列中
PoseGraph2D::DeleteTrajectory() //删除轨迹相关的一些工作项
......

以上的函数都会调用到 PoseGraph2D::AddWorkItem() 函数，往队列中添加工作项。

二、前言