Serialize submaps. (#380)

cartographer/mapping/map_builder.cc

@@ -135,7 +135,24 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
 void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
   // We serialize the pose graph followed by all the data referenced in it.
   writer->WriteProto(sparse_pose_graph_->ToProto());
-  // TODO(whess): Serialize submaps.
+  // Next we serialize all submap data.
+  const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
+  for (int trajectory_id = 0;
+       trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) {
+    for (int submap_index = 0;
+         submap_index != static_cast<int>(submap_data[trajectory_id].size());
+         ++submap_index) {
+      proto::SerializedData proto;
+      auto* const submap_proto = proto.mutable_submap();
+      // TODO(whess): Handle trimmed data.
+      submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id);
+      submap_proto->mutable_submap_id()->set_submap_index(submap_index);
+      submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto);
+      // TODO(whess): Only enable optionally? Resulting pbstream files will be
+      // a lot larger now.
+      writer->WriteProto(proto);
+    }
+  }
   // TODO(whess): Serialize range data ("scan") for each trajectory node.
   // TODO(whess): Serialize additional sensor data: IMU, odometry.
 }

cartographer/mapping/proto/serialization.proto

@@ -0,0 +1,31 @@
+// Copyright 2016 The Cartographer Authors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+syntax = "proto2";
+
+package cartographer.mapping.proto;
+
+import "cartographer/mapping/proto/sparse_pose_graph.proto";
+import "cartographer/mapping/proto/submap.proto";
+
+message Submap {
+  optional SubmapId submap_id = 1;
+  optional Submap2D submap_2d = 2;
+  optional Submap3D submap_3d = 3;
+}
+
+message SerializedData {
+  optional Submap submap = 1;
+  // TODO(whess): Add range data, IMU data, odometry.
+}

cartographer/mapping/proto/sparse_pose_graph.proto

@@ -19,18 +19,18 @@ package cartographer.mapping.proto;
 import "cartographer/mapping/proto/trajectory.proto";
 import "cartographer/transform/proto/transform.proto";
 
