Remove direct access to Submaps from the outside. (#339)

PAIR=SirVer

cartographer/mapping/collated_trajectory_builder.cc

@@ -46,8 +46,13 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
 
 CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
 
-Submaps* CollatedTrajectoryBuilder::submaps() {
+int CollatedTrajectoryBuilder::num_submaps() {
-  return wrapped_trajectory_builder_->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&

cartographer/mapping/collated_trajectory_builder.h

@@ -49,7 +49,8 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
   CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
       delete;
 
-  Submaps* submaps() override;
+  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

@@ -20,12 +20,14 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
+#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping {
@@ -37,6 +39,7 @@ namespace mapping {
 class GlobalTrajectoryBuilderInterface {
  public:
   using PoseEstimate = TrajectoryBuilder::PoseEstimate;
+  using SubmapData = TrajectoryBuilder::SubmapData;
 
   GlobalTrajectoryBuilderInterface() {}
   virtual ~GlobalTrajectoryBuilderInterface() {}
@@ -46,7 +49,8 @@ class GlobalTrajectoryBuilderInterface {
   GlobalTrajectoryBuilderInterface& operator=(
       const GlobalTrajectoryBuilderInterface&) = delete;
 
-  virtual Submaps* submaps() = 0;
+  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

@@ -113,7 +113,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " trajectories.";
   }
 
-  const int num_submaps = sparse_pose_graph_->num_submaps(trajectory_id);
+  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) +
@@ -121,12 +121,10 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " submaps in this trajectory.";
   }
 
-  const Submap* const submap =
+  const auto submap_data =
-      trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
+      trajectory_builders_.at(trajectory_id)->GetSubmapData(submap_index);
-  response->set_submap_version(submap->num_range_data());
+  CHECK(submap_data.submap != nullptr);
-  const auto submap_pose = sparse_pose_graph_->GetSubmapTransform(
+  submap_data.submap->ToResponseProto(submap_data.pose, response);
-      SubmapId{trajectory_id, submap_index});
-  submap->ToResponseProto(submap_pose, response);
   return "";
 }
 

cartographer/mapping/trajectory_builder.h

@@ -53,13 +53,19 @@ class TrajectoryBuilder {
     sensor::PointCloud point_cloud;
   };
 
+  struct SubmapData {
+    const Submap* submap;
+    transform::Rigid3d pose;
+  };
+
   TrajectoryBuilder() {}
   virtual ~TrajectoryBuilder() {}
 
   TrajectoryBuilder(const TrajectoryBuilder&) = delete;
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
-  virtual Submaps* submaps() = 0;
+  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/trajectory_node.h

@@ -28,8 +28,6 @@
 namespace cartographer {
 namespace mapping {
 
-class Submaps;
-
 struct TrajectoryNode {
   struct Data {
     common::Time time;

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -28,8 +28,15 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-Submaps* GlobalTrajectoryBuilder::submaps() {
+int GlobalTrajectoryBuilder::num_submaps() {
-  return local_trajectory_builder_.submaps();
+  return sparse_pose_graph_->num_submaps(trajectory_id_);
+}
+
+GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
+    const int submap_index) {
+  return {local_trajectory_builder_.submaps()->Get(submap_index),
+          sparse_pose_graph_->GetSubmapTransform(
+              mapping::SubmapId{trajectory_id_, submap_index})};
 }
 
 void GlobalTrajectoryBuilder::AddRangefinderData(

cartographer/mapping_2d/global_trajectory_builder.h

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

cartographer/mapping_2d/submaps.cc

@@ -108,6 +108,8 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
 void Submap::ToResponseProto(
     const transform::Rigid3d&,
     mapping::proto::SubmapQuery::Response* const response) const {
+  response->set_submap_version(num_range_data());
+
   Eigen::Array2i offset;
   CellLimits limits;
   probability_grid_.ComputeCroppedLimits(&offset, &limits);

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -30,8 +30,15 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
 
 GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
 
-mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
+int GlobalTrajectoryBuilder::num_submaps() {
-  return local_trajectory_builder_->submaps();
+  return sparse_pose_graph_->num_submaps(trajectory_id_);
+}
+
+GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
+    const int submap_index) {
+  return {local_trajectory_builder_->submaps()->Get(submap_index),
+          sparse_pose_graph_->GetSubmapTransform(
+              mapping::SubmapId{trajectory_id_, submap_index})};
 }
 
 void GlobalTrajectoryBuilder::AddImuData(

cartographer/mapping_3d/global_trajectory_builder.h

@@ -38,7 +38,8 @@ class GlobalTrajectoryBuilder
   GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
   GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
 
-  mapping_3d::Submaps* submaps() override;
+  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/submaps.cc

@@ -329,6 +329,7 @@ Submap::Submap(const float high_resolution, const float low_resolution,
 void Submap::ToResponseProto(
     const transform::Rigid3d& global_submap_pose,
     mapping::proto::SubmapQuery::Response* const response) const {
+  response->set_submap_version(num_range_data());
   // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
   // in the global map frame.
   const float resolution = high_resolution_hybrid_grid_.resolution();