(02)Cartographer源码无死角解析-(62) 2D后端优化→InitializeGlobalSubmapPoses()子图全局位姿的来龙去脉

江南才尽，年少无知！

已于 2023-06-29 22:24:52 修改

阅读量337

点赞数

分类专栏： # (02)Cartographer源码无死角解析-免费文章标签：机器人自动驾驶 Cartographer 增强现实 slam

于 2023-06-28 20:30:17 首次发布

本文链接：https://blog.csdn.net/weixin_43013761/article/details/131441945

版权

(02)Cartographer源码无死角解析-免费专栏收录该内容

61 篇文章 139 订阅

订阅专栏

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

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

一、前言

首先这里重复前面的四个个疑问点：

$\color{red}疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
$\color{red}疑问2:$ 为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。
$\color{red}疑问3:$ data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？
$\color{red}疑问4:$ ComputeConstraintsForNode() 如果返回需要优化，源码中是在哪里执行优化的呢？

该篇博客主要是对 PoseGraph2D::ComputeConstraintsForNode() 函数中调用的 InitializeGlobalSubmapPoses() 进行讲解，其同样实现于 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中。首先回顾一下调用该函数的过程：

    // 获取节点信息数据
    const auto& constant_data =
        data_.trajectory_nodes.at(node_id).constant_data;
    
    // 获取 trajectory_id 下的正处于活跃状态下的子图的SubmapId
    submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);
    CHECK_EQ(submap_ids.size(), insertion_submaps.size());

其先获得节点的静态数据 constant_data (主要由前端计算而来)，然后利用其t成员变量 constant_data->time 与其对应的 node_id.trajectory_id 及活跃的子图 insertion_submaps 作为形参传入。

二、整体注释

在进行细节分析之前，各位朋友可以简单的过一下整体注释(后面有十分详细的讲解)：

// 返回指定轨迹id下的正处于活跃状态下的子图的SubmapId
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  CHECK(!insertion_submaps.empty());

  // submap_data中存的: key 为 SubmapId, values 为对应id的Submap在global坐标系下的全局位姿
  const auto& submap_data = optimization_problem_->submap_data();
  
  // 只有slam刚启动时子图的个数才为1
  if (insertion_submaps.size() == 1) {
    // If we don't already have an entry for the first submap, add one.
    // 如果判断指定id的submap_data的size为0, 这条轨迹上还没有添加submap的pose
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      // 如果没设置初始位姿就是0, 设置了就是1
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        // 把该trajectory_id与其初始位姿的基准轨迹的id关联起来
        data_.trajectory_connectivity_state.Connect(
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
      // 将该submap的global pose加入到optimization_problem_中
      optimization_problem_->AddSubmap(
          trajectory_id, transform::Project2D(
                             ComputeLocalToGlobalTransform(
                                 data_.global_submap_poses_2d, trajectory_id) *
                             insertion_submaps[0]->local_pose()));
    }
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

    // 因为是第一个submap, 所以该submap的ID是(trajectory_id,0), 其中0是submap的index, 从0开始
    const SubmapId submap_id{trajectory_id, 0};
    // 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素.因为我们初始化第一个submap肯定是要被插入的那个submap
    CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
    // 因为是第一个submap, 那就把刚刚建立的submap的id返回
    return {submap_id};
  }

  CHECK_EQ(2, insertion_submaps.size());

  // 获取 submap_data 的末尾 trajectory_id
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);

  // end_it是最后一个元素的下一个位置, 所以它之前的一个submap的id就是submap_data中的最后一个元素
  // 注意, 这里的last_submap_id 是 optimization_problem_->submap_data() 中的
  const SubmapId last_submap_id = std::prev(end_it)->id;

  // 如果是等于第一个子图, 说明insertion_submaps的第二个子图还没有加入到optimization_problem_中
  // 拿着optimization_problem_中子图的索引, 根据这个索引在data_.submap_data中获取地图的指针
  if (data_.submap_data.at(last_submap_id).submap ==
      insertion_submaps.front()) {
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
    
    // 这种情况下, 要给新的submap分配id, 并把它加到OptimizationProblem的submap_data_这个容器中
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    // 解算新的submap的global pose, 插入到OptimizationProblem2D::submap_data_中
    optimization_problem_->AddSubmap(
        trajectory_id,
        // first_submap_pose * constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() = globla指向local的坐标变换
        // globla指向local的坐标变换 * 第二个子图原点在local下的坐标 = 第二个子图原点在global下的坐标
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }

  // 如果是等于第二个子图, 说明第二个子图已经分配了id, 已经在OptimizationProblem的submap_data_中了
  CHECK(data_.submap_data.at(last_submap_id).submap ==
        insertion_submaps.back());
  // 那么第一个子图的index就是last_submap_id.submap_index的前一个, 所以要-1
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK(data_.submap_data.at(front_submap_id).submap ==
        insertion_submaps.front());
  return {front_submap_id, last_submap_id};
}

