From 1a837ef3ab5b36963d75a267bec76c1d06332bde Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Fri, 5 Jan 2018 14:19:08 +0100
Subject: [PATCH] Introduce PoseGraphInterface::GetAllSubmapPoses() (#790)

---
 cartographer/mapping/pose_graph_interface.h  |  8 ++++++++
 cartographer/mapping_2d/pose_graph.cc        | 19 +++++++++++++++++--
 cartographer/mapping_2d/pose_graph.h         |  4 +++-
 cartographer/mapping_3d/pose_graph.cc        | 19 +++++++++++++++++--
 cartographer/mapping_3d/pose_graph.h         |  4 +++-
 cartographer_grpc/mapping/pose_graph_stub.cc |  7 +++++++
 cartographer_grpc/mapping/pose_graph_stub.h  |  2 ++
 7 files changed, 57 insertions(+), 6 deletions(-)

diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h
index f0626f1..2db3b21 100644
--- a/cartographer/mapping/pose_graph_interface.h
+++ b/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).
diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc
index 61a3222..787c5ab 100644
--- a/cartographer/mapping_2d/pose_graph.cc
+++ b/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,
diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h
index 93de584..0514794 100644
--- a/cartographer/mapping_2d/pose_graph.h
+++ b/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>
diff --git a/cartographer/mapping_3d/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc
index a45467f..7e430d1 100644
--- a/cartographer/mapping_3d/pose_graph.cc
+++ b/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,
diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h
index 143caf4..a0863c4 100644
--- a/cartographer/mapping_3d/pose_graph.h
+++ b/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>
diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc
index a33ed75..bbb277c 100644
--- a/cartographer_grpc/mapping/pose_graph_stub.cc
+++ b/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";
diff --git a/cartographer_grpc/mapping/pose_graph_stub.h b/cartographer_grpc/mapping/pose_graph_stub.h
index a8ab1b2..f063c48 100644
--- a/cartographer_grpc/mapping/pose_graph_stub.h
+++ b/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,