(02)Cartographer源码无死角解析-(57) 2D后端优化→MapById::lower_bound()、PoseGraphData、TrajectoryConnectivityState

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

一、前言

上一篇博客，主要是对 src/cartographer/cartographer/mapping/id.h 文件中的 class MapById 进行了分析，主要核心是围绕成员变量 MapById::trajectories_ 的两个迭代器：ConstIterator、ConstTrajectoryIterator。前者能够实现对数据节点的遍历操作，后者是对轨迹的迭代与遍历。这里把上一篇博客绘画的图再贴一下：

这样大家会有更加直观的了解。

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

该疑问并没有找到答案，还有就是关于 MapById::lower_bound() 函数没有进行讲解，现在就来来其过程。
 

二、MapById::lower_bound()

首先该函数有两个形参，第一个 trajectory_id 表示轨迹标识，time 标识一个时间戳。该函数的功能是返回一条轨迹第一个时间大于time的迭代器，如果没有找到会返回轨迹末尾的下一位置。二分法的原理十分的简单，这里就不再进行理论讲解了，其核心原理就是循环对一主定义域对半划分成两个子域，每次划分之后，确定目标在那个子域，然后该把子域作为主域再次进行划分，直到找到目标。代码注释如下：

  // Returns an iterator to the first element in the container belonging to
  // trajectory 'trajectory_id' whose time is not considered to go before
  // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
  // before 'time'.
  // 该函数会返回轨迹中第一个大于time的数据迭代器
  ConstIterator lower_bound(const int trajectory_id,
                            const common::Time time) const {
    
    
    // 如果这个轨迹id的数据为0
    if (SizeOfTrajectoryOrZero(trajectory_id) == 0) {
    
    
      //返回指向轨迹末尾的下一个迭代器,表示为空，未查找到
      return EndOfTrajectory(trajectory_id);
    }

    //获取 trajectory_id 轨迹的数据
    const std::map<int, DataType>& trajectory =
        trajectories_.at(trajectory_id).data_;
    // 如果整个表的最后一个数据的时间比time小, 就返回EndOfTrajectory，表示没有找到
    if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
    
    
      return EndOfTrajectory(trajectory_id);
    }

    // 二分查找，分别获取最左边与最右边数据
    auto left = trajectory.begin();
    auto right = std::prev(trajectory.end());
    //如果首尾不相等，说明还要继续查找
    while (left != right) {
    
    
      // This is never 'right' which is important to guarantee progress.
      // 获得 [left,right] 中间数据节点的索引
      const int middle = left->first + (right->first - left->first) / 2;
      // This could be 'right' in the presence of gaps, so we need to use the
      // previous element in this case.
      // 返回trajectory第一个大于或等于middle迭代器
      auto lower_bound_middle = trajectory.lower_bound(middle);
      // 如果lower_bound_middle大于middle，往前移动一个单位
      // 这样可以保证 lower_bound_middle 小于 middle
      if (lower_bound_middle->first > middle) {
    
    
        CHECK(lower_bound_middle != left);
        lower_bound_middle = std::prev(lower_bound_middle);
      }

      //根据time所在区域，对[left,right]重新划分,
      if (internal::GetTime(lower_bound_middle->second) < time) {
    
    
        left = std::next(lower_bound_middle);
      } else {
    
     
        right = lower_bound_middle;
      }
    }

    return ConstIterator(*this, IdType{
    
    trajectory_id, left->first});
  }

深入了解 MapById 之后，再次回过头来看一下 PoseGraph2D::GetInterpolatedGlobalTrajectoryPose()，此时就比较好理解了，该函数中的 data_.trajectory_nodes 就是一个 MapById<NodeId, TrajectoryNode> 实例，NodeId 对象中包含有 trajectory_id 与 node_index ，分别用于标识轨迹与数据节点。TrajectoryNode 在前面提到过，其就是描述一个轨迹节点。