三、函数输入

前面已经提到了，该函数接收一个 trajectory_id，节点数据 constant_data 的时间 time，以及目前两个活跃的子图 insertion_submaps。接收到参数之后，首先执行了如下代码：

  // submap_data中存的: key 为 SubmapId, values 为对应id的Submap在global坐标系下的全局位姿
  const auto& submap_data = optimization_problem_->submap_data();

其首先获取 optimization_problem_ 中的所有子图数据，赋值给 submap_data。

四、初始处理

如果此时系统刚启动，那么此时 insertion_submaps 中只存储了一个子图，也就符合条件 insertion_submaps.size() == 1。此时会做那些处理呢？源码如下所示：

  // 只有slam刚启动时子图的个数才为1
  if (insertion_submaps.size() == 1) {
    // If we don't already have an entry for the first submap, add one.
    // 如果判断指定id的submap_data的size为0, 这条轨迹上还没有添加submap的pose
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
      // 如果没设置初始位姿就是0, 设置了就是1
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
        // 把该trajectory_id与其初始位姿的基准轨迹的id关联起来
        data_.trajectory_connectivity_state.Connect(
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
      // 将该submap的global pose加入到optimization_problem_中
      optimization_problem_->AddSubmap(
          trajectory_id, transform::Project2D(
                             ComputeLocalToGlobalTransform(
                                 data_.global_submap_poses_2d, trajectory_id) *
                             insertion_submaps[0]->local_pose()));
    }
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

    // 因为是第一个submap, 所以该submap的ID是(trajectory_id,0), 其中0是submap的index, 从0开始
    const SubmapId submap_id{trajectory_id, 0};
    // 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素.因为我们初始化第一个submap肯定是要被插入的那个submap
    CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
    // 因为是第一个submap, 那就把刚刚建立的submap的id返回
    return {submap_id};
  }

其首先判断一下当前这个活跃的子图是否被添加到 submap_data 之中，如果没有添加，则调用 optimization_problem_->AddSubmap() 函数添加至 optimization_problem_ 之中。当然，在这之前首先会判断一下是否有为该轨迹设置初始位姿，如果设置了会调用 data_.trajectory_connectivity_state.Connect 把 trajectory_id 与其初始位姿的基准轨迹的id关联起来。简单的说，就是如果之前已经存在一条轨迹 t 了，可以同通过配置文件中的 initial_trajectory_pose 参数，为 trajectory_id 这条轨迹设置一个相对于轨迹 t 的基准位姿作为初始位置。

需要注意的是，在为 optimization_problem_ 添加子图时，其调用了 ComputeLocalToGlobalTransform 函数计算子图的全局位姿。该函数前面在前面的博客 (02)Cartographer源码无死角解析-(55) 2D后端优化→ComputeLocalToGlobalTransform()，TrajectoryNode 中有进行讲解，先放一下，稍后我们回过来再分析一下。

因为这是第一个子图，所以 trajectory_id 轨迹对应的子图只能存在一个，所以执行

CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));

这段代码判断一下，确保逻辑上的正确性。接着为该子图创建一个 SubmapId 对象，且子图的序列好为 0，表示 trajectory_id 轨迹上的第一个子图。然后以列表的形式返回这个 SubmapId 对象 {submap_id}。

四、添加第二个活跃的子图

完成初始处理之后，后续的都是常规处理了。除了建图开始阶段只有一个活跃的子图，后续都存在两个活跃的子图，地图更新或者插入点云时，都是同时往这两个活跃的子图中插入的。所以源码中执行了 CHECK_EQ(2, insertion_submaps.size())，同理是为了确保逻辑的正确性。接着可以看到如下代码：

  // 获取 submap_data 的末尾 trajectory_id
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);

  // end_it是最后一个元素的下一个位置, 所以它之前的一个submap的id就是submap_data中的最后一个元素
  // 注意, 这里的last_submap_id 是 optimization_problem_->submap_data() 中的
  const SubmapId last_submap_id = std::prev(end_it)->id;

