Introduce Grid and RangeDataInserter (#1108)

- Introduce `RangeDataInserterInterface` as common interface for all range data inserters
- Introduce a minimal `GridInterface` as a base for a common interface for 2D and 3D grids 
- Rename `RangeDataInserter2D` to `ProbabilityGridRangeDataInserter2D`
- Move grid generation logic from `Submap2D` to `ActiveSubmaps2D`
- Update proto and configuration structure to mirror the code structure - backwards compatibility is maintained 
 - Step towards [RFC 0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)

cartographer/io/probability_grid_points_processor.cc

@@ -57,8 +57,8 @@ uint8 ProbabilityToColor(float probability_from_grid) {
 
 ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
     const double resolution,
-    const mapping::proto::RangeDataInserterOptions2D&
+    const mapping::proto::ProbabilityGridRangeDataInserterOptions2D&
-        range_data_inserter_options,
+        probability_grid_range_data_inserter_options,
     const DrawTrajectories& draw_trajectories,
     std::unique_ptr<FileWriter> file_writer,
     const std::vector<mapping::proto::Trajectory>& trajectories,
@@ -67,7 +67,7 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
       trajectories_(trajectories),
       file_writer_(std::move(file_writer)),
       next_(next),
-      range_data_inserter_(range_data_inserter_options),
+      range_data_inserter_(probability_grid_range_data_inserter_options),
       probability_grid_(CreateProbabilityGrid(resolution)) {}
 
 std::unique_ptr<ProbabilityGridPointsProcessor>
@@ -82,7 +82,7 @@ ProbabilityGridPointsProcessor::FromDictionary(
                                      : DrawTrajectories::kNo;
   return common::make_unique<ProbabilityGridPointsProcessor>(
       dictionary->GetDouble("resolution"),
-      mapping::CreateRangeDataInserterOptions2D(
+      mapping::CreateProbabilityGridRangeDataInserterOptions2D(
           dictionary->GetDictionary("range_data_inserter").get()),
       draw_trajectories,
       file_writer_factory(dictionary->GetString("filename") + ".png"),

cartographer/io/probability_grid_points_processor.h

@@ -24,8 +24,8 @@
 #include "cartographer/io/points_batch.h"
 #include "cartographer/io/points_processor.h"
 #include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/proto/range_data_inserter_options_2d.pb.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.pb.h"
 #include "cartographer/mapping/proto/trajectory.pb.h"
 
 namespace cartographer {
@@ -42,8 +42,8 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
   enum class DrawTrajectories { kNo, kYes };
   ProbabilityGridPointsProcessor(
       double resolution,
-      const mapping::proto::RangeDataInserterOptions2D&
+      const mapping::proto::ProbabilityGridRangeDataInserterOptions2D&
-          range_data_inserter_options,
+          probability_grid_range_data_inserter_options,
       const DrawTrajectories& draw_trajectories,
       std::unique_ptr<FileWriter> file_writer,
       const std::vector<mapping::proto::Trajectory>& trajectorios,
@@ -68,7 +68,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
   const std::vector<mapping::proto::Trajectory> trajectories_;
   std::unique_ptr<FileWriter> file_writer_;
   PointsProcessor* const next_;
-  mapping::RangeDataInserter2D range_data_inserter_;
+  mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_;
   mapping::ProbabilityGrid probability_grid_;
 };
 

cartographer/mapping/2d/grid_2d.cc

@@ -20,6 +20,19 @@
 namespace cartographer {
 namespace mapping {
 
+proto::GridOptions2D CreateGridOptions2D(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::GridOptions2D options;
+  const std::string grid_type_string =
+      parameter_dictionary->GetString("grid_type");
+  proto::GridOptions2D_GridType grid_type;
+  CHECK(proto::GridOptions2D_GridType_Parse(grid_type_string, &grid_type))
+      << "Unknown GridOptions2D_GridType kind: " << grid_type_string;
+  options.set_grid_type(grid_type);
+  options.set_resolution(parameter_dictionary->GetDouble("resolution"));
+  return options;
+}
+
 Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
                float max_correspondence_cost)
     : limits_(limits),

cartographer/mapping/2d/grid_2d.h

@@ -21,12 +21,17 @@
 
 #include "cartographer/mapping/2d/map_limits.h"
 #include "cartographer/mapping/2d/proto/grid_2d.pb.h"
+#include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h"
+#include "cartographer/mapping/grid_interface.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 
 namespace cartographer {
 namespace mapping {
 
-class Grid2D {
+proto::GridOptions2D CreateGridOptions2D(
+    common::LuaParameterDictionary* const parameter_dictionary);
+
+class Grid2D : public GridInterface {
  public:
   explicit Grid2D(const MapLimits& limits, float min_correspondence_cost,
                   float max_correspondence_cost);
@@ -57,7 +62,7 @@ class Grid2D {
   // 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 void GrowLimits(const Eigen::Vector2f& point);
 
   virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;
 

cartographer/mapping/2d/probability_grid.cc

@@ -71,6 +71,7 @@ float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
   return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
       correspondence_cost_cells()[ToFlatIndex(cell_index)]));
 }
+
 proto::Grid2D ProbabilityGrid::ToProto() const {
   proto::Grid2D result;
   result = Grid2D::ToProto();

cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 
 #include <cstdlib>
 
@@ -28,9 +28,10 @@
 namespace cartographer {
 namespace mapping {
 
-proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
+proto::ProbabilityGridRangeDataInserterOptions2D
-    common::LuaParameterDictionary* const parameter_dictionary) {
+CreateProbabilityGridRangeDataInserterOptions2D(
-  proto::RangeDataInserterOptions2D options;
+    common::LuaParameterDictionary* parameter_dictionary) {
+  proto::ProbabilityGridRangeDataInserterOptions2D options;
   options.set_hit_probability(
       parameter_dictionary->GetDouble("hit_probability"));
   options.set_miss_probability(
@@ -44,21 +45,22 @@ proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
   return options;
 }
 
-RangeDataInserter2D::RangeDataInserter2D(
+ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
-    const proto::RangeDataInserterOptions2D& options)
+    const proto::ProbabilityGridRangeDataInserterOptions2D& options)
     : options_(options),
       hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
           Odds(options.hit_probability()))),
       miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
           Odds(options.miss_probability()))) {}
 
-void RangeDataInserter2D::Insert(const sensor::RangeData& range_data,
+void ProbabilityGridRangeDataInserter2D::Insert(
-                                 Grid2D* const grid) const {
+    const sensor::RangeData& range_data, GridInterface* const grid) const {
+  ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
   // 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).
   CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
-           CHECK_NOTNULL(static_cast<ProbabilityGrid*>(grid)));
+           CHECK_NOTNULL(probability_grid));
-  grid->FinishUpdate();
+  probability_grid->FinishUpdate();
 }
 
 }  // namespace mapping

cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_
+#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_
-#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_
+#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_
 
 #include <utility>
 #include <vector>
@@ -23,30 +23,35 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/proto/range_data_inserter_options_2d.pb.h"
+#include "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.pb.h"
 #include "cartographer/mapping/2d/xy_index.h"
+#include "cartographer/mapping/range_data_inserter_interface.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
 
 namespace cartographer {
 namespace mapping {
 
-proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
+proto::ProbabilityGridRangeDataInserterOptions2D
+CreateProbabilityGridRangeDataInserterOptions2D(
     common::LuaParameterDictionary* parameter_dictionary);
 
-class RangeDataInserter2D {
+class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface {
  public:
-  explicit RangeDataInserter2D(
+  explicit ProbabilityGridRangeDataInserter2D(
-      const proto::RangeDataInserterOptions2D& options);
+      const proto::ProbabilityGridRangeDataInserterOptions2D& options);
 
-  RangeDataInserter2D(const RangeDataInserter2D&) = delete;
+  ProbabilityGridRangeDataInserter2D(
-  RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete;
+      const ProbabilityGridRangeDataInserter2D&) = delete;
+  ProbabilityGridRangeDataInserter2D& operator=(
+      const ProbabilityGridRangeDataInserter2D&) = delete;
 
   // Inserts 'range_data' into 'probability_grid'.
-  void Insert(const sensor::RangeData& range_data, Grid2D* grid) const;
+  virtual void Insert(const sensor::RangeData& range_data,
+                      GridInterface* grid) const override;
 
  private:
-  const proto::RangeDataInserterOptions2D options_;
+  const proto::ProbabilityGridRangeDataInserterOptions2D options_;
   const std::vector<uint16> hit_table_;
   const std::vector<uint16> miss_table_;
 };
@@ -54,4 +59,4 @@ class RangeDataInserter2D {
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_
+#endif  // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_

cartographer/mapping/2d/proto/grid_2d_options.proto

@@ -0,0 +1,27 @@
+// 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;
+
+message GridOptions2D {
+  enum GridType {
+    INVALID_GRID = 0;
+    PROBABILITY_GRID = 1;
+  }
+
+  GridType grid_type = 1;
+  float resolution = 2;
+}

cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto

@@ -16,7 +16,7 @@ syntax = "proto3";
 
 package cartographer.mapping.proto;
 
-message RangeDataInserterOptions2D {
+message ProbabilityGridRangeDataInserterOptions2D {
   // Probability change for a hit (this will be converted to odds and therefore
   // must be greater than 0.5).
   double hit_probability = 1;

cartographer/mapping/2d/proto/submaps_options_2d.proto

@@ -14,18 +14,17 @@
 
 syntax = "proto3";
 
-import "cartographer/mapping/2d/proto/range_data_inserter_options_2d.proto";
+import "cartographer/mapping/2d/proto/grid_2d_options.proto";
+import "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto";
+import "cartographer/mapping/proto/range_data_inserter_options.proto";
 
 package cartographer.mapping.proto;
 
 message SubmapsOptions2D {
-  // Resolution of the map in meters.
-  double resolution = 1;
-
   // Number of range data before adding a new submap. Each submap will get twice
   // the number of range data inserted: First for initialization without being
   // matched against, then while being matched.
-  int32 num_range_data = 3;
+  int32 num_range_data = 1;
-
+  GridOptions2D grid_options_2d = 2;
-  RangeDataInserterOptions2D range_data_inserter_options = 5;
+  RangeDataInserterOptions range_data_inserter_options = 3;
 }

cartographer/mapping/2d/range_data_inserter_2d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 
 #include <memory>
 
@@ -40,8 +40,10 @@ class RangeDataInserterTest2D : public ::testing::Test {
         "hit_probability = 0.7, "
         "miss_probability = 0.4, "
         "}");
-    options_ = CreateRangeDataInserterOptions2D(parameter_dictionary.get());
+    options_ = CreateProbabilityGridRangeDataInserterOptions2D(
-    range_data_inserter_ = common::make_unique<RangeDataInserter2D>(options_);
+        parameter_dictionary.get());
+    range_data_inserter_ =
+        common::make_unique<ProbabilityGridRangeDataInserter2D>(options_);
   }
 
   void InsertPointCloud() {
@@ -57,8 +59,8 @@ class RangeDataInserterTest2D : public ::testing::Test {
   }
 
   ProbabilityGrid probability_grid_;
-  std::unique_ptr<RangeDataInserter2D> range_data_inserter_;
+  std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
-  proto::RangeDataInserterOptions2D options_;
+  proto::ProbabilityGridRangeDataInserterOptions2D options_;
 };
 
 TEST_F(RangeDataInserterTest2D, InsertPointCloud) {

cartographer/mapping/2d/submap_2d.cc

@@ -25,6 +25,8 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+#include "cartographer/mapping/range_data_inserter_interface.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -33,20 +35,37 @@ namespace mapping {
 proto::SubmapsOptions2D CreateSubmapsOptions2D(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::SubmapsOptions2D options;
-  options.set_resolution(parameter_dictionary->GetDouble("resolution"));
   options.set_num_range_data(
       parameter_dictionary->GetNonNegativeInt("num_range_data"));
+  *options.mutable_grid_options_2d() = CreateGridOptions2D(
+      parameter_dictionary->GetDictionary("grid_options_2d").get());
   *options.mutable_range_data_inserter_options() =
-      CreateRangeDataInserterOptions2D(
+      CreateRangeDataInserterOptions(
           parameter_dictionary->GetDictionary("range_data_inserter").get());
+
+  bool valid_range_data_inserter_grid_combination = false;
+  const proto::GridOptions2D_GridType& grid_type =
+      options.grid_options_2d().grid_type();
+  const proto::RangeDataInserterOptions_RangeDataInserterType&
+      range_data_inserter_type =
+          options.range_data_inserter_options().range_data_inserter_type();
+  if (grid_type == proto::GridOptions2D::PROBABILITY_GRID &&
+      range_data_inserter_type ==
+          proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) {
+    valid_range_data_inserter_grid_combination = true;
+  }
+  CHECK(valid_range_data_inserter_grid_combination)
+      << "Invalid combination grid_type " << grid_type
+      << " with range_data_inserter_type " << range_data_inserter_type;
   CHECK_GT(options.num_range_data(), 0);
   return options;
 }
 
-Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin)
+Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid)
     : Submap(transform::Rigid3d::Translation(
-          Eigen::Vector3d(origin.x(), origin.y(), 0.))),
+          Eigen::Vector3d(origin.x(), origin.y(), 0.))) {
-      grid_(common::make_unique<ProbabilityGrid>(limits)) {}
+  grid_ = std::move(grid);
+}
 
 Submap2D::Submap2D(const proto::Submap2D& proto)
     : Submap(transform::ToRigid3(proto.local_pose())) {
@@ -91,11 +110,12 @@ void Submap2D::ToResponseProto(
   grid()->DrawToSubmapTexture(texture, local_pose());
 }
 
-void Submap2D::InsertRangeData(const sensor::RangeData& range_data,
+void Submap2D::InsertRangeData(
-                               const RangeDataInserter2D& range_data_inserter) {
+    const sensor::RangeData& range_data,
+    const RangeDataInserterInterface* range_data_inserter) {
   CHECK(grid_);
   CHECK(!finished());
-  range_data_inserter.Insert(range_data, grid_.get());
+  range_data_inserter->Insert(range_data, grid_.get());
   set_num_range_data(num_range_data() + 1);
 }
 
@@ -108,27 +128,45 @@ void Submap2D::Finish() {
 
 ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
     : options_(options),
-      range_data_inserter_(options.range_data_inserter_options()) {
+      range_data_inserter_(std::move(CreateRangeDataInserter())) {
   // We always want to have at least one likelihood field which we can return,
   // and will create it at the origin in absence of a better choice.
   AddSubmap(Eigen::Vector2f::Zero());
 }
 
-void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
-  for (auto& submap : submaps_) {
-    submap->InsertRangeData(range_data, range_data_inserter_);
-  }
-  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
-    AddSubmap(range_data.origin.head<2>());
-  }
-}
-
 std::vector<std::shared_ptr<Submap2D>> ActiveSubmaps2D::submaps() const {
   return submaps_;
 }
 
 int ActiveSubmaps2D::matching_index() const { return matching_submap_index_; }
 
+void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
+  for (auto& submap : submaps_) {
+    submap->InsertRangeData(range_data, range_data_inserter_.get());
+  }
+  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
+    AddSubmap(range_data.origin.head<2>());
+  }
+}
+
+std::unique_ptr<RangeDataInserterInterface>
+ActiveSubmaps2D::CreateRangeDataInserter() {
+  return common::make_unique<ProbabilityGridRangeDataInserter2D>(
+      options_.range_data_inserter_options()
+          .probability_grid_range_data_inserter_options_2d());
+}
+
+std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
+    const Eigen::Vector2f& origin) {
+  constexpr int kInitialSubmapSize = 100;
+  float resolution = options_.grid_options_2d().resolution();
+  return common::make_unique<ProbabilityGrid>(
+      MapLimits(resolution,
+                origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
+                                            Eigen::Vector2d::Ones(),
+                CellLimits(kInitialSubmapSize, kInitialSubmapSize)));
+}
+
 void ActiveSubmaps2D::FinishSubmap() {
   Submap2D* submap = submaps_.front().get();
   submap->Finish();
@@ -142,14 +180,10 @@ void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
     // reduce peak memory usage a bit.
     FinishSubmap();
   }
-  constexpr int kInitialSubmapSize = 100;
+
   submaps_.push_back(common::make_unique<Submap2D>(
-      MapLimits(options_.resolution(),
+      origin, std::unique_ptr<Grid2D>(
-                origin.cast<double>() + 0.5 * kInitialSubmapSize *
+                  static_cast<Grid2D*>(CreateGrid(origin).release()))));
-                                            options_.resolution() *
-                                            Eigen::Vector2d::Ones(),
-                CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
-      origin));
   LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
 }
 

cartographer/mapping/2d/submap_2d.h

@@ -25,9 +25,9 @@
 #include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/2d/map_limits.h"
 #include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h"
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
 #include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
+#include "cartographer/mapping/range_data_inserter_interface.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/sensor/range_data.h"
@@ -41,7 +41,7 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
 
 class Submap2D : public Submap {
  public:
-  Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin);
+  Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid);
   explicit Submap2D(const proto::Submap2D& proto);
 
   void ToProto(proto::Submap* proto,
@@ -56,7 +56,7 @@ class Submap2D : public Submap {
   // Insert 'range_data' into this submap using 'range_data_inserter'. The
   // submap must not be finished yet.
   void InsertRangeData(const sensor::RangeData& range_data,
-                       const RangeDataInserter2D& range_data_inserter);
+                       const RangeDataInserterInterface* range_data_inserter);
   void Finish();
 
  private:
@@ -89,13 +89,15 @@ class ActiveSubmaps2D {
   std::vector<std::shared_ptr<Submap2D>> submaps() const;
 
  private:
+  std::unique_ptr<RangeDataInserterInterface> CreateRangeDataInserter();
+  std::unique_ptr<GridInterface> CreateGrid(const Eigen::Vector2f& origin);
   void FinishSubmap();
   void AddSubmap(const Eigen::Vector2f& origin);
 
   const proto::SubmapsOptions2D options_;
   int matching_submap_index_ = 0;
   std::vector<std::shared_ptr<Submap2D>> submaps_;
-  RangeDataInserter2D range_data_inserter_;
+  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
 };
 
 }  // namespace mapping

cartographer/mapping/2d/submap_2d_test.cc

@@ -15,6 +15,7 @@
  */
 
 #include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/2d/probability_grid.h"
 
 #include <map>
 #include <memory>
@@ -35,15 +36,21 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
   constexpr int kNumRangeData = 10;
   auto parameter_dictionary = common::MakeDictionary(
       "return {"
-      "resolution = 0.05, "
       "num_range_data = " +
       std::to_string(kNumRangeData) +
       ", "
+      "grid_options_2d = {"
+      "grid_type = \"PROBABILITY_GRID\","
+      "resolution = 0.05, "
+      "},"
       "range_data_inserter = {"
+      "range_data_inserter_type = \"PROBABILITY_GRID_INSERTER_2D\","
+      "probability_grid_range_data_inserter = {"
       "insert_free_space = true, "
       "hit_probability = 0.53, "
       "miss_probability = 0.495, "
       "},"
+      "},"
       "}");
   ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};
   std::set<std::shared_ptr<Submap2D>> all_submaps;
@@ -69,9 +76,10 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
 }
 
 TEST(Submap2DTest, ToFromProto) {
-  Submap2D expected(
+  MapLimits expected_map_limits(1., Eigen::Vector2d(2., 3.),
-      MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
+                                CellLimits(100, 110));
-      Eigen::Vector2f(4.f, 5.f));
+  Submap2D expected(Eigen::Vector2f(4.f, 5.f),
+                    common::make_unique<ProbabilityGrid>(expected_map_limits));
   proto::Submap proto;
   expected.ToProto(&proto, true /* include_probability_grid_data */);
   EXPECT_TRUE(proto.has_submap_2d());

cartographer/mapping/3d/hybrid_grid.h

@@ -407,13 +407,13 @@ class DynamicGrid {
 };
 
 template <typename ValueType>
-using Grid = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;
+using GridBase = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;
 
 // Represents a 3D grid as a wide, shallow tree.
 template <typename ValueType>
-class HybridGridBase : public Grid<ValueType> {
+class HybridGridBase : public GridBase<ValueType> {
  public:
-  using Iterator = typename Grid<ValueType>::Iterator;
+  using Iterator = typename GridBase<ValueType>::Iterator;
 
   // Creates a new tree-based probability grid with voxels having edge length
   // 'resolution' around the origin which becomes the center of the cell at

cartographer/mapping/grid_interface.h

@@ -0,0 +1,33 @@
+/*
+ * 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_GRID_H_
+#define CARTOGRAPHER_MAPPING_GRID_H_
+
+#include <memory>
+#include <utility>
+
+namespace cartographer {
+namespace mapping {
+
+class GridInterface {
+  // todo(kdaun) move mutual functions of Grid2D/3D here
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_GRID_H_

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

@@ -76,7 +76,8 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
     return nullptr;
   }
   if (options_.use_online_correlative_scan_matching()) {
-    // todo(kdaun) add CHECK on options to guarantee grid is a probability grid
+    CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
+             proto::GridOptions2D_GridType_PROBABILITY_GRID);
     double score = real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
         *static_cast<const ProbabilityGrid*>(matching_submap->grid()),

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

@@ -24,7 +24,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/rigid_transform_test_helpers.h"
@@ -50,12 +50,18 @@ class PoseGraph2DTest : public ::testing::Test {
     {
       auto parameter_dictionary = common::MakeDictionary(R"text(
           return {
-            resolution = 0.05,
             num_range_data = 1,
+            grid_options_2d = {
+              grid_type = "PROBABILITY_GRID",
+              resolution = 0.05,
+            },
             range_data_inserter = {
-              insert_free_space = true,
+              range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
-              hit_probability = 0.53,
+              probability_grid_range_data_inserter = {
-              miss_probability = 0.495,
+                insert_free_space = true,
+                hit_probability = 0.53,
+                miss_probability = 0.495,
+              },
             },
           })text");
       active_submaps_ = common::make_unique<ActiveSubmaps2D>(

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc

@@ -24,7 +24,7 @@
 
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 #include "cartographer/transform/rigid_transform_test_helpers.h"
 #include "cartographer/transform/transform.h"
 #include "gtest/gtest.h"
@@ -125,7 +125,7 @@ CreateFastCorrelativeScanMatcherTestOptions2D(
   return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get());
 }
 
-mapping::proto::RangeDataInserterOptions2D
+mapping::proto::ProbabilityGridRangeDataInserterOptions2D
 CreateRangeDataInserterTestOptions2D() {
   auto parameter_dictionary = common::MakeDictionary(R"text(
       return {
@@ -133,13 +133,14 @@ CreateRangeDataInserterTestOptions2D() {
         hit_probability = 0.7,
         miss_probability = 0.4,
       })text");
-  return mapping::CreateRangeDataInserterOptions2D(parameter_dictionary.get());
+  return mapping::CreateProbabilityGridRangeDataInserterOptions2D(
+      parameter_dictionary.get());
 }
 
 TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
   std::mt19937 prng(42);
   std::uniform_real_distribution<float> distribution(-1.f, 1.f);
-  RangeDataInserter2D range_data_inserter(
+  ProbabilityGridRangeDataInserter2D range_data_inserter(
       CreateRangeDataInserterTestOptions2D());
   constexpr float kMinScore = 0.1f;
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
@@ -187,7 +188,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
 TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
   std::mt19937 prng(42);
   std::uniform_real_distribution<float> distribution(-1.f, 1.f);
-  RangeDataInserter2D range_data_inserter(
+  ProbabilityGridRangeDataInserter2D range_data_inserter(
       CreateRangeDataInserterTestOptions2D());
   constexpr float kMinScore = 0.1f;
   const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);

cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc

@@ -23,7 +23,7 @@
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/transform.h"
@@ -46,8 +46,10 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
           "hit_probability = 0.7, "
           "miss_probability = 0.4, "
           "}");
-      range_data_inserter_ = common::make_unique<RangeDataInserter2D>(
+      range_data_inserter_ =
-          CreateRangeDataInserterOptions2D(parameter_dictionary.get()));
+          common::make_unique<ProbabilityGridRangeDataInserter2D>(
+              CreateProbabilityGridRangeDataInserterOptions2D(
+                  parameter_dictionary.get()));
     }
     point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
     point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
@@ -76,7 +78,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
   }
 
   ProbabilityGrid probability_grid_;
-  std::unique_ptr<RangeDataInserter2D> range_data_inserter_;
+  std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
   sensor::PointCloud point_cloud_;
   std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
       real_time_correlative_scan_matcher_;

cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc

@@ -15,6 +15,7 @@
  */
 
 #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
+#include "cartographer/mapping/2d/probability_grid.h"
 
 #include <functional>
 
@@ -71,8 +72,9 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
   node_data.gravity_alignment = Eigen::Quaterniond::Identity();
   node_data.local_pose = transform::Rigid3d::Identity();
   SubmapId submap_id{0, 1};
-  Submap2D submap(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
+  MapLimits map_limits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110));
-                  Eigen::Vector2f(4.f, 5.f));
+  Submap2D submap(Eigen::Vector2f(4.f, 5.f),
+                  common::make_unique<ProbabilityGrid>(map_limits));
   int expected_nodes = 0;
   for (int i = 0; i < 2; ++i) {
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);

cartographer/mapping/map_builder.cc

@@ -130,6 +130,7 @@ int MapBuilder::AddTrajectoryBuilder(
           trimmer_options.min_covered_area() /
               common::Pow2(trajectory_options.trajectory_builder_2d_options()
                                .submaps_options()
+                               .grid_options_2d()
                                .resolution()),
           trimmer_options.min_added_submaps_count()));
     }

cartographer/mapping/proto/range_data_inserter_options.proto

@@ -0,0 +1,30 @@
+// 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";
+
+import "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto";
+
+package cartographer.mapping.proto;
+
+message RangeDataInserterOptions {
+  enum RangeDataInserterType {
+    INVALID_INSERTER = 0;
+    PROBABILITY_GRID_INSERTER_2D = 1;
+  }
+
+  RangeDataInserterType range_data_inserter_type = 1;
+  ProbabilityGridRangeDataInserterOptions2D
+      probability_grid_range_data_inserter_options_2d = 2;
+}

cartographer/mapping/range_data_inserter_interface.cc

@@ -0,0 +1,43 @@
+/*
+ * 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/range_data_inserter_interface.h"
+#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  proto::RangeDataInserterOptions options;
+  const std::string range_data_inserter_type_string =
+      parameter_dictionary->GetString("range_data_inserter_type");
+  proto::RangeDataInserterOptions_RangeDataInserterType
+      range_data_inserter_type;
+  CHECK(proto::RangeDataInserterOptions_RangeDataInserterType_Parse(
+      range_data_inserter_type_string, &range_data_inserter_type))
+      << "Unknown RangeDataInserterOptions_RangeDataInserterType kind: "
+      << range_data_inserter_type_string;
+  options.set_range_data_inserter_type(range_data_inserter_type);
+  *options.mutable_probability_grid_range_data_inserter_options_2d() =
+      CreateProbabilityGridRangeDataInserterOptions2D(
+          parameter_dictionary
+              ->GetDictionary("probability_grid_range_data_inserter")
+              .get());
+  return options;
+}
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/range_data_inserter_interface.h

@@ -0,0 +1,43 @@
+/*
+ * 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_RANGE_DATA_INSERTER_H_
+#define CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_
+
+#include <utility>
+#include <vector>
+
+#include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h"
+#include "cartographer/mapping/grid_interface.h"
+#include "cartographer/sensor/range_data.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
+    common::LuaParameterDictionary* const parameter_dictionary);
+
+class RangeDataInserterInterface {
+ public:
+  // Inserts 'range_data' into 'grid'.
+  virtual void Insert(const sensor::RangeData& range_data,
+                      GridInterface* grid) const = 0;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_

configuration_files/trajectory_builder_2d.lua

@@ -62,12 +62,18 @@ TRAJECTORY_BUILDER_2D = {
   imu_gravity_time_constant = 10.,
 
   submaps = {
-    resolution = 0.05,
     num_range_data = 90,
+    grid_options_2d = {
+      grid_type = "PROBABILITY_GRID",
+      resolution = 0.05,
+    },
     range_data_inserter = {
-      insert_free_space = true,
+      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
-      hit_probability = 0.55,
+      probability_grid_range_data_inserter = {
-      miss_probability = 0.49,
+        insert_free_space = true,
+        hit_probability = 0.55,
+        miss_probability = 0.49,
+      },
     },
   },
 }