PoseGraph2D::GetInterpolatedGlobalTrajectoryPose() 函数是对时间进行插值，其核心要点就是找到两个两个节点，一个节点时间大于time，一个节点时间小于time，且这两个节点都必须是距离time最近的节点，也就是源码中的 std::prev(it)->data.time() 与 it->data.time()。找到两个时间节点之后，直接进行线性插值即可。
 

三、PoseGraphData

1、额外定义

除了MapById之外，PoseGraph2D 中有一个最重要的成员变量，那就是 PoseGraph2D::data_，定义如下：

class PoseGraph2D : public PoseGraph {
    
    
  ......
  PoseGraphData data_ GUARDED_BY(mutex_);
  ......
}

可知其类型为 PoseGraphData，该结构体实现于 src/cartographer/cartographer/mapping/internal/pose_graph_data.h 文件中，在分析 PoseGraphData 之前，先来看看该 .h 文件中的其他内容。其首先，定义了枚举类：

// The current state of the submap in the background threads. After this
// transitions to 'kFinished', all nodes are tried to match
// against this submap. Likewise, all new nodes are matched against submaps in
// that state.
// 后台线程中子图的当前状态.在此转换为“kFinished”之后, 所有节点都将尝试与此子图匹配, 进行回环检测. 
// 同样, 所有新节点都与kNoConstraintSearch状态下的子图匹配.
enum class SubmapState {
    
     kNoConstraintSearch, kFinished };

其用来标识子图的状态，新节点都与kNoConstraintSearch状态下的子图匹配，同时所有的节点都会与kFinished状态子图匹配。先大致了解一下，后续用到就会有比较深的体会了。

接着，又定义结构体 struct InternalTrajectoryState，名字翻译过来叫做内部轨迹状态，现在比较懵逼，不知道为啥要加上内部：

// 轨迹的状态
struct InternalTrajectoryState {
    
    
  enum class DeletionState {
    
    
    NORMAL, //正常
    SCHEDULED_FOR_DELETION, //计划要被删除
    WAIT_FOR_DELETION //等待删除
  };

  PoseGraphInterface::TrajectoryState state =
      PoseGraphInterface::TrajectoryState::ACTIVE;
  DeletionState deletion_state = DeletionState::NORMAL;
};

从代码上来看，包含了InternalTrajectoryState::state 与 InternalTrajectoryState::deletion_state 两个成员变量。前者存在四种状态：

  enum class TrajectoryState {
    
     ACTIVE, FINISHED, FROZEN, DELETED };
  //激活、完成、冻结、删除

对于删除状态又做了细分，分别为 NORMAL(正常)、 SCHEDULED_FOR_DELETION(计划要被删除)、WAIT_FOR_DELETION(等待删除)。

2、正式定义

对于 pose_graph_data.h 文件中的额外定义讲解完成之后，现在就是正式对 PoseGraphData 讲解。

struct PoseGraphData {
    
    
  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.

  // submap_data_ 里面,包含了所有的submap
  MapById<SubmapId, InternalSubmapData> submap_data;

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

  // Data that are currently being shown.
  // 所有的轨迹节点的id与 节点的在global坐标系下的坐标, 在local map 下的坐标与时间
  MapById<NodeId, TrajectoryNode> trajectory_nodes;

  // Global landmark poses with all observations.
  std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      landmark_nodes;

  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;
  // 节点的个数
  int num_trajectory_nodes = 0;
  // 轨迹与轨迹的状态
  std::map<int, InternalTrajectoryState> trajectories_state;

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

  // 所有的约束数据
  std::vector<PoseGraphInterface::Constraint> constraints;
};

总来说，PoseGraphData包含了多个子图，多条轨迹的，以及所有轨迹节点的信息，且都是以map或者MapById(升级版map)的方式进行存储。 如 submap_data、global_submap_poses_2d、global_submap_poses_3d 都是以 SubmapId 为 key，其包含子图序列号，以及子图所属于轨迹。submap_data 对应的 value 就是子图数据，global_submap_poses_2d、global_submap_poses_3d 对应的 value 存储的都是子图的位姿。

