Adds HybridGrid proto serialization (#220)

cartographer/mapping_3d/hybrid_grid.h

@@ -26,7 +26,9 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping_3d/proto/hybrid_grid.pb.h"
 #include "cartographer/mapping/probability_values.h"
+#include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -468,6 +470,20 @@ class HybridGrid : public HybridGridBase<uint16> {
   HybridGrid(const float resolution, const Eigen::Vector3f& origin)
       : HybridGridBase<uint16>(resolution, origin) {}
 
+  HybridGrid(const proto::HybridGrid& proto)
+      : HybridGrid(proto.resolution(), transform::ToEigen(proto.origin())) {
+    CHECK_EQ(proto.values_size(), proto.x_indices_size());
+    CHECK_EQ(proto.values_size(), proto.y_indices_size());
+    CHECK_EQ(proto.values_size(), proto.z_indices_size());
+
+    for (int i = 0; i < proto.values_size(); ++i) {
+      // SetProbability does some error checking for us.
+      SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i),
+                                     proto.z_indices(i)),
+                     mapping::ValueToProbability(proto.values(i)));
+    }
+  }
+
   // Sets the probability of the cell at 'index' to the given 'probability'.
   void SetProbability(const Eigen::Array3i& index, const float probability) {
     *mutable_value(index) = mapping::ProbabilityToValue(probability);
@@ -515,6 +531,19 @@ class HybridGrid : public HybridGridBase<uint16> {
   std::vector<ValueType*> update_indices_;
 };
 
+inline proto::HybridGrid ToProto(const HybridGrid& grid) {
+  proto::HybridGrid result;
+  result.set_resolution(grid.resolution());
+  *result.mutable_origin() = transform::ToProto(grid.origin());
+  for (const auto it : grid) {
+    result.add_x_indices(it.first.x());
+    result.add_y_indices(it.first.y());
+    result.add_z_indices(it.first.z());
+    result.add_values(it.second);
+  }
+  return result;
+}
+
 }  // namespace mapping_3d
 }  // namespace cartographer
 

cartographer/mapping_3d/hybrid_grid_test.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer/mapping_3d/hybrid_grid.h"
 
+#include <map>
 #include <random>
 #include <tuple>
 
@@ -125,51 +126,59 @@ TEST(HybridGridTest, GetCenterOfCell) {
   EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index));
 }
 
-TEST(HybridGridTest, TestIteration) {
+class RandomHybridGridTest : public ::testing::Test {
-  HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f));
+ public:
+  RandomHybridGridTest()
+      : hybrid_grid_(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)), values_() {
+    std::mt19937 rng(1285120005);
+    std::uniform_real_distribution<float> value_distribution(
+        mapping::kMinProbability, mapping::kMaxProbability);
+    std::uniform_int_distribution<int> xyz_distribution(-3000, 2999);
+    for (int i = 0; i < 10000; ++i) {
+      const auto x = xyz_distribution(rng);
+      const auto y = xyz_distribution(rng);
+      const auto z = xyz_distribution(rng);
+      values_.emplace(std::make_tuple(x, y, z), value_distribution(rng));
+    }
 
-  std::map<std::tuple<int, int, int>, float> values;
+    for (const auto& pair : values_) {
-  std::mt19937 rng(1285120005);
+      const Eigen::Array3i cell_index(std::get<0>(pair.first),
-  std::uniform_real_distribution<float> value_distribution(
+                                      std::get<1>(pair.first),
-      mapping::kMinProbability, mapping::kMaxProbability);
+                                      std::get<2>(pair.first));
-  std::uniform_int_distribution<int> xyz_distribution(-3000, 2999);
+      hybrid_grid_.SetProbability(cell_index, pair.second);
-  for (int i = 0; i < 10000; ++i) {
+    }
-    const auto x = xyz_distribution(rng);
-    const auto y = xyz_distribution(rng);
-    const auto z = xyz_distribution(rng);
-    values.emplace(std::make_tuple(x, y, z), value_distribution(rng));
   }
 
-  for (const auto& pair : values) {
+ protected:
-    const Eigen::Array3i cell_index(std::get<0>(pair.first),
+  HybridGrid hybrid_grid_;
-                                    std::get<1>(pair.first),
+  using ValueMap = std::map<std::tuple<int, int, int>, float>;
-                                    std::get<2>(pair.first));
+  ValueMap values_;
-    hybrid_grid.SetProbability(cell_index, pair.second);
+};
-  }
 
