Move GetSubmapData from GlobalTrajectoryBuilder into SparsePoseGraph. (#352)

- Adds a function to query all SubmapData from the SparsePoseGraph.
- Changes MapBuilder::SubmapToProto to take a SubmapId.
- Remove num_submaps().

cartographer/mapping/collated_trajectory_builder.cc

@@ -46,15 +46,6 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-int CollatedTrajectoryBuilder::num_submaps() {
-  return wrapped_trajectory_builder_->num_submaps();
-}
-
-TrajectoryBuilder::SubmapData CollatedTrajectoryBuilder::GetSubmapData(
-    const int submap_index) {
-  return wrapped_trajectory_builder_->GetSubmapData(submap_index);
-}
-
 const TrajectoryBuilder::PoseEstimate&
 CollatedTrajectoryBuilder::pose_estimate() const {
   return wrapped_trajectory_builder_->pose_estimate();

cartographer/mapping/collated_trajectory_builder.h

@@ -49,8 +49,6 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
   CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
       delete;
 
-  int num_submaps() override;
-  SubmapData GetSubmapData(int submap_index) override;
   const PoseEstimate& pose_estimate() const override;
 
   void AddSensorData(const string& sensor_id,

cartographer/mapping/global_trajectory_builder_interface.h

@@ -39,7 +39,6 @@ namespace mapping {
 class GlobalTrajectoryBuilderInterface {
  public:
   using PoseEstimate = TrajectoryBuilder::PoseEstimate;
-  using SubmapData = TrajectoryBuilder::SubmapData;
 
   GlobalTrajectoryBuilderInterface() {}
   virtual ~GlobalTrajectoryBuilderInterface() {}
@@ -49,8 +48,6 @@ class GlobalTrajectoryBuilderInterface {
   GlobalTrajectoryBuilderInterface& operator=(
       const GlobalTrajectoryBuilderInterface&) = delete;
 
-  virtual int num_submaps() = 0;
-  virtual SubmapData GetSubmapData(int submap_index) = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
   virtual void AddRangefinderData(common::Time time,

cartographer/mapping/map_builder.cc

@@ -104,25 +104,25 @@ int MapBuilder::GetBlockingTrajectoryId() const {
   return sensor_collator_.GetBlockingTrajectoryId();
 }
 
-string MapBuilder::SubmapToProto(const int trajectory_id,
-                                 const int submap_index,
+string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
                                  proto::SubmapQuery::Response* const response) {
-  if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) {
-    return "Requested submap from trajectory " + std::to_string(trajectory_id) +
-           " but there are only " + std::to_string(num_trajectory_builders()) +
-           " trajectories.";
+  if (submap_id.trajectory_id < 0 ||
+      submap_id.trajectory_id >= num_trajectory_builders()) {
+    return "Requested submap from trajectory " +
+           std::to_string(submap_id.trajectory_id) + " but there are only " +
+           std::to_string(num_trajectory_builders()) + " trajectories.";
   }
 
-  const int num_submaps = trajectory_builders_.at(trajectory_id)->num_submaps();
-  if (submap_index < 0 || submap_index >= num_submaps) {
-    return "Requested submap " + std::to_string(submap_index) +
-           " from trajectory " + std::to_string(trajectory_id) +
+  const int num_submaps =
+      sparse_pose_graph_->num_submaps(submap_id.trajectory_id);
+  if (submap_id.submap_index < 0 || submap_id.submap_index >= num_submaps) {
+    return "Requested submap " + std::to_string(submap_id.submap_index) +
+           " from trajectory " + std::to_string(submap_id.trajectory_id) +
            " but there are only " + std::to_string(num_submaps) +
            " submaps in this trajectory.";
   }
 
-  const auto submap_data =
-      trajectory_builders_.at(trajectory_id)->GetSubmapData(submap_index);
+  const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id);
   CHECK(submap_data.submap != nullptr);
   submap_data.submap->ToResponseProto(submap_data.pose, response);
   return "";

cartographer/mapping/map_builder.h

@@ -27,6 +27,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/thread_pool.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/map_builder_options.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
@@ -71,10 +72,9 @@ class MapBuilder {
   // unblocked.
   int GetBlockingTrajectoryId() const;
 
-  // Fills the SubmapQuery::Response corresponding to 'submap_index' from
-  // 'trajectory_id'. Returns an error string on failure, or an empty string on
-  // success.
-  string SubmapToProto(int trajectory_id, int submap_index,
+  // Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
+  // error string on failure, or an empty string on success.
+  string SubmapToProto(const SubmapId& submap_id,
                        proto::SubmapQuery::Response* response);
 
   int num_trajectory_builders() const;

cartographer/mapping/sparse_pose_graph.cc

@@ -64,6 +64,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   std::map<NodeId, NodeId> node_id_remapping;  // Due to trimming.
 
   const auto all_trajectory_nodes = GetTrajectoryNodes();
+  const auto all_submap_data = GetAllSubmapData();
   for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
        ++trajectory_id) {
     const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
@@ -86,12 +87,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
     }
 
     if (!single_trajectory_nodes.empty()) {
-      const int num_submaps_in_trajectory = num_submaps(trajectory_id);
-      for (int submap_index = 0; submap_index != num_submaps_in_trajectory;
-           ++submap_index) {
-        const SubmapId submap_id{static_cast<int>(trajectory_id), submap_index};
+      for (const auto& submap_data : all_submap_data[trajectory_id]) {
         *trajectory_proto->add_submap()->mutable_pose() =
-            transform::ToProto(GetSubmapTransform(submap_id));
+            transform::ToProto(submap_data.pose);
       }
     }
   }

cartographer/mapping/sparse_pose_graph.h

@@ -28,6 +28,7 @@
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
+#include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/transform/rigid_transform.h"
 
@@ -61,6 +62,11 @@ class SparsePoseGraph {
     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
   };
 
+  struct SubmapData {
+    std::shared_ptr<const Submap> submap;
+    transform::Rigid3d pose;
+  };
+
   SparsePoseGraph() {}
   virtual ~SparsePoseGraph() {}
 
@@ -80,8 +86,12 @@ class SparsePoseGraph {
   // Return the number of submaps for the given 'trajectory_id'.
   virtual int num_submaps(int trajectory_id) = 0;
 
-  // Returns the current optimized transform for the given 'submap_id'.
-  virtual transform::Rigid3d GetSubmapTransform(const SubmapId& submap_id) = 0;
+  // Returns the current optimized transform and submap itself for the given
+  // 'submap_id'.
+  virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
+
+  // Returns data for all Submaps by trajectory.
+  virtual std::vector<std::vector<SubmapData>> GetAllSubmapData() = 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

cartographer/mapping/trajectory_builder.h

@@ -53,19 +53,12 @@ class TrajectoryBuilder {
     sensor::PointCloud point_cloud;
   };
 
-  struct SubmapData {
-    std::shared_ptr<const Submap> submap;
-    transform::Rigid3d pose;
-  };
-
   TrajectoryBuilder() {}
   virtual ~TrajectoryBuilder() {}
 
   TrajectoryBuilder(const TrajectoryBuilder&) = delete;
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
-  virtual int num_submaps() = 0;
-  virtual SubmapData GetSubmapData(int submap_index) = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 
   virtual void AddSensorData(const string& sensor_id,

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -28,19 +28,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-int GlobalTrajectoryBuilder::num_submaps() {
-  return sparse_pose_graph_->num_submaps(trajectory_id_);
-}
-
-GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
-    const int submap_index) {
-  // TODO(hrapp): Get rid of this function and query the sparse pose graph
-  // directly.
-  const mapping::SubmapId submap_id{trajectory_id_, submap_index};
-  return {sparse_pose_graph_->GetSubmap(submap_id),
-          sparse_pose_graph_->GetSubmapTransform(submap_id)};
-}
-
 void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {

cartographer/mapping_2d/global_trajectory_builder.h

@@ -35,8 +35,6 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  int num_submaps() override;
-  SubmapData GetSubmapData(int submap_index) override;
   const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
       const override;
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -424,12 +424,6 @@ SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_.data();
 }
 
-std::shared_ptr<const Submap> SparsePoseGraph::GetSubmap(
-    const mapping::SubmapId& submap_id) {
-  common::MutexLocker locker(&mutex_);
-  return submap_data_.at(submap_id).submap;
-}
-
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;
@@ -455,24 +449,29 @@ int SparsePoseGraph::num_submaps(const int trajectory_id) {
   return submap_data_.num_indices(trajectory_id);
 }
 
-transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
+mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
     const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
-  // We already have an optimized pose.
-  if (submap_id.trajectory_id <
-          static_cast<int>(optimized_submap_transforms_.size()) &&
-      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
-                                                    .at(submap_id.trajectory_id)
-                                                    .size())) {
-    return transform::Embed3D(
-        optimized_submap_transforms_.at(submap_id.trajectory_id)
-            .at(submap_id.submap_index)
-            .pose);
+  return GetSubmapDataUnderLock(submap_id);
+}
+
+std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+SparsePoseGraph::GetAllSubmapData() {
+  common::MutexLocker locker(&mutex_);
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+      all_submap_data(submap_data_.num_trajectories());
+  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+       ++trajectory_id) {
+    all_submap_data[trajectory_id].reserve(
+        submap_data_.num_indices(trajectory_id));
+    for (int submap_index = 0;
+         submap_index < submap_data_.num_indices(trajectory_id);
+         ++submap_index) {
+      all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
+          mapping::SubmapId{trajectory_id, submap_index}));
+    }
   }
-  // We have to extrapolate.
-  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-                                       submap_id.trajectory_id) *
-         submap_data_.at(submap_id).submap->local_pose();
+  return all_submap_data;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@@ -493,6 +492,26 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
              .inverse();
 }
 
+mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
+    const mapping::SubmapId& submap_id) {
+  auto submap = submap_data_.at(submap_id).submap;
+  // We already have an optimized pose.
+  if (submap_id.trajectory_id <
+          static_cast<int>(optimized_submap_transforms_.size()) &&
+      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
+                                                    .at(submap_id.trajectory_id)
+                                                    .size())) {
+    return {submap, transform::Embed3D(
+                        optimized_submap_transforms_.at(submap_id.trajectory_id)
+                            .at(submap_id.submap_index)
+                            .pose)};
+  }
+  // We have to extrapolate.
+  return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
+                                                submap_id.trajectory_id) *
+                      submap->local_pose()};
+}
+
 SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
     : parent_(parent) {}
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -84,14 +84,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
-  transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
-      EXCLUDES(mutex_) override;
+  mapping::SparsePoseGraph::SubmapData GetSubmapData(
+      const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
-  std::shared_ptr<const Submap> GetSubmap(const mapping::SubmapId& submap_id)
-      EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:
@@ -154,11 +154,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
           submap_transforms,
       int trajectory_id) const REQUIRES(mutex_);
 
+  mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
+      const mapping::SubmapId& submap_id) REQUIRES(mutex_);
+
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;
 
   // If it exists, further scans must be added to this queue, and will be
