Tiny improvement of HybridGrid::ToProto(). (#401)

Makes it a member function and adds a check for left over
update markers.

cartographer/io/hybrid_grid_points_processor.cc

@@ -46,7 +46,7 @@ void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
 
 PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() {
   const mapping_3d::proto::HybridGrid hybrid_grid_proto =
-      mapping_3d::ToProto(hybrid_grid_);
+      hybrid_grid_.ToProto();
   string serialized;
   hybrid_grid_proto.SerializeToString(&serialized);
   file_writer_->Write(serialized.data(), serialized.size());

cartographer/mapping/map_builder.cc

@@ -19,7 +19,7 @@
 #include <cmath>
 #include <limits>
 #include <memory>
-#include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 #include "cartographer/common/make_unique.h"

cartographer/mapping_2d/probability_grid.h

@@ -172,9 +172,8 @@ class ProbabilityGrid {
     for (const auto cell : cells_) {
       result.mutable_cells()->Add(cell);
     }
-    CHECK(update_indices_.empty()) << "Serialiazing a probability grid during "
+    CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
-                                      "an update is not supported. Finish the "
+                                      "not supported. Finish the update first.";
-                                      "update first.";
     if (!known_cells_box_.isEmpty()) {
       result.set_max_x(known_cells_box_.max().x());
       result.set_max_y(known_cells_box_.max().y());

cartographer/mapping_3d/hybrid_grid.h

@@ -471,7 +471,6 @@ class HybridGrid : public HybridGridBase<uint16> {
     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),
@@ -522,23 +521,25 @@ class HybridGrid : public HybridGridBase<uint16> {
   // Returns true if the probability at the specified 'index' is known.
   bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; }
 
+  proto::HybridGrid ToProto() const {
+    CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
+                                      "not supported. Finish the update first.";
+    proto::HybridGrid result;
+    result.set_resolution(resolution());
+    for (const auto it : *this) {
+      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;
+  }
+
  private:
   // Markers at changed cells.
   std::vector<ValueType*> update_indices_;
 };
 
-inline proto::HybridGrid ToProto(const HybridGrid& grid) {
-  proto::HybridGrid result;
-  result.set_resolution(grid.resolution());
-  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

@@ -185,9 +185,8 @@ TEST_F(RandomHybridGridTest, TestIteration) {
 }
 
 TEST_F(RandomHybridGridTest, ToProto) {
-  const auto proto = ToProto(hybrid_grid_);
+  const auto proto = hybrid_grid_.ToProto();
   EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
-
   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());
@@ -208,8 +207,6 @@ TEST_F(RandomHybridGridTest, ToProto) {
   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()) <
@@ -217,10 +214,8 @@ struct EigenComparator {
   }
 };
 
-}  // namespace
-
 TEST_F(RandomHybridGridTest, FromProto) {
-  const HybridGrid constructed_grid(ToProto(hybrid_grid_));
+  const HybridGrid constructed_grid(hybrid_grid_.ToProto());
 
   std::map<Eigen::Vector3i, float, EigenComparator> member_map(
       hybrid_grid_.begin(), hybrid_grid_.end());

cartographer/mapping_3d/submaps.cc

@@ -185,9 +185,9 @@ void Submap::ToProto(mapping::proto::Submap* const proto) const {
   submap_3d->set_num_range_data(num_range_data());
   submap_3d->set_finished(finished_);
   *submap_3d->mutable_high_resolution_hybrid_grid() =
-      mapping_3d::ToProto(high_resolution_hybrid_grid());
+      high_resolution_hybrid_grid().ToProto();
   *submap_3d->mutable_low_resolution_hybrid_grid() =
-      mapping_3d::ToProto(low_resolution_hybrid_grid());
+      low_resolution_hybrid_grid().ToProto();
 }
 
 void Submap::ToResponseProto(