Adds probability grid serialization to proto (#208)

cartographer/mapping_2d/map_limits.h

@@ -23,8 +23,9 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
-#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/mapping_2d/proto/map_limits.pb.h"
 #include "cartographer/mapping_2d/xy_index.h"
+#include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/sensor/laser.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -46,6 +47,11 @@ class MapLimits {
     CHECK_GT(cell_limits.num_y_cells, 0.);
   }
 
+  explicit MapLimits(const proto::MapLimits& map_limits)
+      : resolution_(map_limits.resolution()),
+        max_(transform::ToEigen(map_limits.max())),
+        cell_limits_(map_limits.cell_limits()) {}
+
   // Returns the cell size in meters. All cells are square and the resolution is
   // the length of one side.
   double resolution() const { return resolution_; }
@@ -73,9 +79,8 @@ class MapLimits {
   // Returns true of the ProbabilityGrid contains 'xy_index'.
   bool Contains(const Eigen::Array2i& xy_index) const {
     return (Eigen::Array2i(0, 0) <= xy_index).all() &&
-           (xy_index <
+           (xy_index < Eigen::Array2i(cell_limits_.num_x_cells,
-            Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells))
+                                      cell_limits_.num_y_cells)).all();
-               .all();
   }
 
   // Computes MapLimits that contain the origin, and all laser rays (both
@@ -129,6 +134,14 @@ class MapLimits {
   CellLimits cell_limits_;
 };
 
+inline proto::MapLimits ToProto(const MapLimits& map_limits) {
+  proto::MapLimits result;
+  result.set_resolution(map_limits.resolution());
+  *result.mutable_max() = transform::ToProto(map_limits.max());
+  *result.mutable_cell_limits() = ToProto(map_limits.cell_limits());
+  return result;
+}
+
 }  // namespace mapping_2d
 }  // namespace cartographer
 

cartographer/mapping_2d/map_limits_test.cc