第一步是确保 submap_data 中轨迹 trajectory_id 对应的子图数不为0，另外获得最后一个子图的 SubmapId。随后，运行了如下这段代码：

  // 如果是等于第一个子图, 说明insertion_submaps的第二个子图还没有加入到optimization_problem_中
  // 拿着optimization_problem_中子图的索引, 根据这个索引在data_.submap_data中获取地图的指针
  if (data_.submap_data.at(last_submap_id).submap ==
      insertion_submaps.front()) {
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
    
    // 这种情况下, 要给新的submap分配id, 并把它加到OptimizationProblem的submap_data_这个容器中
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    // 解算新的submap的global pose, 插入到OptimizationProblem2D::submap_data_中
    optimization_problem_->AddSubmap(
        trajectory_id,
        // first_submap_pose * constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() = globla指向local的坐标变换
        // globla指向local的坐标变换 * 第二个子图原点在local下的坐标 = 第二个子图原点在global下的坐标
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }

其先判断一下 trajectory_id 对应的最后子图 last_submap_id，其是否为活跃子图的第一个子图，如果是，则说明第二个活跃的子图还没有添加到 optimization_problem_ 之中，接下来的操作不用多说，也知道就是调用 optimization_problem_->AddSubmap() 函数进行添加。first_submap_pose 活跃的第一个子图(第二个此时还没有添加)的 global 位姿，这里我们记为 $\mathbf {Submap1}^{global}_{pose}$ ，constraints::ComputeSubmapPose(*insertion_submaps[0]) 是第一个活跃子图的第 local 位姿，这里我们记为 $\mathbf {Submap1}^{local}_{pose}$ ，constraints::ComputeSubmapPose(*insertion_submaps[1]) 当然表示第二个子图的 local 位姿，同理记为 $\mathbf {Submap2}^{local}_{pose}$ ，那么最终等价的数学公式如下：
$\color{Green} \tag{01} \mathbf {Submap2}^{global}_{pose} = \mathbf {Submap1}^{global}_{pose}*[\mathbf {Submap1}^{local}_{pose}]^{-1}*\mathbf {Submap2}^{loacl}_{pose}$
可以很明显的知道最终求得 Submap2，也就是第二个活跃的子图 global 系下的位姿。添加了一个新的子图到 optimization_problem_ 之中，其构建的 SubmapId 对应的 submap_id 比之前进行 +1 操作。然后返回两个活跃子图的 SubmapId。

五、添加第二个活跃的子图

通过前面四、五的两种情况，就会为把所有活跃的子图都添加至 optimization_problem_ 之中了，且每个子图都只被添加了一次。添加之后，optimization_problem_ 就存储了子图对应的 SubmapId，后续自己获取返回即可，代码如下：

  // 如果是等于第二个子图, 说明第二个子图已经分配了id, 已经在OptimizationProblem的submap_data_中了
  CHECK(data_.submap_data.at(last_submap_id).submap ==
        insertion_submaps.back());
  // 那么第一个子图的index就是last_submap_id.submap_index的前一个, 所以要-1
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK(data_.submap_data.at(front_submap_id).submap ==
        insertion_submaps.front());
  return {front_submap_id, last_submap_id};

六、ComputeLocalToGlobalTransform

$\color{red}疑问3:$ data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？

现在就是要解答这个疑问了，回到前面提到的 ComputeLocalToGlobalTransform() 函数，首先要注意的是，其只在 optimization_problem_ 添加第一个子图的时候需要调用该函数，后续子图的 global 位姿都是依靠前一个子图的 global 位姿计算出来的。该函数又是依旧 data_.global_submap_poses_2d 推算子图 global 位姿的。但是， $\color{red}注意:$ , 此时第一个子图都还没有添加，那么也就是说 data_.global_submap_poses_2d 肯定是空的，也就是说 ComputeLocalToGlobalTransform() 函数执行的是下面这段代码：

  // 没找到这个轨迹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();
    }

总的来说，如果 trajectory_id 轨迹有设置先对于其他轨迹的初始位姿，则会使用线性插值计算出子图的 global 位姿，如果没有设置，则认为第一个子图的位姿就是 transform::Rigid3d::Identity()。

七、总结

这样我们就解答了

$\color{red}疑问3:$ data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？

总的来说，第一个子图的 global 位姿认为是 transform::Rigid3d::Identity()，后面的子图位姿都是参考结合子图的局部位姿，近而推算出 global_ 位姿。

江南才尽，年少无知！

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
打赏
0
评论
(02)Cartographer源码无死角解析-(62) 2D后端优化→InitializeGlobalSubmapPoses()子图全局位姿的来龙去脉

疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。疑问2疑问2为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。疑问3疑问3data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？疑问4疑问4。
复制链接

扫一扫