trajectory_nodes 以 NodeId 为索引，存储的都是 TrajectoryNode 对象，其记录了一个节点扫描相关数据，以及该节点的全局位姿；landmark_nodes 记录由 landmark 推断出来的全局位置，以及观测到的 landmark 等，其以每次观测到的 landmark 为一个节点；

trajectory_connectivity_state 记录不同轨迹的连接状态，下面单独进行分析；num_trajectory_nodes 记录轨迹节点数据；trajectories_state 记录轨迹总体状态(对删除状态还做了细化)；initial_trajectory_poses 记录没条轨迹的初始位姿(通常没有设置，或者来自于配置文件)

constraints 是一个重点，计算出来子图内约束或者子图间约束都会保存在该变量之中。
 

四、ConnectedComponents

对于成员变量 PoseGraphData::trajectory_connectivity_state 定义如下：

  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;

其主要用于描述多个轨迹之间的连接关系。但是如果直接对 TrajectoryConnectivityState 进行分析，也不是很好，在 src/cartographer/cartographer/mapping/internal/trajectory_connectivity_state.h 文件中，可以看到成员变量 TrajectoryConnectivityState::connected_components_ 才是核心，其类型为 ConnectedComponents，只有了解了他之后，再来了解 TrajectoryConnectivityState 才不会显得那么困难。

该类在 src/cartographer/cartographer/mapping/internal/connected_components.h 中申明。ConnectedComponents 从翻译来看，其表示组件连接，看起来还是比较迷糊，这里暂时先把每条轨迹看成一个组件，或许方便理解一些。

ConnectedComponents 包含一个核心成员变量 forest_，定义如下：

  // Tracks transitive connectivity using a disjoint set forest, i.e. each
  // entry points towards the representative for the given trajectory.
  std::map<int, int> forest_ GUARDED_BY(lock_);

用来表示两个轨迹得连接关系。如轨迹a的下一个轨迹是b，说明a与b是连接到一起的，表示为 std::pair(a,b)。需要注意，这里的a,b位置是不能互换的，因为描述的是一个传递关系，轨迹a可以传递给b，不表示b可以传递给a，也就是说其存在先后关系。另外还存在成员变量如下：

  // Tracks the number of direct connections between a pair of trajectories.
  std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);

翻译过来: 跟踪一对轨迹之间的直接连接数。感觉不够明显，不过没关系，后续分析函数后续就能了解到了。那么就来看看 ConnectedComponents 的成员函数吧。

1.Add()

// 添加一个最初只连接到它自己的轨迹
void ConnectedComponents::Add(const int trajectory_id) {
    
    
  absl::MutexLock locker(&lock_);
  forest_.emplace(trajectory_id, trajectory_id);
}

该函数就比较简单了，添加一个新的轨迹，后面没有跟着任何的轨迹，所以表示为(trajectory_id,trajectory_id)。从这里可以看出，如果 forest_ 一组元素的key与value先同，说明其为整条轨迹链的最后一条轨迹。这里举例一下：

轨迹链: (1,2)、(2,3)、(3,5)、(5,5)

如果上诉为一条轨迹链，也就是说轨迹1后接着轨迹2，轨迹2后接着轨迹3、轨迹3后接着轨迹5、轨迹5就到最后了，其后没有接着任何的轨迹。

2.FindSet()

该函数就比较重要了，其主要找到一条轨迹链的最后一条轨迹。这里先举一个例子，如下是 forest_ 中的所有数据：

(3,2)  (4,8)  (2,9)  (11,5)  (9,12)  (6,10)  (12,12)

如果输入的参数 trajectory_id=3；那么函数返回最终结果 it->second=12，因为该条轨迹链为 (3,2)→(2,9)→(9,12)→(12,12)，函数注释如下：

