Add PoseGraphInterface::GetTrajectoryNodePoses() (#795)

cartographer/mapping/pose_graph_interface.h

@@ -82,6 +82,9 @@ class PoseGraphInterface {
   // Returns the current optimized trajectories.
   virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
 
+  // Returns the current optimized trajectory poses.
+  virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() = 0;
+
   // Checks if the given trajectory is finished.
   virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
 

cartographer/mapping/trajectory_node.h

@@ -29,6 +29,14 @@
 namespace cartographer {
 namespace mapping {
 
+struct TrajectoryNodePose {
+  // Indicates whether the node has constant data set.
+  bool has_constant_data;
+
+  // The node pose in the global SLAM frame.
+  transform::Rigid3d global_pose;
+};
+
 struct TrajectoryNode {
   struct Data {
     common::Time time;

cartographer/mapping_2d/pose_graph.cc

@@ -575,6 +575,19 @@ PoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+PoseGraph::GetTrajectoryNodePoses() {
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& node_id_data : trajectory_nodes_) {
+    node_poses.Insert(
+        node_id_data.id,
+        mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+                                    node_id_data.data.global_pose});
+  }
+  return node_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();

cartographer/mapping_2d/pose_graph.h

@@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer/mapping_3d/pose_graph.cc

@@ -605,6 +605,19 @@ PoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+PoseGraph::GetTrajectoryNodePoses() {
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& node_id_data : trajectory_nodes_) {
+    node_poses.Insert(
+        node_id_data.id,
+        mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+                                    node_id_data.data.global_pose});
+  }
+  return node_poses;
+}
+
 sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
   common::MutexLocker locker(&mutex_);
   return optimization_problem_.imu_data();

cartographer/mapping_3d/pose_graph.h

@@ -110,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -52,6 +52,12 @@ PoseGraphStub::GetTrajectoryNodes() {
   LOG(FATAL) << "Not implemented";
 }
 
+cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                               cartographer::mapping::TrajectoryNodePose>
+PoseGraphStub::GetTrajectoryNodePoses() {
+  LOG(FATAL) << "Not implemented";
+}
+
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
   LOG(FATAL) << "Not implemented";
 }

cartographer_grpc/mapping/pose_graph_stub.h

@@ -42,6 +42,9 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
   cartographer::mapping::MapById<cartographer::mapping::NodeId,
                                  cartographer::mapping::TrajectoryNode>
   GetTrajectoryNodes() override;
+  cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                                 cartographer::mapping::TrajectoryNodePose>
+  GetTrajectoryNodePoses() override;
   bool IsTrajectoryFinished(int trajectory_id) override;
   std::vector<Constraint> constraints() override;