-  for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
+TEST_F(RandomHybridGridTest, TestIteration) {
+  for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) {
     const Eigen::Array3i cell_index = it.GetCellIndex();
     const float iterator_probability =
         mapping::ValueToProbability(it.GetValue());
-    EXPECT_EQ(iterator_probability, hybrid_grid.GetProbability(cell_index));
+    EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index));
     const std::tuple<int, int, int> key =
         std::make_tuple(cell_index[0], cell_index[1], cell_index[2]);
-    EXPECT_TRUE(values.count(key));
+    EXPECT_TRUE(values_.count(key));
-    EXPECT_NEAR(values[key], iterator_probability, 1e-4);
+    EXPECT_NEAR(values_[key], iterator_probability, 1e-4);
-    values.erase(key);
+    values_.erase(key);
   }
 
   // Test that range based loop is equivalent to using the iterator.
-  auto it = HybridGrid::Iterator(hybrid_grid);
+  auto it = HybridGrid::Iterator(hybrid_grid_);
-  for (const auto& cell : hybrid_grid) {
+  for (const auto& cell : hybrid_grid_) {
     ASSERT_FALSE(it.Done());
     EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex()));
     EXPECT_EQ(cell.second, it.GetValue());
     it.Next();
   }
 
-  // Now 'values' must not contain values.
+  // Now 'values_' must not contain values.
-  for (const auto& pair : values) {
+  for (const auto& pair : values_) {
     const Eigen::Array3i cell_index(std::get<0>(pair.first),
                                     std::get<1>(pair.first),
                                     std::get<2>(pair.first));
@@ -177,6 +186,56 @@ TEST(HybridGridTest, TestIteration) {
   }
 }
 
+TEST_F(RandomHybridGridTest, ToProto) {
+  const auto proto = ToProto(hybrid_grid_);
+  EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
+  EXPECT_EQ(hybrid_grid_.origin().x(), proto.origin().x());
+  EXPECT_EQ(hybrid_grid_.origin().y(), proto.origin().y());
+  EXPECT_EQ(hybrid_grid_.origin().z(), proto.origin().z());
+
+  ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size());
+  ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size());
+  ASSERT_EQ(proto.x_indices_size(), proto.values_size());
+
+  ValueMap proto_map;
+  for (int i = 0; i < proto.x_indices_size(); ++i) {
+    proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i),
+                              proto.z_indices(i))] = proto.values(i);
+  }
+
+  // Get hybrid_grid_ into the same format.
+  ValueMap hybrid_grid_map;
+  for (const auto i : hybrid_grid_) {
+    hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] =
+        i.second;
+  }
+
+  EXPECT_EQ(proto_map, hybrid_grid_map);
+}
+
+namespace {
+
+struct EigenComparator {
+  bool operator()(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) {
+    return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) <
+           std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z());
+  }
+};
+
+}  // namespace
+
+TEST_F(RandomHybridGridTest, FromProto) {
+  const HybridGrid constructed_grid(ToProto(hybrid_grid_));
+
+  std::map<Eigen::Vector3i, float, EigenComparator> member_map(
+      hybrid_grid_.begin(), hybrid_grid_.end());
+
+  std::map<Eigen::Vector3i, float, EigenComparator> constructed_map(
+      constructed_grid.begin(), constructed_grid.end());
+
+  EXPECT_EQ(member_map, constructed_map);
+}
+
 }  // namespace
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/proto/hybrid_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/transform/proto/transform.proto";
+
+package cartographer.mapping_3d.proto;
+
+message HybridGrid {
+  optional float resolution = 1;
+  optional transform.proto.Vector3f origin = 2;
+  // '{x, y, z}_indices[i]' is the index of 'values[i]'.
+  repeated sint32 x_indices = 3 [packed = true];
+  repeated sint32 y_indices = 4 [packed = true];
+  repeated sint32 z_indices = 5 [packed = true];
+  // The entries in 'values' should be uint16s, not int32s, but protos don't
+  // have a uint16 type.
+  repeated int32 values = 6 [packed = true];
+}