Introduce PoseGraphInterface::GetAllSubmapPoses() (#790)

cartographer/mapping/pose_graph_interface.h

@@ -49,6 +49,11 @@ class PoseGraphInterface {
     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
   };
 
+  struct SubmapPose {
+    int version;
+    transform::Rigid3d pose;
+  };
+
   struct SubmapData {
     std::shared_ptr<const Submap> submap;
     transform::Rigid3d pose;
@@ -66,6 +71,9 @@ class PoseGraphInterface {
   // Returns data for all submaps.
   virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
 
+  // Returns the global poses for all submaps.
+  virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() = 0;
+
   // Returns the transform converting data in the local map frame (i.e. the
   // continuous, non-loop-closed frame) into the global map frame (i.e. the
   // discontinuous, loop-closed frame).

cartographer/mapping_2d/pose_graph.cc

@@ -652,10 +652,11 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
   return GetSubmapDataUnderLock(submap_id);
 }
 
-mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
+mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
 PoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
+  mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
+      submaps;
   for (const auto& submap_id_data : submap_data_) {
     submaps.Insert(submap_id_data.id,
                    GetSubmapDataUnderLock(submap_id_data.id));
@@ -663,6 +664,20 @@ PoseGraph::GetAllSubmapData() {
   return submaps;
 }
 
+mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
+PoseGraph::GetAllSubmapPoses() {
+  common::MutexLocker locker(&mutex_);
+  mapping::MapById<mapping::SubmapId, SubmapPose> submap_poses;
+  for (const auto& submap_id_data : submap_data_) {
+    auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
+    submap_poses.Insert(
+        submap_id_data.id,
+        mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
+                                       submap_data.pose});
+  }
+  return submap_poses;
+}
+
 transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
     const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
         global_submap_poses,

cartographer/mapping_2d/pose_graph.h

@@ -102,8 +102,10 @@ class PoseGraph : public mapping::PoseGraph {
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   mapping::PoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
-  mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
+  mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
   GetAllSubmapData() EXCLUDES(mutex_) override;
+  mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
+      EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>

cartographer/mapping_3d/pose_graph.cc

@@ -671,10 +671,11 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
   return GetSubmapDataUnderLock(submap_id);
 }
 
-mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
+mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
 PoseGraph::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
+  mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
+      submaps;
   for (const auto& submap_id_data : submap_data_) {
     submaps.Insert(submap_id_data.id,
                    GetSubmapDataUnderLock(submap_id_data.id));
@@ -682,6 +683,20 @@ PoseGraph::GetAllSubmapData() {
   return submaps;
 }
 
+mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
+PoseGraph::GetAllSubmapPoses() {
+  common::MutexLocker locker(&mutex_);
+  mapping::MapById<mapping::SubmapId, SubmapPose> submap_poses;
+  for (const auto& submap_id_data : submap_data_) {
+    auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
+    submap_poses.Insert(
+        submap_id_data.id,
+        mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
+                                       submap_data.pose});
+  }
+  return submap_poses;
+}
+
 transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
     const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
         global_submap_poses,

cartographer/mapping_3d/pose_graph.h

@@ -102,8 +102,10 @@ class PoseGraph : public mapping::PoseGraph {
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   mapping::PoseGraph::SubmapData GetSubmapData(
       const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
-  mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
+  mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
   GetAllSubmapData() EXCLUDES(mutex_) override;
+  mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
+      EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -34,6 +34,13 @@ PoseGraphStub::GetAllSubmapData() {
   LOG(FATAL) << "Not implemented";
 }
 
+cartographer::mapping::MapById<
+    cartographer::mapping::SubmapId,
+    cartographer::mapping::PoseGraphInterface::SubmapPose>
+PoseGraphStub::GetAllSubmapPoses() {
+  LOG(FATAL) << "Not implemented";
+}
+
 cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
     int trajectory_id) {
   LOG(FATAL) << "Not implemented";

cartographer_grpc/mapping/pose_graph_stub.h

@@ -35,6 +35,8 @@ class PoseGraphStub : public cartographer::mapping::PoseGraphInterface {
   void RunFinalOptimization() override;
   cartographer::mapping::MapById<cartographer::mapping::SubmapId, SubmapData>
   GetAllSubmapData() override;
+  cartographer::mapping::MapById<cartographer::mapping::SubmapId, SubmapPose>
+  GetAllSubmapPoses() override;
   cartographer::transform::Rigid3d GetLocalToGlobalTransform(
       int trajectory_id) override;
   cartographer::mapping::MapById<cartographer::mapping::NodeId,