// 找到这条轨迹连接链的尽头, (b, b)是尽头
int ConnectedComponents::FindSet(const int trajectory_id) {
    
    
  //寻找到trajectory_id对应的迭代器
  auto it = forest_.find(trajectory_id);
  //如果没有找到则报错
  CHECK(it != forest_.end());
  //it指向的轨迹非末端轨迹
  if (it->first != it->second) {
    
    
    // Path compression for efficiency.
    //递归调用，以it->second为索引，找到与该轨迹链接的下一个轨迹
    //然后赋值给 it->second
    it->second = FindSet(it->second);
  }
  //返回末端轨迹
  return it->second;
}

3.Union()

实现 FindSet() 函数后，在该基础上就可以做很多其他操作了，如Union()函数，该函数比较简单，就是把两个轨迹链接起来。

// 将轨迹a的id与轨迹b的id连接起来, 添加到forest_中
void ConnectedComponents::Union(const int trajectory_id_a,
                                const int trajectory_id_b) {
    
    
  //自己与自己连接
  forest_.emplace(trajectory_id_a, trajectory_id_a);
  forest_.emplace(trajectory_id_b, trajectory_id_b);
  //获取轨迹链a,b的末端
  const int representative_a = FindSet(trajectory_id_a);
  const int representative_b = FindSet(trajectory_id_b);
  //把a的末端连接到b的末端上   
  forest_[representative_a] = representative_b;
}

这里可能存在一些疑问，为什么要把末端连接起来。原理是这样的，首先往 forest_ 中添加了(a,a) 与 (b,b) 两个元素，然后再构建一个(a,b)，那么可以形成 (a,a)→(a,b)→(b,b) 这样一条轨迹链。

4.Connect()

该函数本质上是对Union() 函数的利用，其是把两条轨迹连接在一起，不过其还会根据轨迹id大小进行一个排序，sorted_pair 第一个值为最小值，第二个为最大值。然后把 sorted_pair 添加到 connection_map_ 之中。总的来说，Connect() 调用 Union() 把两个轨迹连接起来的同事，还把这对轨迹添加到了 ConnectedComponents::connection_map_ 之中：

// 将轨迹a的id与轨迹b的id连接起来, 添加到connection_map_中
void ConnectedComponents::Connect(const int trajectory_id_a,
                                  const int trajectory_id_b) {
    
    
  absl::MutexLock locker(&lock_);
  Union(trajectory_id_a, trajectory_id_b);
  auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
  ++connection_map_[sorted_pair];
}

不过特别要注意一个点，那就是两个轨迹连接之后都会执行 ++ 操作，也就是说 connection_map_ 是用于记录两个轨迹连接的次数。

5.TransitivelyConnected()

该函数主要判断两个轨迹是否具备传递关系，如下图所示：轨迹1，2，3，4，5，6 中任意两条轨迹都是具备传递关系的，调用 TransitivelyConnected() 函数都会返回 true：

但是他们与轨迹10是不具备传递关系的，代码注释如下：

// 判断2个轨迹是否处于连接状态
bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
                                                const int trajectory_id_b) {
    
    
  //如果相同，则说明判断自己是否与自己连接，返回true
  if (trajectory_id_a == trajectory_id_b) {
    
    
    return true;
  }
  //上锁操作，因为对 forest_ 进行氛访问
  absl::MutexLock locker(&lock_);

  // count 功能为统计给定key的数目，由于key不能重复，所以只能返回0或者1
  // trajectory_id_a 与 trajectory_id_b 任一轨迹不存在，那么该两轨迹则不会连接
  if (forest_.count(trajectory_id_a) == 0 ||
      forest_.count(trajectory_id_b) == 0) {
    
    
    return false;
  }
  // 如果 trajectory_id_a 与 trajectory_id_b 都存在于 forest_ 之中
  // 则判断两条轨迹链的末端是否相等，相等则表示连接，否则表示未连接。
  return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
}

6.GetComponent()

该函数以vector的形式，返回与trajectory_id轨迹链具有相同末端轨迹的列表(可能存在多个元素)

