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

一、前言

现在开始，以PoseGraph2D::AddNode()为线索，进行展开，对 PoseGraph2D 中所有函数进行细节分析。关于 PoseGraph2D::AddNode() 的大致流程在上一篇博客中有纤细讲解，其包含了如下代码：

 // local系下的位姿变换到global系下
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

代码功能是将local系下的位姿变换到global系下，很容易看出，其核心部分就是关于 GetLocalToGlobalTransform(trajectory_id) 函数的调用。从命名来看，该函数的作用就是传入一个 trajectory_id，获得其轨迹对应的一个变换矩阵，如果左乘该矩阵，可以将local系下的位姿变换至Global系，这里记该矩阵为 ${\mathbf{R}}_{local}^{global}$，在local系下的位姿 constant_data->local_pose 为 ${\mathbf{Robot}}_{local}^{tracking}$，以及global系下机器人位姿记为 ${\mathbf{Robot}}_{tracking}^{global}$ ，那么等价于上述代码数学公式：
$${\mathbf{Robot}}_{tracking}^{global}={\mathbf{R}}_{local}^{global}\ast {\mathbf{Robot}}_{tracking}^{local}\text{\hspace{1em}(01)}$$

二、ComputeLocalToGlobalTransform

顺着上面的思路，GetLocalToGlobalTransform(trajectory_id) 函数同样在pose_graph_2d.cc中实现，代码如下所示：

// 计算 global frame 指向 local frame 的坐标变换
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
    const int trajectory_id) const {
  // 可能同时间有多个线程调用这同一个函数, 所以要加锁
  absl::MutexLock locker(&mutex_);
  return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
                                       trajectory_id);
}

其代码还是比较简单的，就是上锁然后调用 PoseGraph2D::ComputeLocalToGlobalTransform() 函数，该函数接受两个参数，第一个参数 data_.global_submap_poses_2d 声明如下：

  // Global submap poses currently used for displaying data.
  // submap 在 global 坐标系下的坐标
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

总的来说 global_submap_poses_2d 存储了所有2D子图在global系下的位姿，如果对 MapById 不好理解，暂时把其看作一个map或者说字典，其中key为SubmapId，value是该对应子图global下位姿。总的来说，data_.global_submap_poses_2d 包含了所有子图在 global 系下的位姿。

ComputeLocalToGlobalTransform() 函数接收第二个实参是 trajectory_id，从命名来看，可以知道，该函数主要功能就是计算local系坐标到global系坐标的变变换矩阵 ${\mathbf{R}}_{local}^{global}$，那么其是如何计算的呢？

$(01)$ 形参global_submap_poses中包含了所有子图在global系下的位姿，key为SubmapId类型，其存在成员变量SubmapId::trajectory_id与SubmapId::submap_index。总来说，global_submap_poses 中包含所有轨迹的所有子图位姿。所以通过key获取子图位姿时，需要指定轨迹id，以及子图索引。

$(2)$ 原来中首先根据 trajectory_id 从 global_submap_poses 获得其轨迹所有子图的起始迭代器begin_it，以及末尾迭代器end_it。

$(3)$如果begin_i=end_it，说明该轨迹还没有任何子图，则返回轨迹在global系下的初始位姿，作为子图位姿。需要注意的是，初始位姿可以设定，也可以不设定，所以源码中先调用 data_.initial_trajectory_poses.find(trajectory_id)函数，根据trajectory_id查询初始位姿。如果找到，说明其进行了设置，调用 GetInterpolatedGlobalTrajectoryPose() 函数，使用插值的方式返回初始位姿(x轴为时间轴)。如果没有查询到，则说明没有设置，直接放回单位旋转矩阵及0平移。