Improve handling of grid updates. (#400)

Changes the grids to only contain update markers during updates.
Serialized grids will never contain them.

cartographer/mapping_2d/probability_grid.h

@@ -46,10 +46,7 @@ class ProbabilityGrid {
                mapping::kUnknownProbabilityValue) {}
 
   explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
-      : limits_(proto.limits()),
+      : limits_(proto.limits()), cells_() {
-        cells_(),
-        update_indices_(proto.update_indices().begin(),
-                        proto.update_indices().end()) {
     if (proto.has_min_x()) {
       known_cells_box_ =
           Eigen::AlignedBox2i(Eigen::Vector2i(proto.min_x(), proto.min_y()),
@@ -65,8 +62,8 @@ class ProbabilityGrid {
   // Returns the limits of this ProbabilityGrid.
   const MapLimits& limits() const { return limits_; }
 
-  // Starts the next update sequence.
+  // Finishes the update sequence.
-  void StartUpdate() {
+  void FinishUpdate() {
     while (!update_indices_.empty()) {
       DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
       cells_[update_indices_.back()] -= mapping::kUpdateMarker;
@@ -87,7 +84,7 @@ class ProbabilityGrid {
   // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
   // to the probability of the cell at 'cell_index' if the cell has not already
   // been updated. Multiple updates of the same cell will be ignored until
-  // StartUpdate() is called. Returns true if the cell was updated.
+  // FinishUpdate() is called. Returns true if the cell was updated.
   //
   // If this is the first call to ApplyOdds() for the specified cell, its value
   // will be set to probability corresponding to 'odds'.
@@ -136,7 +133,7 @@ class ProbabilityGrid {
 
   // Grows the map as necessary to include 'point'. This changes the meaning of
   // these coordinates going forward. This method must be called immediately
-  // after 'StartUpdate', before any calls to 'ApplyLookupTable'.
+  // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
   void GrowLimits(const Eigen::Vector2f& point) {
     CHECK(update_indices_.empty());
     while (!limits_.Contains(limits_.GetCellIndex(point))) {
@@ -175,10 +172,9 @@ class ProbabilityGrid {
     for (const auto cell : cells_) {
       result.mutable_cells()->Add(cell);
     }
-    result.mutable_update_indices()->Reserve(update_indices_.size());
+    CHECK(update_indices_.empty()) << "Serialiazing a probability grid during "
-    for (const auto update : update_indices_) {
+                                      "an update is not supported. Finish the "
-      result.mutable_update_indices()->Add(update);
+                                      "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_2d/probability_grid_test.cc

@@ -32,9 +32,6 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
   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);
@@ -43,8 +40,8 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
   ProbabilityGrid grid(proto);
   EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
 
-  // TODO(macmason): Figure out how to test the contents of cells_,
+  // TODO(macmason): Figure out how to test the contents of cells_ and
-  // update_indices_, and {min, max}_{x, y}_ gracefully.
+  // {min, max}_{x, y}_ gracefully.
 }
 
 TEST(ProbabilityGridTest, ToProto) {
@@ -55,8 +52,8 @@ TEST(ProbabilityGridTest, ToProto) {
   EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
             proto.limits().DebugString());
 
-  // TODO(macmason): Figure out how to test the contents of cells_,
+  // TODO(macmason): Figure out how to test the contents of cells_ and
-  // update_indices_, and {min, max}_{x, y}_ gracefully.
+  // {min, max}_{x, y}_ gracefully.
 }
 
 TEST(ProbabilityGridTest, ApplyOdds) {
@@ -75,36 +72,34 @@ TEST(ProbabilityGridTest, ApplyOdds) {
 
   probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
 
-  probability_grid.StartUpdate();
   probability_grid.ApplyLookupTable(
       Eigen::Array2i(1, 0),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
+  probability_grid.FinishUpdate();
   EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
 
-  probability_grid.StartUpdate();
   probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
 
-  probability_grid.StartUpdate();
   probability_grid.ApplyLookupTable(
       Eigen::Array2i(0, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
+  probability_grid.FinishUpdate();
   EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
 
   // Tests adding odds to an unknown cell.
-  probability_grid.StartUpdate();
   probability_grid.ApplyLookupTable(
       Eigen::Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
   EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
               1e-4);
 
-  // Tests that further updates are ignored if StartUpdate() isn't called.
+  // Tests that further updates are ignored if FinishUpdate() isn't called.
   probability_grid.ApplyLookupTable(
       Eigen::Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
   EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
               1e-4);
-  probability_grid.StartUpdate();
+  probability_grid.FinishUpdate();
   probability_grid.ApplyLookupTable(
       Eigen::Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
@@ -123,7 +118,6 @@ TEST(ProbabilityGridTest, GetProbability) {
   ASSERT_EQ(2, cell_limits.num_x_cells);
   ASSERT_EQ(2, cell_limits.num_y_cells);
 
-  probability_grid.StartUpdate();
   probability_grid.SetProbability(
       limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)),
       mapping::kMaxProbability);
@@ -185,7 +179,6 @@ TEST(ProbabilityGridTest, CorrectCropping) {
       mapping::kMinProbability, mapping::kMaxProbability);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
-  probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
            Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
     probability_grid.SetProbability(xy_index, value_distribution(rng));

cartographer/mapping_2d/proto/probability_grid.proto

@@ -23,7 +23,6 @@ message ProbabilityGrid {
   // 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;

cartographer/mapping_2d/range_data_inserter.cc

@@ -53,11 +53,11 @@ RangeDataInserter::RangeDataInserter(
 
 void RangeDataInserter::Insert(const sensor::RangeData& range_data,
                                ProbabilityGrid* const probability_grid) const {
-  // By not starting a new update after hits are inserted, we give hits priority
+  // By not finishing the update after hits are inserted, we give hits priority
   // (i.e. no hits will be ignored because of a miss in the same cell).
-  CHECK_NOTNULL(probability_grid)->StartUpdate();
   CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
-           probability_grid);
+           CHECK_NOTNULL(probability_grid));
+  probability_grid->FinishUpdate();
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/range_data_inserter_test.cc

@@ -51,8 +51,8 @@ class RangeDataInserterTest : public ::testing::Test {
     range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
     range_data.origin.x() = -0.5f;
     range_data.origin.y() = 0.5f;
-    probability_grid_.StartUpdate();
     range_data_inserter_->Insert(range_data, &probability_grid_);
+    probability_grid_.FinishUpdate();
   }
 
   ProbabilityGrid probability_grid_;

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -41,7 +41,6 @@ TEST(PrecomputationGridTest, CorrectValues) {
   std::uniform_int_distribution<int> distribution(0, 255);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
-  probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
     probability_grid.SetProbability(
@@ -76,7 +75,6 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
   std::uniform_int_distribution<int> distribution(0, 255);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
-  probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
     probability_grid.SetProbability(
@@ -151,7 +149,6 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
 
     ProbabilityGrid probability_grid(
         MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
-    probability_grid.StartUpdate();
     range_data_inserter.Insert(
         sensor::RangeData{
             Eigen::Vector3f(expected_pose.translation().x(),
@@ -160,6 +157,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
                 point_cloud, transform::Embed3D(expected_pose.cast<float>())),
             {}},
         &probability_grid);
+    probability_grid.FinishUpdate();
 
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
                                                              options);
@@ -204,7 +202,6 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
 
     ProbabilityGrid probability_grid(
         MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
-    probability_grid.StartUpdate();
     range_data_inserter.Insert(
         sensor::RangeData{
             transform::Embed3D(expected_pose * perturbation).translation(),
@@ -212,6 +209,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
                                         transform::Embed3D(expected_pose)),
             {}},
         &probability_grid);
+    probability_grid.FinishUpdate();
 
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
                                                              options);

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -55,10 +55,10 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
     point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
     point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
     point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
-    probability_grid_.StartUpdate();
     range_data_inserter_->Insert(
         sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
         &probability_grid_);
+    probability_grid_.FinishUpdate();
     {
       auto parameter_dictionary = common::MakeDictionary(
           "return {"

cartographer/mapping_2d/submaps.cc

@@ -40,7 +40,6 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
       probability_grid.limits().max() -
       resolution * Eigen::Vector2d(offset.y(), offset.x());
   ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
-  cropped_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
     if (probability_grid.IsKnown(xy_index + offset)) {
       cropped_grid.SetProbability(

cartographer/mapping_3d/hybrid_grid.h

@@ -485,8 +485,8 @@ class HybridGrid : public HybridGridBase<uint16> {
     *mutable_value(index) = mapping::ProbabilityToValue(probability);
   }
 
-  // Starts the next update sequence.
+  // Finishes the update sequence.
-  void StartUpdate() {
+  void FinishUpdate() {
     while (!update_indices_.empty()) {
       DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker);
       *update_indices_.back() -= mapping::kUpdateMarker;
@@ -497,7 +497,7 @@ class HybridGrid : public HybridGridBase<uint16> {
   // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
   // to the probability of the cell at 'index' if the cell has not already been
   // updated. Multiple updates of the same cell will be ignored until
-  // StartUpdate() is called. Returns true if the cell was updated.
+  // FinishUpdate() is called. Returns true if the cell was updated.
   //
   // If this is the first call to ApplyOdds() for the specified cell, its value
   // will be set to probability corresponding to 'odds'.

cartographer/mapping_3d/hybrid_grid_test.cc

@@ -40,33 +40,32 @@ TEST(HybridGridTest, ApplyOdds) {
 
   hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f);
 
-  hybrid_grid.StartUpdate();
   hybrid_grid.ApplyLookupTable(
       Eigen::Array3i(1, 0, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
+  hybrid_grid.FinishUpdate();
   EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f);
 
   hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f);
 
-  hybrid_grid.StartUpdate();
   hybrid_grid.ApplyLookupTable(
       Eigen::Array3i(0, 1, 0),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f)));
+  hybrid_grid.FinishUpdate();
   EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f);
 
   // Tests adding odds to an unknown cell.
-  hybrid_grid.StartUpdate();
   hybrid_grid.ApplyLookupTable(
       Eigen::Array3i(1, 1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f)));
   EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
 
-  // Tests that further updates are ignored if StartUpdate() isn't called.
+  // Tests that further updates are ignored if FinishUpdate() isn't called.
   hybrid_grid.ApplyLookupTable(
       Eigen::Array3i(1, 1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
   EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
-  hybrid_grid.StartUpdate();
+  hybrid_grid.FinishUpdate();
   hybrid_grid.ApplyLookupTable(
       Eigen::Array3i(1, 1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));

cartographer/mapping_3d/range_data_inserter.cc

@@ -78,7 +78,7 @@ RangeDataInserter::RangeDataInserter(
 
 void RangeDataInserter::Insert(const sensor::RangeData& range_data,
                                HybridGrid* hybrid_grid) const {
-  CHECK_NOTNULL(hybrid_grid)->StartUpdate();
+  CHECK_NOTNULL(hybrid_grid);
 
   for (const Eigen::Vector3f& hit : range_data.returns) {
     const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
@@ -89,6 +89,7 @@ void RangeDataInserter::Insert(const sensor::RangeData& range_data,
   // (i.e. no hits will be ignored because of a miss in the same cell).
   InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
                        hybrid_grid, options_.num_free_space_voxels());
+  hybrid_grid->FinishUpdate();
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -88,13 +88,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
             Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
 
     HybridGrid hybrid_grid(0.05f);
-    hybrid_grid.StartUpdate();
     range_data_inserter.Insert(
         sensor::RangeData{
             expected_pose.translation(),
             sensor::TransformPointCloud(point_cloud, expected_pose),
             {}},
         &hybrid_grid);
+    hybrid_grid.FinishUpdate();
 
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
                                                              options);