@@ -22,6 +22,32 @@ namespace cartographer {
 namespace mapping_2d {
 namespace {
 
+TEST(MapLimitsTest, ToProto) {
+  const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
+  const auto proto = ToProto(limits);
+  EXPECT_EQ(limits.resolution(), proto.resolution());
+  EXPECT_EQ(limits.max().x(), proto.max().x());
+  EXPECT_EQ(limits.max().y(), proto.max().y());
+  EXPECT_EQ(ToProto(limits.cell_limits()).DebugString(),
+            proto.cell_limits().DebugString());
+}
+
+TEST(MapLimitsTest, ProtoConstructor) {
+  proto::MapLimits limits;
+  limits.set_resolution(1.);
+  limits.mutable_max()->set_x(2.);
+  limits.mutable_max()->set_y(3.);
+  limits.mutable_cell_limits()->set_num_x_cells(4);
+  limits.mutable_cell_limits()->set_num_y_cells(5);
+
+  const MapLimits native(limits);
+  EXPECT_EQ(limits.resolution(), native.resolution());
+  EXPECT_EQ(limits.max().x(), native.max().x());
+  EXPECT_EQ(limits.max().y(), native.max().y());
+  EXPECT_EQ(limits.cell_limits().DebugString(),
+            ToProto(native.cell_limits()).DebugString());
+}
+
 TEST(MapLimitsTest, ConstructAndGet) {
   const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
 

cartographer/mapping_2d/probability_grid.h

@@ -27,9 +27,10 @@
 
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
-#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/map_limits.h"
+#include "cartographer/mapping_2d/proto/probability_grid.pb.h"
 #include "cartographer/mapping_2d/xy_index.h"
+#include "cartographer/mapping/probability_values.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -48,6 +49,22 @@ class ProbabilityGrid {
         min_x_(limits_.cell_limits().num_x_cells - 1),
         min_y_(limits_.cell_limits().num_y_cells - 1) {}
 
+  explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
+      : limits_(proto.limits()),
+        cells_(),
+        update_indices_(proto.update_indices().begin(),
+                        proto.update_indices().end()),
+        max_x_(proto.max_x()),
+        max_y_(proto.max_y()),
+        min_x_(proto.min_x()),
+        min_y_(proto.min_y()) {
+    cells_.reserve(proto.cells_size());
+    for (const auto cell : proto.cells()) {
+      CHECK_LE(cell, std::numeric_limits<uint16>::max());
+      cells_.push_back(cell);
+    }
+  }
+
   // Returns the limits of this ProbabilityGrid.
   const MapLimits& limits() const { return limits_; }
 
@@ -106,8 +123,9 @@ class ProbabilityGrid {
 
   // Returns true if the probability at the specified index is known.
   bool IsKnown(const Eigen::Array2i& xy_index) const {
-    return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
+    return limits_.Contains(xy_index) &&
-                                             mapping::kUnknownProbabilityValue;
+           cells_[GetIndexOfCell(xy_index)] !=
+               mapping::kUnknownProbabilityValue;
   }
 
   // Fills in 'offset' and 'limits' to define a subregion of that contains all
@@ -154,6 +172,24 @@ class ProbabilityGrid {
     }
   }
 
+  proto::ProbabilityGrid ToProto() {
+    proto::ProbabilityGrid result;
+    *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
+    result.mutable_cells()->Reserve(cells_.size());
+    for (const auto cell : cells_) {
+      result.mutable_cells()->Add(cell);
+    }
+    result.mutable_update_indices()->Reserve(update_indices_.size());
+    for (const auto update : update_indices_) {
+      result.mutable_update_indices()->Add(update);
+    }
+    result.set_max_x(max_x_);
+    result.set_max_y(max_y_);
+    result.set_min_x(min_x_);
+    result.set_min_y(min_y_);
+    return result;
+  }
+
  private:
   // Returns the index of the cell at 'xy_index'.
   int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
@@ -172,7 +208,7 @@ class ProbabilityGrid {
   std::vector<uint16> cells_;  // Highest bit is update marker.
   std::vector<int> update_indices_;
 
-  // Minimum and maximum cell coordinates of know cells to efficiently compute
+  // Minimum and maximum cell coordinates of known cells to efficiently compute
   // cropping limits.
   int max_x_;
   int max_y_;

cartographer/mapping_2d/probability_grid_test.cc

@@ -17,6 +17,7 @@
 #include "cartographer/mapping_2d/probability_grid.h"
 
 #include <random>
+#include <vector>
 
 #include "gtest/gtest.h"
 
@@ -24,6 +25,40 @@ namespace cartographer {
 namespace mapping_2d {
 namespace {
 
+TEST(ProbabilityGridTest, ProtoConstructor) {
+  proto::ProbabilityGrid proto;
+  const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
+  *proto.mutable_limits() = ToProto(limits);
+  for (int i = 6; i < 12; ++i) {
+    proto.mutable_cells()->Add(static_cast<uint16>(i));
+  }
+  for (int i = 13; i < 18; ++i) {
+    proto.mutable_update_indices()->Add(i);
+  }
+  proto.set_max_x(19);
+  proto.set_max_y(20);
+  proto.set_min_x(21);
+  proto.set_min_y(22);
+
+  ProbabilityGrid grid(proto);
+  EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
+
+  // TODO(macmason): Figure out how to test the contents of cells_,
+  // update_indices_, and {min, max}_{x, y}_ gracefully.
+}
+
+TEST(ProbabilityGridTest, ToProto) {
+  ProbabilityGrid probability_grid(
+      MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
+
+  const auto proto = probability_grid.ToProto();
+  EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
+            proto.limits().DebugString());
+
+  // TODO(macmason): Figure out how to test the contents of cells_,
+  // update_indices_, and {min, max}_{x, y}_ gracefully.
+}
+
 TEST(ProbabilityGridTest, ApplyOdds) {
   ProbabilityGrid probability_grid(
       MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
@@ -111,35 +146,26 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
   const CellLimits& cell_limits = limits.cell_limits();
   ASSERT_EQ(14, cell_limits.num_x_cells);
   ASSERT_EQ(8, cell_limits.num_y_cells);
-  EXPECT_TRUE(
+  EXPECT_TRUE((Eigen::Array2i(0, 0) ==
-      (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13))
+               limits.GetXYIndexOfCellContainingPoint(7, 13)).all());
-          .all());
+  EXPECT_TRUE((Eigen::Array2i(13, 0) ==
-  EXPECT_TRUE(
+               limits.GetXYIndexOfCellContainingPoint(7, -13)).all());
-      (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13))
+  EXPECT_TRUE((Eigen::Array2i(0, 7) ==
-          .all());
+               limits.GetXYIndexOfCellContainingPoint(-7, 13)).all());
-  EXPECT_TRUE(
+  EXPECT_TRUE((Eigen::Array2i(13, 7) ==
-      (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13))
+               limits.GetXYIndexOfCellContainingPoint(-7, -13)).all());
-          .all());
-  EXPECT_TRUE(
-      (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13))
-          .all());
 
   // Check around the origin.
-  EXPECT_TRUE(
+  EXPECT_TRUE((Eigen::Array2i(6, 3) ==
-      (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5))
+               limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)).all());
-          .all());
+  EXPECT_TRUE((Eigen::Array2i(6, 3) ==
-  EXPECT_TRUE(
+               limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)).all());
-      (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5))
-          .all());
   EXPECT_TRUE((Eigen::Array2i(7, 3) ==
-               limits.GetXYIndexOfCellContainingPoint(0.5, -0.5))
+               limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)).all());
-                  .all());
   EXPECT_TRUE((Eigen::Array2i(6, 4) ==
-               limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5))
+               limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)).all());
-                  .all());
   EXPECT_TRUE((Eigen::Array2i(7, 4) ==
-               limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5))
+               limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)).all());
-                  .all());
 }
 
 TEST(ProbabilityGridTest, CorrectCropping) {

cartographer/mapping_2d/proto/cell_limits.proto

@@ -0,0 +1,22 @@
+// 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_2d.proto;
+
+message CellLimits {
+  optional int32 num_x_cells = 1;
+  optional int32 num_y_cells = 2;
+}

cartographer/mapping_2d/proto/map_limits.proto

@@ -0,0 +1,26 @@
+// 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";
+
+import "cartographer/mapping_2d/proto/cell_limits.proto";
+import "cartographer/transform/proto/transform.proto";
+
+package cartographer.mapping_2d.proto;
+
+message MapLimits {
+  optional double resolution = 1;
+  optional cartographer.transform.proto.Vector2d max = 2;
+  optional CellLimits cell_limits = 3;
+}

cartographer/mapping_2d/proto/probability_grid.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";
+
+import "cartographer/mapping_2d/proto/map_limits.proto";
+
+package cartographer.mapping_2d.proto;
+
+message ProbabilityGrid {
+  optional MapLimits limits = 1;
+  // These values are actually int16s, but protos don't have a native int16 type.
+  repeated int32 cells = 2;
+  repeated int32 update_indices = 3;
+  optional int32 max_x = 4;
+  optional int32 max_y = 5;
+  optional int32 min_x = 6;
+  optional int32 min_y = 7;
+}
+

cartographer/mapping_2d/xy_index.h

@@ -25,6 +25,7 @@
 #include "Eigen/Core"
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping_2d/proto/cell_limits.pb.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -35,10 +36,21 @@ struct CellLimits {
   CellLimits(int init_num_x_cells, int init_num_y_cells)
       : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {}
 
+  explicit CellLimits(const proto::CellLimits& cell_limits)
+      : num_x_cells(cell_limits.num_x_cells()),
+        num_y_cells(cell_limits.num_y_cells()) {}
+
   int num_x_cells = 0;
   int num_y_cells = 0;
 };
 
+inline proto::CellLimits ToProto(const CellLimits& cell_limits) {
+  proto::CellLimits result;
+  result.set_num_x_cells(cell_limits.num_x_cells);
+  result.set_num_y_cells(cell_limits.num_y_cells);
+  return result;
+}
+
 // Iterates in row-major order through a range of xy-indices.
 class XYIndexRangeIterator
     : public std::iterator<std::input_iterator_tag, Eigen::Array2i> {

cartographer/mapping_2d/xy_index_test.cc

@@ -22,6 +22,23 @@ namespace cartographer {
 namespace mapping_2d {
 namespace {
 
+TEST(XYIndexTest, CellLimitsToProto) {
+  const CellLimits limits(1, 2);
+  const auto proto = ToProto(limits);
+  EXPECT_EQ(limits.num_x_cells, proto.num_x_cells());
+  EXPECT_EQ(limits.num_y_cells, proto.num_y_cells());
+}
+
+TEST(XYIndexTest, CellLimitsProtoConstructor) {
+  proto::CellLimits limits;
+  limits.set_num_x_cells(1);
+  limits.set_num_y_cells(2);
+
+  auto native = CellLimits(limits);
+  EXPECT_EQ(limits.num_x_cells(), native.num_x_cells);
+  EXPECT_EQ(limits.num_y_cells(), native.num_y_cells);
+}
+
 TEST(XYIndexTest, XYIndexRangeIterator) {
   const Eigen::Array2i min(1, 2);
   const Eigen::Array2i max(3, 4);

cartographer/transform/transform.cc

@@ -76,6 +76,13 @@ proto::Rigid3f ToProto(const transform::Rigid3f& rigid) {
   return proto;
 }
 
+proto::Vector2d ToProto(const Eigen::Vector2d& vector) {
+  proto::Vector2d proto;
+  proto.set_x(vector.x());
+  proto.set_y(vector.y());
+  return proto;
+}
+
 proto::Vector3f ToProto(const Eigen::Vector3f& vector) {
   proto::Vector3f proto;
   proto.set_x(vector.x());

cartographer/transform/transform.h

@@ -125,6 +125,7 @@ proto::Rigid2f ToProto(const Rigid2f& transform);
 proto::Rigid3d ToProto(const Rigid3d& rigid);
 Rigid3d ToRigid3(const proto::Rigid3d& rigid);
 proto::Rigid3f ToProto(const Rigid3f& rigid);
+proto::Vector2d ToProto(const Eigen::Vector2d& vector);
 proto::Vector3f ToProto(const Eigen::Vector3f& vector);
 proto::Vector3d ToProto(const Eigen::Vector3d& vector);
 proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion);