(02)Cartographer源码无死角解析-(55) 2D后端优化→ComputeLocalToGlobalTransform()，TrajectoryNode

讲解关于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平移。

$(4)$如果begin_i != end_it，那么就通过形参数global_submap_poses获得子图对应的的位姿。不过需要注意的是，ComputeLocalToGlobalTransform() 函数只接收到 trajectory_id，并没有接收到 submap_index，那么其是如何构建 SubmapId 实例作为一个key进行索引的呢？

$(5)$ 源码中有这样一句代码 const SubmapId last_optimized_submap_id = std::prev(end_it)->id;，其获取到的是 trajectory_id 轨迹中最后一个子图id，也就是 trajectory_id 轨迹最新子图id。从命名来看，last_optimized_submap_id 对应的子图是经过优化之后的。

$(6)$ 前面都是对于没有找到的情况，如果 global_submap_poses 中找到了 last_optimized_submap_id 子图对应的 global 位姿，则直接右乘机器人local位姿，也就是相对于子图的位姿即可。等价于(1)式子 ${\mathbf{Robot}}_{tracking}^{global}={\mathbf{R}}_{local}^{global}\ast {\mathbf{Robot}}_{tracking}^{local}$

$疑问1:$ 。根据last_optimized_submap_id命名，其带有优化字样，last_optimized_submap_id 对应的子图是优化过之后的？但是如何体现呢？
代码注释如下：

// 计算 global frame 指向 local frame 的坐标变换
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
    const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
    const int trajectory_id) const {
    
    
  auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
  auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
  // 没找到这个轨迹id
  if (begin_it == end_it) {
    
    
    const auto it = data_.initial_trajectory_poses.find(trajectory_id);
    // 如果设置了初始位姿
    if (it != data_.initial_trajectory_poses.end()) {
    
    
      return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *
             it->second.relative_pose;
    }
    // note: 没设置初始位姿就将返回(0,0,0)的平移和旋转
    else {
    
    
      return transform::Rigid3d::Identity();
    }
  }

  // 找到了就获取优化后的最后一个子图的id
  const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
  // Accessing 'local_pose' in Submap is okay, since the member is const.
  // 通过最后一个优化后的 global_pose * local_pose().inverse() 获取 global_pose->local_pose的坐标变换
  // tag: 画图说明一下
  return transform::Embed3D(
             global_submap_poses.at(last_optimized_submap_id).global_pose) *
         data_.submap_data.at(last_optimized_submap_id)
             .submap->local_pose()
             .inverse();
}

暂时只需要知道，GetLocalToGlobalTransform() 函数先上锁，然后调用 ComputeLocalToGlobalTransform() ，获得矩阵 ${\mathbf{R}}_{local}^{global}$，如果左乘该矩阵可以把local系下的位姿变换至global系。

三、 GetInterpolatedGlobalTrajectoryPose()

上面的函数中，调用到了 GetInterpolatedGlobalTrajectoryPose() 函数，但是没有对齐进行详细分析，那么现在来看看，首先该函数被调代码如下：

	 const auto it = data_.initial_trajectory_poses.find(trajectory_id);
    // 如果设置了初始位姿
    if (it != data_.initial_trajectory_poses.end()) {
    
    
      return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *
             it->second.relative_pose;
    }

it 是如下类型的一个指针：

  // 相对于to_trajectory_id的初始位姿
  struct InitialTrajectoryPose {
    
    
    int to_trajectory_id;
    transform::Rigid3d relative_pose;
    common::Time time;
  };

其描述的是time时刻相对于to_trajectory_id的轨迹位姿，也就时说该初始位姿为一个相对值。在源代码中搜索一下，可以知道看到如下代码：

/**
 * @brief 设置当前轨迹的起始坐标
 * 
 * @param[in] from_trajectory_id 当前的轨迹id
 * @param[in] to_trajectory_id pose相对于的轨迹的id
 * @param[in] pose 在to_trajectory_id中的坐标
 * @param[in] time 当前的时间
 */
void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
                                           const int to_trajectory_id,
                                           const transform::Rigid3d& pose,
                                           const common::Time time) {
    
    
  absl::MutexLock locker(&mutex_);
  data_.initial_trajectory_poses[from_trajectory_id] =
      InitialTrajectoryPose{
    
    to_trajectory_id, pose, time};
}

该函数的作用就是为一个轨迹设置初始位姿，关于 data_.initial_trajectory_poses 的类型声明如下：

  // Set of all initial trajectory poses.
  std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

本质上initial_trajectory_poses是一个map类型，存储所有轨迹的初始位姿。PoseGraph2D::SetInitialTrajectoryPose() 函数是在 src/cartographer/cartographer/mapping/map_builder.cc 的 MapBuilder::AddTrajectoryBuilder() 函数被调用，注释如下：

  // 如果给了初始位姿
  if (trajectory_options.has_initial_trajectory_pose()) {
    
    
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    
    // 在位姿图中设置初始位姿
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }

has_initial_trajectory_pose 参数源码中被没有配置，所以该处的代码没有被执行，所以就不进行讲解了。有兴趣的朋友可以详细了解一下。似乎绕得比较远了，这里主要讲的是 GetInterpolatedGlobalTrajectoryPose() 函数，先来看其如何使用的？

      return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *
             it->second.relative_pose;

