Submap serialization support. (#376)

cartographer/mapping/proto/submap.proto

@@ -0,0 +1,30 @@
+// 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/transform/proto/transform.proto";
+import "cartographer/mapping_2d/proto/probability_grid.proto";
+import "cartographer/mapping_3d/proto/hybrid_grid.proto";
+
+message Submap {
+  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;
+}

cartographer/mapping/submaps.h

@@ -72,14 +72,13 @@ class Submap {
       const transform::Rigid3d& global_submap_pose,
       proto::SubmapQuery::Response* response) const = 0;
 
+ protected:
+  void SetNumRangeData(int num_range_data) { num_range_data_ = num_range_data; }
+
  private:
   const transform::Rigid3d local_pose_;
-
- protected:
-  // TODO(hrapp): This should be private.
   int num_range_data_ = 0;
 };
-
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping_2d/submaps.cc

@@ -69,6 +69,22 @@ 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)
+    : 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 {
+  mapping::proto::Submap proto;
+  *proto.mutable_local_pose() = transform::ToProto(local_pose());
+  proto.set_num_range_data(num_range_data());
+  proto.set_finished(finished_);
+  *proto.mutable_probability_grid() = probability_grid_.ToProto();
+  return proto;
+}
+
 void Submap::ToResponseProto(
     const transform::Rigid3d&,
     mapping::proto::SubmapQuery::Response* const response) const {
@@ -119,7 +135,7 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data,
                              const RangeDataInserter& range_data_inserter) {
   CHECK(!finished_);
   range_data_inserter.Insert(range_data, &probability_grid_);
-  ++num_range_data_;
+  SetNumRangeData(num_range_data() + 1);
 }
 
 void Submap::Finish() {

cartographer/mapping_2d/submaps.h

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

cartographer/mapping_2d/submaps_test.cc

@@ -68,6 +68,24 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
   EXPECT_EQ(correct_num_scans, all_submaps.size() - 2);
 }
 
+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());
+  EXPECT_TRUE(expected.local_pose().translation().isApprox(
+      actual.local_pose().translation(), 1e-6));
+  EXPECT_TRUE(expected.local_pose().rotation().isApprox(
+      actual.local_pose().rotation(), 1e-6));
+  EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
+  EXPECT_EQ(expected.finished(), actual.finished());
+  EXPECT_NEAR(expected.probability_grid().limits().resolution(),
+              actual.probability_grid().limits().resolution(), 1e-6);
+  EXPECT_TRUE(expected.probability_grid().limits().max().isApprox(
+      actual.probability_grid().limits().max(), 1e-6));
+  EXPECT_EQ(expected.probability_grid().limits().cell_limits().num_x_cells,
+            actual.probability_grid().limits().cell_limits().num_x_cells);
+}
+
 }  // namespace
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_3d/submaps.cc

@@ -326,6 +326,26 @@ 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)
+    : 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()) {
+  SetNumRangeData(proto.num_range_data());
+  finished_ = proto.finished();
+}
+
+mapping::proto::Submap Submap::ToProto() const {
+  mapping::proto::Submap proto;
+  *proto.mutable_local_pose() = transform::ToProto(local_pose());
+  proto.set_num_range_data(num_range_data());
+  proto.set_finished(finished_);
+  *proto.mutable_high_resolution_hybrid_grid() =
+      mapping_3d::ToProto(high_resolution_hybrid_grid());
+  *proto.mutable_low_resolution_hybrid_grid() =
+      mapping_3d::ToProto(low_resolution_hybrid_grid());
+  return proto;
+}
+
 void Submap::ToResponseProto(
     const transform::Rigid3d& global_submap_pose,
     mapping::proto::SubmapQuery::Response* const response) const {
@@ -372,7 +392,7 @@ void Submap::InsertRangeData(const sensor::RangeData& range_data,
       &high_resolution_hybrid_grid_);
   range_data_inserter.Insert(transformed_range_data,
                              &low_resolution_hybrid_grid_);
-  ++num_range_data_;
+  SetNumRangeData(num_range_data() + 1);
 }
 
 void Submap::Finish() {

cartographer/mapping_3d/submaps.h

@@ -24,6 +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/submap_visualization.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping_2d/probability_grid.h"
@@ -51,6 +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);
+
+  mapping::proto::Submap ToProto() const;
 
   const HybridGrid& high_resolution_hybrid_grid() const {
     return high_resolution_hybrid_grid_;
@@ -58,7 +62,7 @@ class Submap : public mapping::Submap {
   const HybridGrid& low_resolution_hybrid_grid() const {
     return low_resolution_hybrid_grid_;
   }
-  const bool finished() const { return finished_; }
+  bool finished() const { return finished_; }
 
   void ToResponseProto(
       const transform::Rigid3d& global_submap_pose,

cartographer/mapping_3d/submaps_test.cc

@@ -0,0 +1,43 @@
+/*
+ * 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.
+ */
+
+#include "cartographer/mapping_3d/submaps.h"
+
+#include "cartographer/transform/transform.h"
+#include "gmock/gmock.h"
+
+namespace cartographer {
+namespace mapping_3d {
+namespace {
+
+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());
+  EXPECT_TRUE(expected.local_pose().translation().isApprox(
+      actual.local_pose().translation(), 1e-6));
+  EXPECT_TRUE(expected.local_pose().rotation().isApprox(
+      actual.local_pose().rotation(), 1e-6));
+  EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
+  EXPECT_EQ(expected.finished(), actual.finished());
+  EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6);
+  EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6);
+}
+
+}  // namespace
+}  // namespace mapping_3d
+}  // namespace cartographer