-message SparsePoseGraph {
+message SubmapId {
-  message Constraint {
-    message SubmapId {
   optional int32 trajectory_id = 1;
   optional int32 submap_index = 2;  // Submap index in the given trajectory.
-    }
+}
 
-    message ScanId {
+message ScanId {
   optional int32 trajectory_id = 1;
   optional int32 scan_index = 2;  // Scan index in the given trajectory.
-    }
+}
 
+message SparsePoseGraph {
+  message Constraint {
     // Differentiates between intra-submap (where the scan was inserted into the
     // submap) and inter-submap constraints (where the scan was not inserted
     // into the submap).

cartographer/mapping/proto/submap.proto

@@ -16,15 +16,23 @@ syntax = "proto2";
 
 package cartographer.mapping.proto;
 
-import "cartographer/transform/proto/transform.proto";
 import "cartographer/mapping_2d/proto/probability_grid.proto";
 import "cartographer/mapping_3d/proto/hybrid_grid.proto";
+import "cartographer/transform/proto/transform.proto";
 
-message Submap {
+// Serialized state of a mapping_2d::Submap.
+message Submap2D {
   optional transform.proto.Rigid3d local_pose = 1;
   optional int32 num_range_data = 2;
   optional bool finished = 3;
   optional mapping_2d.proto.ProbabilityGrid probability_grid = 4;
-  optional mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 5;
+}
-  optional mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 6;
+
+// Serialized state of a mapping_3d::Submap.
+message Submap3D {
+  optional transform.proto.Rigid3d local_pose = 1;
+  optional int32 num_range_data = 2;
+  optional bool finished = 3;
+  optional mapping_3d.proto.HybridGrid high_resolution_hybrid_grid = 4;
+  optional mapping_3d.proto.HybridGrid low_resolution_hybrid_grid = 5;
 }

cartographer/mapping/submaps.h

@@ -25,6 +25,7 @@
 #include "cartographer/common/port.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/probability_values.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/mapping_2d/probability_grid.h"
@@ -61,6 +62,8 @@ class Submap {
   Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {}
   virtual ~Submap() {}
 
+  virtual void ToProto(proto::Submap* proto) const = 0;
+
   // Local SLAM pose of this submap.
   transform::Rigid3d local_pose() const { return local_pose_; }
 
@@ -73,7 +76,9 @@ class Submap {
       proto::SubmapQuery::Response* response) const = 0;
 
  protected:
-  void SetNumRangeData(int num_range_data) { num_range_data_ = num_range_data; }
+  void SetNumRangeData(const int num_range_data) {
+    num_range_data_ = num_range_data;
+  }
 
  private:
   const transform::Rigid3d local_pose_;

cartographer/mapping_2d/submaps.cc

@@ -69,20 +69,19 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
           Eigen::Vector3d(origin.x(), origin.y(), 0.))),
       probability_grid_(limits) {}
 
-Submap::Submap(const mapping::proto::Submap& proto)
+Submap::Submap(const mapping::proto::Submap2D& proto)
     : mapping::Submap(transform::ToRigid3(proto.local_pose())),
       probability_grid_(ProbabilityGrid(proto.probability_grid())) {
   SetNumRangeData(proto.num_range_data());
   finished_ = proto.finished();
 }
 
-mapping::proto::Submap Submap::ToProto() const {
+void Submap::ToProto(mapping::proto::Submap* const proto) const {
-  mapping::proto::Submap proto;
+  auto* const submap_2d = proto->mutable_submap_2d();
-  *proto.mutable_local_pose() = transform::ToProto(local_pose());
+  *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
-  proto.set_num_range_data(num_range_data());
+  submap_2d->set_num_range_data(num_range_data());
-  proto.set_finished(finished_);
+  submap_2d->set_finished(finished_);
-  *proto.mutable_probability_grid() = probability_grid_.ToProto();
+  *submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
-  return proto;
 }
 
 void Submap::ToResponseProto(

cartographer/mapping_2d/submaps.h

@@ -22,7 +22,7 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/mapping/proto/submap.pb.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
@@ -45,9 +45,9 @@ proto::SubmapsOptions CreateSubmapsOptions(
 class Submap : public mapping::Submap {
  public:
   Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
-  explicit Submap(const mapping::proto::Submap& proto);
+  explicit Submap(const mapping::proto::Submap2D& proto);
 
-  mapping::proto::Submap ToProto() const;
+  void ToProto(mapping::proto::Submap* proto) const override;
 
   const ProbabilityGrid& probability_grid() const { return probability_grid_; }
   bool finished() const { return finished_; }

cartographer/mapping_2d/submaps_test.cc

@@ -71,7 +71,11 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
 TEST(SubmapsTest, ToFromProto) {
   Submap expected(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
                   Eigen::Vector2f(4.f, 5.f));
-  const auto actual = Submap(expected.ToProto());
+  mapping::proto::Submap proto;
+  expected.ToProto(&proto);
+  EXPECT_TRUE(proto.has_submap_2d());
+  EXPECT_FALSE(proto.has_submap_3d());
+  const auto actual = Submap(proto.submap_2d());
   EXPECT_TRUE(expected.local_pose().translation().isApprox(
       actual.local_pose().translation(), 1e-6));
   EXPECT_TRUE(expected.local_pose().rotation().isApprox(

cartographer/mapping_3d/submaps.cc

@@ -326,7 +326,7 @@ Submap::Submap(const float high_resolution, const float low_resolution,
       high_resolution_hybrid_grid_(high_resolution),
       low_resolution_hybrid_grid_(low_resolution) {}
 
-Submap::Submap(const mapping::proto::Submap& proto)
+Submap::Submap(const mapping::proto::Submap3D& proto)
     : mapping::Submap(transform::ToRigid3(proto.local_pose())),
       high_resolution_hybrid_grid_(proto.high_resolution_hybrid_grid()),
       low_resolution_hybrid_grid_(proto.low_resolution_hybrid_grid()) {
@@ -334,16 +334,15 @@ Submap::Submap(const mapping::proto::Submap& proto)
   finished_ = proto.finished();
 }
 
-mapping::proto::Submap Submap::ToProto() const {
+void Submap::ToProto(mapping::proto::Submap* const proto) const {
-  mapping::proto::Submap proto;
+  auto* const submap_3d = proto->mutable_submap_3d();
-  *proto.mutable_local_pose() = transform::ToProto(local_pose());
+  *submap_3d->mutable_local_pose() = transform::ToProto(local_pose());
-  proto.set_num_range_data(num_range_data());
+  submap_3d->set_num_range_data(num_range_data());
-  proto.set_finished(finished_);
+  submap_3d->set_finished(finished_);
-  *proto.mutable_high_resolution_hybrid_grid() =
+  *submap_3d->mutable_high_resolution_hybrid_grid() =
       mapping_3d::ToProto(high_resolution_hybrid_grid());
-  *proto.mutable_low_resolution_hybrid_grid() =
+  *submap_3d->mutable_low_resolution_hybrid_grid() =
       mapping_3d::ToProto(low_resolution_hybrid_grid());
-  return proto;
 }
 
 void Submap::ToResponseProto(

cartographer/mapping_3d/submaps.h

@@ -24,7 +24,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/port.h"
 #include "cartographer/mapping/id.h"
-#include "cartographer/mapping/proto/submap.pb.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping_2d/probability_grid.h"
@@ -52,9 +52,9 @@ class Submap : public mapping::Submap {
  public:
   Submap(float high_resolution, float low_resolution,
          const transform::Rigid3d& local_pose);
-  explicit Submap(const mapping::proto::Submap& proto);
+  explicit Submap(const mapping::proto::Submap3D& proto);
 
-  mapping::proto::Submap ToProto() const;
+  void ToProto(mapping::proto::Submap* proto) const override;
 
   const HybridGrid& high_resolution_hybrid_grid() const {
     return high_resolution_hybrid_grid_;

cartographer/mapping_3d/submaps_test.cc

@@ -27,7 +27,11 @@ TEST(SubmapsTest, ToFromProto) {
   const Submap expected(0.05, 0.25,
                         transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
                                            Eigen::Quaterniond(0., 0., 0., 1.)));
-  const auto actual = Submap(expected.ToProto());
+  mapping::proto::Submap proto;
+  expected.ToProto(&proto);
+  EXPECT_FALSE(proto.has_submap_2d());
+  EXPECT_TRUE(proto.has_submap_3d());
+  const auto actual = Submap(proto.submap_3d());
   EXPECT_TRUE(expected.local_pose().translation().isApprox(
       actual.local_pose().translation(), 1e-6));
   EXPECT_TRUE(expected.local_pose().rotation().isApprox(