Introduce Grid2D as base class for 2D grids (#1046)

This is a first step towards generalizing submaps (see [ongoing RFC discussion](https://github.com/googlecartographer/rfcs/pull/30)).
Although this PR already introduces a correspondence cost grid, the grid is still used as probability grid.

cartographer/mapping/2d/grid_2d.cc

@@ -0,0 +1,147 @@
+/*
+ * Copyright 2018 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/2d/grid_2d.h"
+
+#include "cartographer/mapping/probability_values.h"
+
+namespace cartographer {
+namespace mapping {
+
+Grid2D::Grid2D(const MapLimits& limits)
+    : limits_(limits),
+      correspondence_cost_cells_(
+          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
+          kUnknownCorrespondenceValue) {}
+
+Grid2D::Grid2D(const proto::Grid2D& proto)
+    : limits_(proto.limits()), correspondence_cost_cells_() {
+  if (proto.has_known_cells_box()) {
+    const auto& box = proto.known_cells_box();
+    known_cells_box_ =
+        Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
+                            Eigen::Vector2i(box.max_x(), box.max_y()));
+  }
+  correspondence_cost_cells_.reserve(proto.cells_size());
+  for (const auto& cell : proto.cells()) {
+    CHECK_LE(cell, std::numeric_limits<uint16>::max());
+    correspondence_cost_cells_.push_back(cell);
+  }
+}
+
+// Finishes the update sequence.
+void Grid2D::FinishUpdate() {
+  while (!update_indices_.empty()) {
+    DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
+              kUpdateMarker);
+    correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
+    update_indices_.pop_back();
+  }
+}
+
+// Sets the probability of the cell at 'cell_index' to the given
+// 'probability'. Only allowed if the cell was unknown before.
+void Grid2D::SetCorrespondenceCost(const Eigen::Array2i& cell_index,
+                                   const float correspondence_cost) {
+  LOG(FATAL) << "Not implemented";
+}
+
+// Returns the probability of the cell with 'cell_index'.
+float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const {
+  LOG(FATAL) << "Not implemented";
+  return kUnknownCorrespondenceValue;
+}
+
+// Returns true if the probability at the specified index is known.
+bool Grid2D::IsKnown(const Eigen::Array2i& cell_index) const {
+  return limits_.Contains(cell_index) &&
+         correspondence_cost_cells_[ToFlatIndex(cell_index)] !=
+             kUnknownCorrespondenceValue;
+}
+
+// Fills in 'offset' and 'limits' to define a subregion of that contains all
+// known cells.
+void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
+                                  CellLimits* const limits) const {
+  if (known_cells_box_.isEmpty()) {
+    *offset = Eigen::Array2i::Zero();
+    *limits = CellLimits(1, 1);
+    return;
+  }
+  *offset = known_cells_box_.min().array();
+  *limits = CellLimits(known_cells_box_.sizes().x() + 1,
+                       known_cells_box_.sizes().y() + 1);
+}
+
+// Grows the map as necessary to include 'point'. This changes the meaning of
+// these coordinates going forward. This method must be called immediately
+// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
+void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
+  CHECK(update_indices_.empty());
+  while (!limits_.Contains(limits_.GetCellIndex(point))) {
+    const int x_offset = limits_.cell_limits().num_x_cells / 2;
+    const int y_offset = limits_.cell_limits().num_y_cells / 2;
+    const MapLimits new_limits(
+        limits_.resolution(),
+        limits_.max() +
+            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
+        CellLimits(2 * limits_.cell_limits().num_x_cells,
+                   2 * limits_.cell_limits().num_y_cells));
+    const int stride = new_limits.cell_limits().num_x_cells;
+    const int offset = x_offset + stride * y_offset;
+    const int new_size = new_limits.cell_limits().num_x_cells *
+                         new_limits.cell_limits().num_y_cells;
+    std::vector<uint16> new_cells(new_size, kUnknownCorrespondenceValue);
+    for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
+      for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
+        new_cells[offset + j + i * stride] =
+            correspondence_cost_cells_[j +
+                                       i * limits_.cell_limits().num_x_cells];
+      }
+    }
+    correspondence_cost_cells_ = new_cells;
+    limits_ = new_limits;
+    if (!known_cells_box_.isEmpty()) {
+      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
+    }
+  }
+}
+
+proto::Grid2D Grid2D::ToProto() const {
+  proto::Grid2D result;
+  *result.mutable_limits() = mapping::ToProto(limits_);
+  result.mutable_cells()->Reserve(correspondence_cost_cells_.size());
+  for (const auto& cell : correspondence_cost_cells_) {
+    result.mutable_cells()->Add(cell);
+  }
+  CHECK(update_indices().empty()) << "Serializing a grid during an update is "
+                                     "not supported. Finish the update first.";
+  if (!known_cells_box().isEmpty()) {
+    auto* const box = result.mutable_known_cells_box();
+    box->set_max_x(known_cells_box().max().x());
+    box->set_max_y(known_cells_box().max().y());
+    box->set_min_x(known_cells_box().min().x());
+    box->set_min_y(known_cells_box().min().y());
+  }
+  return result;
+}
+
+int Grid2D::ToFlatIndex(const Eigen::Array2i& cell_index) const {
+  CHECK(limits_.Contains(cell_index)) << cell_index;
+  return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/2d/grid_2d.h

@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 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.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_2D_GRID_2D_H_
+#define CARTOGRAPHER_MAPPING_2D_GRID_2D_H_
+
+#include <vector>
+
+#include "cartographer/mapping/2d/map_limits.h"
+#include "cartographer/mapping/2d/proto/grid_2d.pb.h"
+
+namespace cartographer {
+namespace mapping {
+
+class Grid2D {
+ public:
+  explicit Grid2D(const MapLimits& limits);
+  explicit Grid2D(const proto::Grid2D& proto);
+
+  // Returns the limits of this ProbabilityGrid.
+  const MapLimits& limits() const { return limits_; }
+
+  // Finishes the update sequence.
+  void FinishUpdate();
+
+  // Sets the correspondence cost of the cell at 'cell_index' to the given
+  // 'correspondence cost'. Only allowed if the cell was unknown before.
+  void SetCorrespondenceCost(const Eigen::Array2i& cell_index,
+                             const float correspondence_cost);
+
+  // Returns the probability of the cell with 'cell_index'.
+  float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
+
+  // Returns true if the probability at the specified index is known.
+  bool IsKnown(const Eigen::Array2i& cell_index) const;
+
+  // Fills in 'offset' and 'limits' to define a subregion of that contains all
+  // known cells.
+  void ComputeCroppedLimits(Eigen::Array2i* const offset,
+                            CellLimits* const limits) const;
+
+  // Grows the map as necessary to include 'point'. This changes the meaning of
+  // these coordinates going forward. This method must be called immediately
+  // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
+  void GrowLimits(const Eigen::Vector2f& point);
+
+  virtual proto::Grid2D ToProto() const;
+
+ protected:
+  const std::vector<uint16>& correspondence_cost_cells() const {
+    return correspondence_cost_cells_;
+  }
+  const std::vector<int>& update_indices() const { return update_indices_; }
+  const Eigen::AlignedBox2i& known_cells_box() const {
+    return known_cells_box_;
+  }
+
+  std::vector<uint16>* mutable_correspondence_cost_cells() {
+    return &correspondence_cost_cells_;
+  }
+  std::vector<int>* mutable_update_indices() { return &update_indices_; }
+  Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
+
+  // Converts a 'cell_index' into an index into 'cells_'.
+  int ToFlatIndex(const Eigen::Array2i& cell_index) const;
+
+ private:
+  MapLimits limits_;
+  std::vector<uint16> correspondence_cost_cells_;
+  std::vector<int> update_indices_;
+
+  // Bounding box of known cells to efficiently compute cropping limits.
+  Eigen::AlignedBox2i known_cells_box_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_GRID_2D_H_

cartographer/mapping/2d/probability_grid.cc

@@ -1,3 +1,18 @@
+/*
+ * 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/2d/probability_grid.h"
 
 #include <limits>
@@ -6,54 +21,22 @@
 
 namespace cartographer {
 namespace mapping {
-namespace {
 
-// Converts a 'cell_index' into an index into 'cells_'.
+ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) : Grid2D(limits) {}
-int ToFlatIndex(const Eigen::Array2i& cell_index, const MapLimits& limits) {
-  CHECK(limits.Contains(cell_index)) << cell_index;
-  return limits.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
-}
 
-}  // namespace
+ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto) : Grid2D(proto) {
-
+  CHECK(proto.has_probability_grid_2d());
-ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
-    : limits_(limits),
-      cells_(
-          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
-          kUnknownProbabilityValue) {}
-
-ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto)
-    : limits_(proto.limits()), cells_() {
-  if (proto.has_known_cells_box()) {
-    const auto& box = proto.known_cells_box();
-    known_cells_box_ =
-        Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
-                            Eigen::Vector2i(box.max_x(), box.max_y()));
-  }
-  cells_.reserve(proto.cells_size());
-  for (const auto& cell : proto.cells()) {
-    CHECK_LE(cell, std::numeric_limits<uint16>::max());
-    cells_.push_back(cell);
-  }
-}
-
-// Finishes the update sequence.
-void ProbabilityGrid::FinishUpdate() {
-  while (!update_indices_.empty()) {
-    DCHECK_GE(cells_[update_indices_.back()], kUpdateMarker);
-    cells_[update_indices_.back()] -= kUpdateMarker;
-    update_indices_.pop_back();
-  }
 }
 
 // Sets the probability of the cell at 'cell_index' to the given
 // 'probability'. Only allowed if the cell was unknown before.
 void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                      const float probability) {
-  uint16& cell = cells_[ToFlatIndex(cell_index, limits_)];
+  uint16& cell =
+      (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
   CHECK_EQ(cell, kUnknownProbabilityValue);
   cell = ProbabilityToValue(probability);
-  known_cells_box_.extend(cell_index.matrix());
+  mutable_known_cells_box()->extend(cell_index.matrix());
 }
 
 // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
@@ -66,95 +49,28 @@ void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
 bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                        const std::vector<uint16>& table) {
   DCHECK_EQ(table.size(), kUpdateMarker);
-  const int flat_index = ToFlatIndex(cell_index, limits_);
+  const int flat_index = ToFlatIndex(cell_index);
-  uint16* cell = &cells_[flat_index];
+  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
   if (*cell >= kUpdateMarker) {
     return false;
   }
-  update_indices_.push_back(flat_index);
+  mutable_update_indices()->push_back(flat_index);
   *cell = table[*cell];
   DCHECK_GE(*cell, kUpdateMarker);
-  known_cells_box_.extend(cell_index.matrix());
+  mutable_known_cells_box()->extend(cell_index.matrix());
   return true;
 }
 
 // Returns the probability of the cell with 'cell_index'.
 float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
-  if (limits_.Contains(cell_index)) {
+  if (!limits().Contains(cell_index)) return kMinProbability;
-    return ValueToProbability(cells_[ToFlatIndex(cell_index, limits_)]);
+  return ValueToProbability(
-  }
+      correspondence_cost_cells()[ToFlatIndex(cell_index)]);
-  return kMinProbability;
 }
-
+proto::Grid2D ProbabilityGrid::ToProto() const {
-// Returns true if the probability at the specified index is known.
+  proto::Grid2D result;
-bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const {
+  result = Grid2D::ToProto();
-  return limits_.Contains(cell_index) &&
+  result.mutable_probability_grid_2d();
-         cells_[ToFlatIndex(cell_index, limits_)] != kUnknownProbabilityValue;
-}
-
-// Fills in 'offset' and 'limits' to define a subregion of that contains all
-// known cells.
-void ProbabilityGrid::ComputeCroppedLimits(Eigen::Array2i* const offset,
-                                           CellLimits* const limits) const {
-  if (known_cells_box_.isEmpty()) {
-    *offset = Eigen::Array2i::Zero();
-    *limits = CellLimits(1, 1);
-    return;
-  }
-  *offset = known_cells_box_.min().array();
-  *limits = CellLimits(known_cells_box_.sizes().x() + 1,
-                       known_cells_box_.sizes().y() + 1);
-}
-
-// Grows the map as necessary to include 'point'. This changes the meaning of
-// these coordinates going forward. This method must be called immediately
-// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
-void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) {
-  CHECK(update_indices_.empty());
-  while (!limits_.Contains(limits_.GetCellIndex(point))) {
-    const int x_offset = limits_.cell_limits().num_x_cells / 2;
-    const int y_offset = limits_.cell_limits().num_y_cells / 2;
-    const MapLimits new_limits(
-        limits_.resolution(),
-        limits_.max() +
-            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
-        CellLimits(2 * limits_.cell_limits().num_x_cells,
-                   2 * limits_.cell_limits().num_y_cells));
-    const int stride = new_limits.cell_limits().num_x_cells;
-    const int offset = x_offset + stride * y_offset;
-    const int new_size = new_limits.cell_limits().num_x_cells *
-                         new_limits.cell_limits().num_y_cells;
-    std::vector<uint16> new_cells(new_size, kUnknownProbabilityValue);
-    for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
-      for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
-        new_cells[offset + j + i * stride] =
-            cells_[j + i * limits_.cell_limits().num_x_cells];
-      }
-    }
-    cells_ = new_cells;
-    limits_ = new_limits;
-    if (!known_cells_box_.isEmpty()) {
-      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
-    }
-  }
-}
-
-proto::ProbabilityGrid ProbabilityGrid::ToProto() const {
-  proto::ProbabilityGrid result;
-  *result.mutable_limits() = mapping::ToProto(limits_);
-  result.mutable_cells()->Reserve(cells_.size());
-  for (const auto& cell : cells_) {
-    result.mutable_cells()->Add(cell);
-  }
-  CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
-                                    "not supported. Finish the update first.";
-  if (!known_cells_box_.isEmpty()) {
-    auto* const box = result.mutable_known_cells_box();
-    box->set_max_x(known_cells_box_.max().x());
-    box->set_max_y(known_cells_box_.max().y());
-    box->set_min_x(known_cells_box_.min().x());
-    box->set_min_y(known_cells_box_.min().y());
-  }
   return result;
 }
 

cartographer/mapping/2d/probability_grid.h

@@ -20,24 +20,18 @@
 #include <vector>
 
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/2d/map_limits.h"
-#include "cartographer/mapping/2d/proto/probability_grid.pb.h"
 #include "cartographer/mapping/2d/xy_index.h"
 
 namespace cartographer {
 namespace mapping {
 
 // Represents a 2D grid of probabilities.
-class ProbabilityGrid {
+class ProbabilityGrid : public Grid2D {
  public:
   explicit ProbabilityGrid(const MapLimits& limits);
-  explicit ProbabilityGrid(const proto::ProbabilityGrid& proto);
+  explicit ProbabilityGrid(const proto::Grid2D& proto);
-
-  // Returns the limits of this ProbabilityGrid.
-  const MapLimits& limits() const { return limits_; }
-
-  // Finishes the update sequence.
-  void FinishUpdate();
 
   // Sets the probability of the cell at 'cell_index' to the given
   // 'probability'. Only allowed if the cell was unknown before.
@@ -57,28 +51,7 @@ class ProbabilityGrid {
   // Returns the probability of the cell with 'cell_index'.
   float GetProbability(const Eigen::Array2i& cell_index) const;
 
-  // Returns true if the probability at the specified index is known.
+  virtual proto::Grid2D ToProto() const override;
-  bool IsKnown(const Eigen::Array2i& cell_index) const;
-
-  // Fills in 'offset' and 'limits' to define a subregion of that contains all
-  // known cells.
-  void ComputeCroppedLimits(Eigen::Array2i* const offset,
-                            CellLimits* const limits) const;
-
-  // Grows the map as necessary to include 'point'. This changes the meaning of
-  // these coordinates going forward. This method must be called immediately
-  // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
-  void GrowLimits(const Eigen::Vector2f& point);
-
-  proto::ProbabilityGrid ToProto() const;
-
- private:
-  MapLimits limits_;
-  std::vector<uint16> cells_;  // Highest bit is update marker.
-  std::vector<int> update_indices_;
-
-  // Bounding box of known cells to efficiently compute cropping limits.
-  Eigen::AlignedBox2i known_cells_box_;
 };
 
 }  // namespace mapping

cartographer/mapping/2d/probability_grid_test.cc

@@ -29,7 +29,7 @@ using Eigen::Array2i;
 using Eigen::Vector2f;
 
 TEST(ProbabilityGridTest, ProtoConstructor) {
-  proto::ProbabilityGrid proto;
+  proto::Grid2D proto;
   const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
   *proto.mutable_limits() = ToProto(limits);
   for (int i = 6; i < 12; ++i) {
@@ -39,6 +39,7 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
   proto.mutable_known_cells_box()->set_max_y(20);
   proto.mutable_known_cells_box()->set_min_x(21);
   proto.mutable_known_cells_box()->set_min_y(22);
+  proto.mutable_probability_grid_2d();
 
   ProbabilityGrid grid(proto);
   EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());

cartographer/mapping/2d/proto/grid_2d.proto

@@ -0,0 +1,38 @@
+// Copyright 2018 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 = "proto3";
+
+package cartographer.mapping.proto;
+
+import "cartographer/mapping/2d/proto/probability_grid.proto";
+import "cartographer/mapping/2d/proto/map_limits.proto";
+
+message Grid2D {
+  message CellBox {
+    int32 max_x = 1;
+    int32 max_y = 2;
+    int32 min_x = 3;
+    int32 min_y = 4;
+  }
+
+  MapLimits limits = 1;
+  // These values are actually int16s, but protos don't have a native int16
+  // type.
+  repeated int32 cells = 2;
+  CellBox known_cells_box = 3;
+  oneof grid {
+    ProbabilityGrid probability_grid_2d = 4;
+  }
+}

cartographer/mapping/2d/proto/probability_grid.proto

@@ -14,21 +14,7 @@
 
 syntax = "proto3";
 
-import "cartographer/mapping/2d/proto/map_limits.proto";
-
 package cartographer.mapping.proto;
 
 message ProbabilityGrid {
-  message CellBox {
-    int32 max_x = 1;
-    int32 max_y = 2;
-    int32 min_x = 3;
-    int32 min_y = 4;
-  }
-
-  MapLimits limits = 1;
-  // These values are actually int16s, but protos don't have a native int16
-  // type.
-  repeated int32 cells = 2;
-  CellBox known_cells_box = 8;
 }

cartographer/mapping/2d/submap_2d.cc

@@ -69,7 +69,7 @@ Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin)
 
 Submap2D::Submap2D(const proto::Submap2D& proto)
     : Submap(transform::ToRigid3(proto.local_pose())),
-      probability_grid_(ProbabilityGrid(proto.probability_grid())) {
+      probability_grid_(ProbabilityGrid(proto.grid())) {
   set_num_range_data(proto.num_range_data());
   set_finished(proto.finished());
 }
@@ -81,7 +81,7 @@ void Submap2D::ToProto(proto::Submap* const proto,
   submap_2d->set_num_range_data(num_range_data());
   submap_2d->set_finished(finished());
   if (include_probability_grid_data) {
-    *submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
+    *submap_2d->mutable_grid() = probability_grid_.ToProto();
   }
 }
 
@@ -90,8 +90,8 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) {
   const auto& submap_2d = proto.submap_2d();
   set_num_range_data(submap_2d.num_range_data());
   set_finished(submap_2d.finished());
-  if (submap_2d.has_probability_grid()) {
+  if (submap_2d.has_grid()) {
-    probability_grid_ = ProbabilityGrid(submap_2d.probability_grid());
+    probability_grid_ = ProbabilityGrid(submap_2d.grid());
   }
 }
 

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc

@@ -47,24 +47,26 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
     submap_2d.set_finished(is_finished);
     *submap_2d.mutable_local_pose() = transform::ToProto(pose_3d);
 
-    auto* probability_grid = submap_2d.mutable_probability_grid();
+    auto* grid = submap_2d.mutable_grid();
     for (int i = 0; i < num_cells * num_cells; ++i) {
-      probability_grid->add_cells(1);
+      grid->add_cells(1);
     }
 
-    auto* map_limits = probability_grid->mutable_limits();
+    auto* map_limits = grid->mutable_limits();
     map_limits->set_resolution(1.0);
     *map_limits->mutable_max() =
         transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
     map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
     map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
 
-    auto* know_cells_box = probability_grid->mutable_known_cells_box();
+    auto* know_cells_box = grid->mutable_known_cells_box();
     know_cells_box->set_min_x(0);
     know_cells_box->set_min_y(0);
     know_cells_box->set_max_x(num_cells - 1);
     know_cells_box->set_max_y(num_cells - 1);
 
+    grid->mutable_probability_grid_2d();
+    LOG(INFO) << submap_2d.DebugString();
     fake_pose_graph_.mutable_submap_data()->Insert(
         {0 /* trajectory_id */, submap_index},
         {std::make_shared<const Submap2D>(submap_2d), pose_3d});

cartographer/mapping/probability_values.h

@@ -44,6 +44,7 @@ inline float ClampProbability(const float probability) {
 }
 
 constexpr uint16 kUnknownProbabilityValue = 0;
+constexpr uint16 kUnknownCorrespondenceValue = 0;
 constexpr uint16 kUpdateMarker = 1u << 15;
 
 // Converts a probability to a uint16 in the [1, 32767] range.

cartographer/mapping/proto/submap.proto

@@ -16,7 +16,7 @@ syntax = "proto3";
 
 package cartographer.mapping.proto;
 
-import "cartographer/mapping/2d/proto/probability_grid.proto";
+import "cartographer/mapping/2d/proto/grid_2d.proto";
 import "cartographer/mapping/3d/proto/hybrid_grid.proto";
 import "cartographer/transform/proto/transform.proto";
 
@@ -25,7 +25,7 @@ message Submap2D {
   transform.proto.Rigid3d local_pose = 1;
   int32 num_range_data = 2;
   bool finished = 3;
-  ProbabilityGrid probability_grid = 4;
+  Grid2D grid = 4;
 }
 
 // Serialized state of a Submap3D.