-  // considered later.
+  // considered later
   std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
       GUARDED_BY(mutex_);
 

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -30,19 +30,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-int GlobalTrajectoryBuilder::num_submaps() {
-  return sparse_pose_graph_->num_submaps(trajectory_id_);
-}
-
-GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
-    const int submap_index) {
-  // TODO(hrapp): Get rid of this function and query the sparse pose graph
-  // directly.
-  const mapping::SubmapId submap_id{trajectory_id_, submap_index};
-  return {sparse_pose_graph_->GetSubmap(submap_id),
-          sparse_pose_graph_->GetSubmapTransform(submap_id)};
-}
-
 void GlobalTrajectoryBuilder::AddImuData(
     const common::Time time, const Eigen::Vector3d& linear_acceleration,
     const Eigen::Vector3d& angular_velocity) {

cartographer/mapping_3d/global_trajectory_builder.h

@@ -38,8 +38,6 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  int num_submaps() override;
-  SubmapData GetSubmapData(int submap_index) override;
   void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
                   const Eigen::Vector3d& angular_velocity) override;
   void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -424,12 +424,6 @@ SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_.data();
 }
 
-std::shared_ptr<const Submap> SparsePoseGraph::GetSubmap(
-    const mapping::SubmapId& submap_id) {
-  common::MutexLocker locker(&mutex_);
-  return submap_data_.at(submap_id).submap;
-}
-
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;
@@ -455,23 +449,29 @@ int SparsePoseGraph::num_submaps(const int trajectory_id) {
   return submap_data_.num_indices(trajectory_id);
 }
 
-transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
+mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
     const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
-  // We already have an optimized pose.
-  if (submap_id.trajectory_id <
-          static_cast<int>(optimized_submap_transforms_.size()) &&
-      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
-                                                    .at(submap_id.trajectory_id)
-                                                    .size())) {
-    return optimized_submap_transforms_.at(submap_id.trajectory_id)
-        .at(submap_id.submap_index)
-        .pose;
+  return GetSubmapDataUnderLock(submap_id);
+}
+
+std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+SparsePoseGraph::GetAllSubmapData() {
+  common::MutexLocker locker(&mutex_);
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+      all_submap_data(submap_data_.num_trajectories());
+  for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
+       ++trajectory_id) {
+    all_submap_data[trajectory_id].reserve(
+        submap_data_.num_indices(trajectory_id));
+    for (int submap_index = 0;
+         submap_index < submap_data_.num_indices(trajectory_id);
+         ++submap_index) {
+      all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
+          mapping::SubmapId{trajectory_id, submap_index}));
+    }
   }
-  // We have to extrapolate.
-  return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
-                                       submap_id.trajectory_id) *
-         submap_data_.at(submap_id).submap->local_pose();
+  return all_submap_data;
 }
 
 transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@@ -492,5 +492,24 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
              .inverse();
 }
 
+mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
+    const mapping::SubmapId& submap_id) {
+  auto submap = submap_data_.at(submap_id).submap;
+  // We already have an optimized pose.
+  if (submap_id.trajectory_id <
+          static_cast<int>(optimized_submap_transforms_.size()) &&
+      submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
+                                                    .at(submap_id.trajectory_id)
+                                                    .size())) {
+    return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id)
+                        .at(submap_id.submap_index)
+                        .pose};
+  }
+  // We have to extrapolate.
+  return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
+                                                submap_id.trajectory_id) *
+                      submap->local_pose()};
+}
+
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/sparse_pose_graph.h

@@ -83,14 +83,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void RunFinalOptimization() override;
   std::vector<std::vector<int>> GetConnectedTrajectories() override;
   int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
-  transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
-      EXCLUDES(mutex_) override;
+  mapping::SparsePoseGraph::SubmapData GetSubmapData(
+      const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
+  std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
+  GetAllSubmapData() EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
-  std::shared_ptr<const Submap> GetSubmap(const mapping::SubmapId& submap_id)
-      EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:
@@ -153,6 +153,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
           submap_transforms,
       int trajectory_id) const REQUIRES(mutex_);
 
+  mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
+      const mapping::SubmapId& submap_id) REQUIRES(mutex_);
+
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;