GetInterpolatedGlobalTrajectoryPose() 函数输入一个轨迹id，以及时间戳，返回该 it->second.time 时刻 it->second.to_trajectory_id 对应轨迹的位姿(基于global系)，这里记为 ${\mathbf{Trajectory}}_{Trajectory}^{global}$，根据前面的了解 it->second.relative_pose 是一个相对于轨迹位姿的位姿(相对位姿)，这里记录为 ${\mathbf{InitPose}}_{init}^{Trajectory}$。
$${\mathbf{R}}_{local}^{global}={\mathbf{R}}_{init}^{global}={\mathbf{Trajectory}}_{Trajectory}^{global}\ast {\mathbf{InitPose}}_{init}^{Trajectory}\text{\hspace{1em}(02)}$$
这里的init本质上也是一个local坐标系，这样(2)式与前面的(1)式就联系起来了，下面来看一下 GetInterpolatedGlobalTrajectoryPose() 函数注释：

// 线性插值计算指定时间的global_pose，总体来说，该函数目的就是
// 求解trajectory_id轨迹time时刻的位姿
transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
    const int trajectory_id, const common::Time time) const {
    
    
  //保证轨迹的数目大于0
  CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
  //使用二分查找法，找到第一个不低于time时刻的轨迹节点迭代器
  const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);

  //如果该trajectory_id轨迹第一个节点的time已经比time大了，则直接返回该轨迹节点的全局位姿
  //总的来说，如果time比第一个轨迹节点的时间大，则放回第一个轨迹节点的全局位姿
  if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) {
    
    
    return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)
        ->data.global_pose;
  }

  //time比所有轨迹节点的time都要小，则返回最后一个轨迹节点的全局位姿
  if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) {
    
    
    return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
        ->data.global_pose;
  }

  // 线性插值计算指定时间的global_pose
  // std::prev(it)->data.time() < time < it->data.time()
  // 利用线性插值的方式求解time时刻轨迹的全局位姿
  return transform::Interpolate(
             transform::TimestampedTransform{
    
    std::prev(it)->data.time(),
                                             std::prev(it)->data.global_pose},
             transform::TimestampedTransform{
    
    it->data.time(),
                                             it->data.global_pose},
             time)
      .transform;
}

如果不是很能理解上面的代码，来看下图：

蓝色的曲线表示机器人trajectory_id轨迹，该轨迹上包含了多个节点，也就是上图中的黄色圆形，其代表某个时刻，机器的全局位姿。代码中是使用 TrajectoryNode 进行描述，该结构体如下，该结构体细节后续再进行讲解，其可以看到其包含成员变量 TrajectoryNode::global_pose。

根据上图，假设x轴为time代表时刻，y轴为pose，其表示全局位姿，现在要求time?时刻的全局位姿，源码中的逻辑就是找到节点1与节点2，如下：

//节点1的x,y坐标
std::prev(it)->data.time(),   std::prev(it)->data.global_pose

//节点2的x,y坐标
it->data.time(),   it->data.global_pose

获得节点1与节点2之后，直接利用线性插值即可求解处time?时刻的全局位姿。总的来说，求解一个轨迹某个时间的位姿，是利用轨迹节点 TrajectoryNode::global_pose 结合线性插值完成的，关于轨迹节点的相关内容，后续再进行详细分析。

四、结语

从源码上来看，关于计算Local系坐标到Global系坐标的坐标变换矩阵 ${\mathbf{R}}_{local}^{global}$，调用流程如下：

GetLocalToGlobalTransform(trajectory_id)
	ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,trajectory_id);
		GetInterpolatedGlobalTrajectoryPose()

关于 ComputeLocalToGlobalTransform() 函数的原理，简单的来说，就是把子图坐标系看作local坐标系，先获得其在global系的位姿，然后再结合Robot在local系下的位姿，利用矩阵乘法，获得Robot先对于global系的位姿。另外还留下了一个疑问，那就是:

$疑问1:$ 。根据last_optimized_submap_id命名，其带有优化字样，last_optimized_submap_id 对应的子图是优化过之后的？但是如何体现呢？

另外，还有一个要提及的东西，那就是TrajectoryNode这个结构体，该结构体是比较重要的。上一篇博客中已经进行了部分讲解，也就是成员变量 TrajectoryNode::Data。这里给出全面的注释：

// tag: TrajectoryNode
// 节点在global坐标系下的位姿, 与前端的结果
struct TrajectoryNode {
    
    
  // 前端匹配所用的数据与计算出的local坐标系下的位姿
  struct Data {
    
    
    common::Time time; //点云数据时间戳

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment; //重力校正旋转四元数

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud; //经过滤波之后的点云数据

    // Used for loop closure in 3D.用于3D回环检测，
    sensor::PointCloud high_resolution_point_cloud; //高分辨率点云数据
    sensor::PointCloud low_resolution_point_cloud; //低分辨率点云数据
    Eigen::VectorXf rotational_scan_matcher_histogram; //旋转扫描匹配直方图

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose; //节点相对于local SLAM frame(可以理解为lcoal地图)的位姿，MatchingResult::local_pose是一致的
  };
  
  common::Time time() const {
    
     return constant_data->time; }

  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data; // 不会被后端优化修改的数据, 所以是constant

  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose; // 后端优化后, global_pose会发生改变
};

本质上来说，TrajectoryNode 就两个核型成员变量 constant_data，global_pose。前者存储一帧点云数据扫描匹配过程中，涉及的重力校正矩阵，重力校正之后的点云，高低分辨率点云，局部位姿等，这些都是由第一帧点云数据推断而来，优化过程中是不会对这些数据做改变的，类似于做了一个记录；但是其后者，也就是 global_pose，在后续优化过程中，是会发生改变的。

往一条轨迹中添加节点，是在上一篇博客中提到的 PoseGraph2D::AddNode() 函数中如下代码完成：

  // 向节点列表加入节点,并得到节点的id
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);

至于具体是如何添加的，后面再进行分析。总的来说，每次根据点云数据扫描匹配成功之后，获得的位姿，都可以看作是一个轨迹的节点。