// 返回与trajectory_id的连接组件的轨迹 ID 列表
std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
    
    
  absl::MutexLock locker(&lock_);
  //找到trajectory_id对应的末端轨迹
  const int set_id = FindSet(trajectory_id);

  //存储末端轨迹相同的入口轨迹
  std::vector<int> trajectory_ids;
  for (const auto& entry : forest_) {
    
    
    if (FindSet(entry.first) == set_id) {
    
    
      trajectory_ids.push_back(entry.first);
    }
  }
  //返回与trajectory_id存在连接关系的所有轨迹。可以认为末端轨迹相同的轨迹链，他们是相互连接的。
  return trajectory_ids;
}

7.Components()

该函数可以理解为 GetComponent() 的升级版，GetComponent() 与指定轨迹链具有相同轨迹末端的所有轨迹链(入口轨迹)。但是 Components() 不用指定轨迹链，而是对每条轨迹都会做一个统计，统计与其具有相同轨迹末端的所有轨迹。简单的说，GetComponent() 是统计单个轨迹的，Components() 是统计所有轨迹的。所以 Components() 是以 std::vector<std::vector> result 二维数组的方式返回。

// 获取链接的组件的轨迹ID列表
std::vector<std::vector<int>> ConnectedComponents::Components() {
    
    
  // Map from cluster exemplar -> growing cluster.
  // 创建一个map对象，key存储末端轨迹，value存储末端轨迹相同的所有轨迹链的起始(入口)轨迹
  // 总的来说，一个末端轨迹可能对应多个起始(入口轨迹)
  absl::flat_hash_map<int, std::vector<int>> map;

  //forest_ 的每个key都认为是一个入口轨迹，循环遍历forest_所有元素
  // 记录每个末端轨迹对应的所有入口轨迹
  absl::MutexLock locker(&lock_);
  for (const auto& trajectory_id_entry : forest_) {
    
    
    map[FindSet(trajectory_id_entry.first)].push_back(
        trajectory_id_entry.first);
  }

  // result 存储所有入口轨迹，且结束轨迹相同的入口轨迹在同一个 std::vector<int> 对象中。
  std::vector<std::vector<int>> result;
  result.reserve(map.size());
  for (auto& pair : map) {
    
    
    result.emplace_back(std::move(pair.second));
  }
  return result;
}

8.ConnectionCount()

// 获取这个连接的个数
int ConnectedComponents::ConnectionCount(const int trajectory_id_a,
                                         const int trajectory_id_b) {
    
    
  absl::MutexLock locker(&lock_);
  //connection_map_记录两个轨迹链被连接的次数，如果connection_map_中无法找到
  //(trajectory_id_a, trajectory_id_b)轨迹对，则认为没有连接过，次数为0
  const auto it =
      connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
  return it != connection_map_.end() ? it->second : 0;
}

该函数能够返回一对轨迹被连接的次数，这里只是返回结果，具体的统计代码是 ConnectedComponents::Connect() 函数中的 ++ 操作。
 

六、TrajectoryConnectivityState

对 ConnectedComponents 有一定了解之后，再来查看 TrajectoryConnectivityState 就比较简单了，再前面已经提到其成员变量 connected_components_ 是一个核心。除此之外，还存在另外一个成员变量：

std::map<std::pair<int, int>, common::Time> last_connection_time_map_;

其用于记录两条轨迹上一次连接的时间，所以其是可以被刷新覆盖的，也就是说一对轨迹重新连接之后，会更新连接时间。这也是 TrajectoryConnectivityState 对 ConnectedComponents 再一次封装的目的。这里应该是用到了模式设计中的装饰器模式。

1.Add()

该函数的目的添加一条轨迹，本质上就是把 (trajectory_id,trajectory_id) 这样一个元素，添加到 TrajectoryConnectivityState::connected_components_::forest_ 之中，等价于自己与自己连接。

