Cartographer学习记录：手把手教你如何获取3D SLAM后端的位姿

原创

已于 2022-03-12 13:59:41 修改 · 2.2k 阅读

16 ·

CC 4.0 BY-SA版权

文章标签：

#学习 #3d #自动驾驶

于 2022-03-12 13:55:43 首次发布

本文详细介绍了如何在Cartographer构建地图后，通过获取位姿信息并将其作为ROS topic发布。首先，分析了PoseGraphData结构体，重点在于TrajectoryNode数据，其中包含了位姿信息。接着，阐述了如何在cartographer_ros中设计接口函数，调用底层的GetTrajectoryNodePoses()获取位姿，并创建发布者发布TrajectoryNodePose消息。最后，讲解了如何创建自定义ROS消息类型my_msgs::trajectorynodepose，以便在程序中使用。

在我们利用cartographer建图完成后用来定位时，就需要获取位姿信息，那么该如何获取后端的位姿和时间并将其作为topic信息发布出来呢？本篇文章将手把手教你如何实现。

一.明确需要的数据

（1）首先我们需要知道的是，底层cartographer主要依靠MapBuilder类来进行地图的构建、节点局部位姿和全局位姿的确定，地图的构建依赖Submap，局部位姿靠local_slam，全局位姿优化靠pose_graph，既然我们需要位姿这个数据，不妨将目光集中在pose_graph身上。

（2）pose_graph的数据储存在pose_graph_data里面，因此我们就可以将目光放在cartographer/mapping/pose_graph_data.h文件上，注意：cartographer/pose_graph文件夹下也有pose_graph_data.h文件，但是此文件包含数据量较少，不是我们所需要，先略过。

在cartographer/mapping/pose_graph_data.h文件中，我们可以发现这样一个数据结构体：

struct PoseGraphData {
  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.
  MapById<SubmapId, InternalSubmapData> submap_data;

  // Global submap poses currently used for displaying data.
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

  // Data that are currently being shown.
  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;
};

（3）这是一个所含数据较全的struct，包含了子图数据、轨迹节点数据、地标节点数据、轨迹状态、轨迹初始位姿、节点约束等数据，回顾我们的目标——找位姿，位姿是什么的位姿？那必然是轨迹上各个节点的位姿，所以我们将目光投向MapById<NodeId, TrajectoryNode> trajectory_nodes上。这是一个容器，通过轨迹的id查询得到对应轨迹数据，所以，从上面的分析可以知道，我们需要的是TrajectoryNode数据。

（4）接下来就将目光转移到TrajectoryNode上，该类定义在cartographer/mapping/trajectory_node.h中

namespace cartographer {
namespace mapping {

struct TrajectoryNodePose {
  struct ConstantPoseData {
    common::Time time;
    transform::Rigid3d local_pose;
  };
  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;

  absl::optional<ConstantPoseData> constant_pose_data;
};

struct TrajectoryNode {
  struct Data {
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SL