Allow trimmers fetch submap data. (#1024)

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -716,12 +716,7 @@ PoseGraph::SubmapData PoseGraph2D::GetSubmapData(const SubmapId& submap_id) {
 MapById<SubmapId, PoseGraphInterface::SubmapData>
 PoseGraph2D::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  return GetSubmapDataUnderLock();
-  for (const auto& submap_id_data : submap_data_) {
-    submaps.Insert(submap_id_data.id,
-                   GetSubmapDataUnderLock(submap_id_data.id));
-  }
-  return submaps;
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapPose>
@@ -788,6 +783,11 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
   return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
 }
 
+MapById<SubmapId, PoseGraphInterface::SubmapData>
+PoseGraph2D::TrimmingHandle::GetAllSubmapData() const {
+  return parent_->GetSubmapDataUnderLock();
+}
+
 std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
     int trajectory_id) const {
   std::vector<SubmapId> submap_ids;
@@ -860,5 +860,15 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
   }
 }
 
+MapById<SubmapId, PoseGraphInterface::SubmapData>
+PoseGraph2D::GetSubmapDataUnderLock() {
+  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  for (const auto& submap_id_data : submap_data_) {
+    submaps.Insert(submap_id_data.id,
+                   GetSubmapDataUnderLock(submap_id_data.id));
+  }
+  return submaps;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -153,6 +153,8 @@ class PoseGraph2D : public PoseGraph {
     SubmapState state = SubmapState::kActive;
   };
 
+  MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock();
+
   // Handles a new work item.
   void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
@@ -279,6 +281,8 @@ class PoseGraph2D : public PoseGraph {
 
     int num_submaps(int trajectory_id) const override;
     std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
+    MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+        const override;
     void MarkSubmapAsTrimmed(const SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override;

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -749,12 +749,7 @@ PoseGraph::SubmapData PoseGraph3D::GetSubmapData(const SubmapId& submap_id) {
 MapById<SubmapId, PoseGraphInterface::SubmapData>
 PoseGraph3D::GetAllSubmapData() {
   common::MutexLocker locker(&mutex_);
-  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  return GetSubmapDataUnderLock();
-  for (const auto& submap_id_data : submap_data_) {
-    submaps.Insert(submap_id_data.id,
-                   GetSubmapDataUnderLock(submap_id_data.id));
-  }
-  return submaps;
 }
 
 MapById<SubmapId, PoseGraphInterface::SubmapPose>
@@ -828,6 +823,10 @@ std::vector<SubmapId> PoseGraph3D::TrimmingHandle::GetSubmapIds(
   }
   return submap_ids;
 }
+MapById<SubmapId, PoseGraphInterface::SubmapData>
+PoseGraph3D::TrimmingHandle::GetAllSubmapData() const {
+  return parent_->GetSubmapDataUnderLock();
+}
 
 bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
   return parent_->IsTrajectoryFinished(trajectory_id);
@@ -891,5 +890,15 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed(
   }
 }
 
+MapById<SubmapId, PoseGraphInterface::SubmapData>
+PoseGraph3D::GetSubmapDataUnderLock() {
+  MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
+  for (const auto& submap_id_data : submap_data_) {
+    submaps.Insert(submap_id_data.id,
+                   GetSubmapDataUnderLock(submap_id_data.id));
+  }
+  return submaps;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -158,6 +158,8 @@ class PoseGraph3D : public PoseGraph {
     SubmapState state = SubmapState::kActive;
   };
 
+  MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock();
+
   // Handles a new work item.
   void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
 
@@ -284,6 +286,8 @@ class PoseGraph3D : public PoseGraph {
 
     int num_submaps(int trajectory_id) const override;
     std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
+    MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+        const override;
     void MarkSubmapAsTrimmed(const SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override;

cartographer/mapping/pose_graph_trimmer.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_MAPPING_POSE_GRAPH_TRIMMER_H_
 
 #include "cartographer/mapping/id.h"
+#include "cartographer/mapping/pose_graph_interface.h"
 
 namespace cartographer {
 namespace mapping {
@@ -31,6 +32,8 @@ class Trimmable {
   virtual int num_submaps(int trajectory_id) const = 0;
 
   virtual std::vector<SubmapId> GetSubmapIds(int trajectory_id) const = 0;
+  virtual MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+      const = 0;
 
   // Marks 'submap_id' and corresponding intra-submap nodes as trimmed. They
   // will no longer take part in scan matching, loop closure, visualization.

cartographer/mapping/pose_graph_trimmer_test.cc

@@ -42,6 +42,11 @@ class FakePoseGraph : public Trimmable {
     return submaps_;
   }
 
+  MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
+      const override {
+    return {};
+  }
+
   void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
     trimmed_submaps_.push_back(submap_id);
   }