// 添加一个最初只连接到自身的轨迹
void TrajectoryConnectivityState::Add(const int trajectory_id) {
    
    
  connected_components_.Add(trajectory_id);
}

2.Connect()

该函数两条分支逻辑：①首先判断输入的两条轨迹 trajectory_id_a 与 trajectory_id_b 是否具备传递(间接连接)关系，如果具备则直接刷新该轨迹对的连接时间即可。②如果不具备传递关系，则需要找到两条轨迹链的所有轨迹，a的所有轨迹与b的所有轨迹进行两两匹配，匹配之后的轨迹对都对齐时间进行更新。

最后，两条分支逻辑无论走那条，都会调用 connected_components_.Connect(trajectory_id_a, trajectory_id_b) 函数，把 trajectory_id_a 与 trajectory_id_b 连接到一起，这里需要注意一下，连接次数的增加，也就是对 TrajectoryConnectivityState::connected_components_::connection_map_ 的操作。函数代码如下所示：

// 连接两条轨迹. 如果任一轨迹未跟踪, 则将对其进行跟踪,重复调用 Connect 会增加连接计数并更新上次连接时间
void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
                                          const int trajectory_id_b,
                                          const common::Time time) {
    
    
  if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) {
    
    
    // The trajectories are transitively connected, i.e. they belong to the same
    // connected component. In this case we only update the last connection time
    // of those two trajectories.
    // 更新时间
    auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
    if (last_connection_time_map_[sorted_pair] < time) {
    
    
      last_connection_time_map_[sorted_pair] = time;
    }
  } else {
    
    
    // The connection between these two trajectories is about to join to
    // connected components. Here we update all bipartite trajectory pairs for
    // the two connected components with the connection time. This is to quickly
    // change to a more efficient loop closure search (by constraining the
    // search window) when connected components are joined.
    // 这两条轨迹之间的连接即将加入连接的组件.在这里, 我们使用连接时间更新两个连接组件的所有二分轨迹对
    // 这是为了在连接组件连接时快速更改为更有效的回环搜索（通过约束搜索窗口）.
    
    // 获取所有与轨迹a连接的轨迹id
    std::vector<int> component_a =
        connected_components_.GetComponent(trajectory_id_a);
    // 获取所有与轨迹a连接的轨迹id
    std::vector<int> component_b =
        connected_components_.GetComponent(trajectory_id_b);
    // 为所有的a与b间的连接组合设置时间
    for (const auto id_a : component_a) {
    
    
      for (const auto id_b : component_b) {
    
    
        // c++11: std::minmax 返回由最小值与最大值组成的一个pair
        auto id_pair = std::minmax(id_a, id_b);
        last_connection_time_map_[id_pair] = time;
      }
    }
  }
  // 为两条轨迹添加连接关系
  connected_components_.Connect(trajectory_id_a, trajectory_id_b);
}

3.剩余函数

// 判断2个轨迹是否处于连接状态
bool TrajectoryConnectivityState::TransitivelyConnected(
    const int trajectory_id_a, const int trajectory_id_b) const {
    
    
  return connected_components_.TransitivelyConnected(trajectory_id_a,
                                                     trajectory_id_b);
}

// 获取链接的组件的轨迹ID列表
std::vector<std::vector<int>> TrajectoryConnectivityState::Components() const {
    
    
  return connected_components_.Components();
}

// 返回两个轨迹之间的最后连接时间
common::Time TrajectoryConnectivityState::LastConnectionTime(
    const int trajectory_id_a, const int trajectory_id_b) {
    
    
  const auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
  return last_connection_time_map_[sorted_pair];
}

剩下的这几个函数就不再重复讲解了，因为其都是对 ConnectedComponents 成员函数的调用，该些函数在前面都已经讲解过了。
 

六、结语

这篇博客的东西有点多了，对 MapById::lower_bound() 、PoseGraphData、TrajectoryConnectivityState、ConnectedComponents 都进行了比较细的分析。不过这里要把前面的疑问重述一下，免得遗忘了：

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