Integrate TSDF components into SLAM pipeline. (#1361)

[RFC=0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)

cartographer/cloud/internal/client_server_test.cc

@@ -75,7 +75,8 @@ constexpr char kOdometryDataProtoString[] = "odometry_data {}";
 constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}";
 constexpr char kLandmarkDataProtoString[] = "landmark_data {}";
 
-class ClientServerTest : public ::testing::Test {
+template <class T>
+class ClientServerTestBase : public T {
  protected:
   void SetUp() override {
     // TODO(cschuet): Due to the hard-coded addresses these tests will become
@@ -178,6 +179,32 @@ class ClientServerTest : public ::testing::Test {
     EXPECT_TRUE(stub_for_uploading_server_ != nullptr);
   }
 
+  void SetOptionsToTSDF2D() {
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_submaps_options()
+        ->mutable_range_data_inserter_options()
+        ->set_range_data_inserter_type(
+            ::cartographer::mapping::proto::RangeDataInserterOptions::
+                TSDF_INSERTER_2D);
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_submaps_options()
+        ->mutable_grid_options_2d()
+        ->set_grid_type(::cartographer::mapping::proto::GridOptions2D::TSDF);
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_ceres_scan_matcher_options()
+        ->set_occupied_space_weight(10.0);
+    map_builder_server_options_.mutable_map_builder_options()
+        ->mutable_pose_graph_options()
+        ->mutable_constraint_builder_options()
+        ->mutable_ceres_scan_matcher_options()
+        ->set_occupied_space_weight(50.0);
+    uploading_map_builder_server_options_.mutable_map_builder_options()
+        ->mutable_pose_graph_options()
+        ->mutable_constraint_builder_options()
+        ->mutable_ceres_scan_matcher_options()
+        ->set_occupied_space_weight(50.0);
+  }
+
   void WaitForLocalSlamResults(size_t size) {
     std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
     local_slam_result_condition_.wait(
@@ -212,13 +239,26 @@ class ClientServerTest : public ::testing::Test {
   int number_of_insertion_results_;
 };
 
+class ClientServerTest : public ClientServerTestBase<::testing::Test> {};
+class ClientServerTestByGridType
+    : public ClientServerTestBase<
+          ::testing::TestWithParam<::cartographer::mapping::GridType>> {};
+
+INSTANTIATE_TEST_CASE_P(
+    ClientServerTestByGridType, ClientServerTestByGridType,
+    ::testing::Values(::cartographer::mapping::GridType::PROBABILITY_GRID,
+                      ::cartographer::mapping::GridType::TSDF));
+
 TEST_F(ClientServerTest, StartAndStopServer) {
   InitializeRealServer();
   server_->Start();
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, AddTrajectoryBuilder) {
+TEST_P(ClientServerTestByGridType, AddTrajectoryBuilder) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -231,7 +271,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
+TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithMock) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeServerWithMockMapBuilder();
   server_->Start();
   InitializeStub();
@@ -250,7 +293,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, AddSensorData) {
+TEST_P(ClientServerTestByGridType, AddSensorData) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   trajectory_builder_options_.mutable_trajectory_builder_2d_options()
       ->set_use_imu_data(true);
   InitializeRealServer();
@@ -268,7 +314,10 @@ TEST_F(ClientServerTest, AddSensorData) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, AddSensorDataWithMock) {
+TEST_P(ClientServerTestByGridType, AddSensorDataWithMock) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeServerWithMockMapBuilder();
   server_->Start();
   InitializeStub();
@@ -297,7 +346,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, LocalSlam2D) {
+TEST_P(ClientServerTestByGridType, LocalSlam2D) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -328,7 +380,10 @@ TEST_F(ClientServerTest, LocalSlam2D) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, LocalSlamAndDelete2D) {
+TEST_P(ClientServerTestByGridType, LocalSlamAndDelete2D) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -401,7 +456,10 @@ TEST_F(ClientServerTest, GlobalSlam3D) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) {
+TEST_P(ClientServerTestByGridType, StartAndStopUploadingServerAndServer) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeRealUploadingServer();
@@ -410,7 +468,10 @@ TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) {
+TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithUploadingServer) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeRealUploadingServer();
@@ -433,7 +494,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
+TEST_P(ClientServerTestByGridType, LocalSlam2DWithUploadingServer) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -482,7 +546,10 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
   server_->Shutdown();
 }
 
-TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) {
+TEST_P(ClientServerTestByGridType, LocalSlam2DUplinkServerRestarting) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -528,7 +595,10 @@ TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) {
   server_->WaitForShutdown();
 }
 
-TEST_F(ClientServerTest, LoadStateAndDelete) {
+TEST_P(ClientServerTestByGridType, LoadStateAndDelete) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();
@@ -568,7 +638,10 @@ TEST_F(ClientServerTest, LoadStateAndDelete) {
 
 // TODO(gaschler): Test-cover LoadStateFromFile.
 
-TEST_F(ClientServerTest, LocalSlam2DHandlesInvalidRequests) {
+TEST_P(ClientServerTestByGridType, LocalSlam2DHandlesInvalidRequests) {
+  if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
+    SetOptionsToTSDF2D();
+  }
   InitializeRealServer();
   server_->Start();
   InitializeStub();

cartographer/mapping/2d/grid_2d.cc

@@ -63,7 +63,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
                float max_correspondence_cost,
                ValueConversionTables* conversion_tables)
     : limits_(limits),
-      grid_type_(GridType::NONE),
       correspondence_cost_cells_(
           limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
           kUnknownCorrespondenceValue),
@@ -78,7 +77,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
 Grid2D::Grid2D(const proto::Grid2D& proto,
                ValueConversionTables* conversion_tables)
     : limits_(proto.limits()),
-      grid_type_(GridType::NONE),
       correspondence_cost_cells_(),
       min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)),
       max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)),

cartographer/mapping/2d/grid_2d.h

@@ -32,7 +32,7 @@ namespace mapping {
 proto::GridOptions2D CreateGridOptions2D(
     common::LuaParameterDictionary* const parameter_dictionary);
 
-enum class GridType { NONE, PROBABILITY_GRID, TSDF };
+enum class GridType { PROBABILITY_GRID, TSDF };
 
 class Grid2D : public GridInterface {
  public:
@@ -45,13 +45,14 @@ class Grid2D : public GridInterface {
   // Returns the limits of this Grid2D.
   const MapLimits& limits() const { return limits_; }
 
-  GridType grid_type() const { return grid_type_; }
-
   // Finishes the update sequence.
   void FinishUpdate();
+
   // Returns the correspondence cost of the cell with 'cell_index'.
   float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
 
+  virtual GridType GetGridType() const = 0;
+
   // Returns the minimum possible correspondence cost.
   float GetMinCorrespondenceCost() const { return min_correspondence_cost_; }
 
@@ -96,7 +97,6 @@ class Grid2D : public GridInterface {
     return &correspondence_cost_cells_;
   }
 
-  GridType* mutable_grid_type() { return &grid_type_; }
   std::vector<int>* mutable_update_indices() { return &update_indices_; }
   Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
 
@@ -105,7 +105,6 @@ class Grid2D : public GridInterface {
 
  private:
   MapLimits limits_;
-  GridType grid_type_;
   std::vector<uint16> correspondence_cost_cells_;
   float min_correspondence_cost_;
   float max_correspondence_cost_;

cartographer/mapping/2d/probability_grid.cc

@@ -28,15 +28,12 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
                                  ValueConversionTables* conversion_tables)
     : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
              conversion_tables),
-      conversion_tables_(conversion_tables) {
+      conversion_tables_(conversion_tables) {}
-  *mutable_grid_type() = GridType::PROBABILITY_GRID;
-}
 
 ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
                                  ValueConversionTables* conversion_tables)
     : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
   CHECK(proto.has_probability_grid_2d());
-  *mutable_grid_type() = GridType::PROBABILITY_GRID;
 }
 
 // Sets the probability of the cell at 'cell_index' to the given
@@ -73,6 +70,10 @@ bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
   return true;
 }
 
+GridType ProbabilityGrid::GetGridType() const {
+  return GridType::PROBABILITY_GRID;
+}
+
 // Returns the probability of the cell with 'cell_index'.
 float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
   if (!limits().Contains(cell_index)) return kMinProbability;

cartographer/mapping/2d/probability_grid.h

@@ -50,12 +50,14 @@ class ProbabilityGrid : public Grid2D {
   bool ApplyLookupTable(const Eigen::Array2i& cell_index,
                         const std::vector<uint16>& table);
 
+  GridType GetGridType() const override;
+
   // Returns the probability of the cell with 'cell_index'.
   float GetProbability(const Eigen::Array2i& cell_index) const;
 
-  virtual proto::Grid2D ToProto() const override;
+  proto::Grid2D ToProto() const override;
-  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
+  std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
-  virtual bool DrawToSubmapTexture(
+  bool DrawToSubmapTexture(
       proto::SubmapQuery::Response::SubmapTexture* const texture,
       transform::Rigid3d local_pose) const override;
 

cartographer/mapping/2d/probability_grid_test.cc

@@ -44,7 +44,7 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
   ValueConversionTables conversion_tables;
   ProbabilityGrid grid(proto, &conversion_tables);
   EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
-  EXPECT_EQ(grid.grid_type(), GridType::PROBABILITY_GRID);
+  EXPECT_EQ(grid.GetGridType(), GridType::PROBABILITY_GRID);
 
   // TODO(macmason): Figure out how to test the contents of cells_ and
   // {min, max}_{x, y}_ gracefully.
@@ -55,7 +55,7 @@ TEST(ProbabilityGridTest, ConstructorGridType) {
   ProbabilityGrid probability_grid(
       MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
       &conversion_tables);
-  EXPECT_EQ(probability_grid.grid_type(), GridType::PROBABILITY_GRID);
+  EXPECT_EQ(probability_grid.GetGridType(), GridType::PROBABILITY_GRID);
 }
 
 TEST(ProbabilityGridTest, ToProto) {

cartographer/mapping/2d/submap_2d.cc

@@ -26,6 +26,7 @@
 #include "absl/memory/memory.h"
 #include "cartographer/common/port.h"
 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
 #include "cartographer/mapping/range_data_inserter_interface.h"
 #include "glog/logging.h"
 
@@ -54,6 +55,11 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
           proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) {
     valid_range_data_inserter_grid_combination = true;
   }
+  if (grid_type == proto::GridOptions2D::TSDF &&
+      range_data_inserter_type ==
+          proto::RangeDataInserterOptions::TSDF_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;
@@ -74,22 +80,26 @@ Submap2D::Submap2D(const proto::Submap2D& proto,
     : Submap(transform::ToRigid3(proto.local_pose())),
       conversion_tables_(conversion_tables) {
   if (proto.has_grid()) {
-    CHECK(proto.grid().has_probability_grid_2d());
+    if (proto.grid().has_probability_grid_2d()) {
-    grid_ =
+      grid_ =
-        absl::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_);
+          absl::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_);
+    } else if (proto.grid().has_tsdf_2d()) {
+      grid_ = absl::make_unique<TSDF2D>(proto.grid(), conversion_tables_);
+    } else {
+      LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
+    }
   }
   set_num_range_data(proto.num_range_data());
   set_finished(proto.finished());
 }
 
-proto::Submap Submap2D::ToProto(
+proto::Submap Submap2D::ToProto(const bool include_grid_data) const {
-    const bool include_probability_grid_data) const {
   proto::Submap proto;
   auto* const submap_2d = proto.mutable_submap_2d();
   *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
   submap_2d->set_num_range_data(num_range_data());
   submap_2d->set_finished(finished());
-  if (include_probability_grid_data) {
+  if (include_grid_data) {
     CHECK(grid_);
     *submap_2d->mutable_grid() = grid_->ToProto();
   }
@@ -102,9 +112,15 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) {
   set_num_range_data(submap_2d.num_range_data());
   set_finished(submap_2d.finished());
   if (proto.submap_2d().has_grid()) {
-    CHECK(proto.submap_2d().grid().has_probability_grid_2d());
+    if (proto.submap_2d().grid().has_probability_grid_2d()) {
-    grid_ = absl::make_unique<ProbabilityGrid>(submap_2d.grid(),
+      grid_ = absl::make_unique<ProbabilityGrid>(proto.submap_2d().grid(),
-                                               conversion_tables_);
+                                                 conversion_tables_);
+    } else if (proto.submap_2d().grid().has_tsdf_2d()) {
+      grid_ = absl::make_unique<TSDF2D>(proto.submap_2d().grid(),
+                                        conversion_tables_);
+    } else {
+      LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
+    }
   }
 }
 
@@ -159,21 +175,50 @@ std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
 
 std::unique_ptr<RangeDataInserterInterface>
 ActiveSubmaps2D::CreateRangeDataInserter() {
-  return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
+  switch (options_.range_data_inserter_options().range_data_inserter_type()) {
-      options_.range_data_inserter_options()
+    case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
-          .probability_grid_range_data_inserter_options_2d());
+      return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
+          options_.range_data_inserter_options()
+              .probability_grid_range_data_inserter_options_2d());
+    case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
+      return absl::make_unique<TSDFRangeDataInserter2D>(
+          options_.range_data_inserter_options()
+              .tsdf_range_data_inserter_options_2d());
+    default:
+      LOG(FATAL) << "Unknown RangeDataInserterType.";
+  }
 }
 
 std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
     const Eigen::Vector2f& origin) {
   constexpr int kInitialSubmapSize = 100;
   float resolution = options_.grid_options_2d().resolution();
-  return absl::make_unique<ProbabilityGrid>(
+  switch (options_.grid_options_2d().grid_type()) {
-      MapLimits(resolution,
+    case proto::GridOptions2D::PROBABILITY_GRID:
-                origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
+      return absl::make_unique<ProbabilityGrid>(
-                                            Eigen::Vector2d::Ones(),
+          MapLimits(resolution,
-                CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
+                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
-      &conversion_tables_);
+                                                resolution *
+                                                Eigen::Vector2d::Ones(),
+                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
+          &conversion_tables_);
+    case proto::GridOptions2D::TSDF:
+      return absl::make_unique<TSDF2D>(
+          MapLimits(resolution,
+                    origin.cast<double>() + 0.5 * kInitialSubmapSize *
+                                                resolution *
+                                                Eigen::Vector2d::Ones(),
+                    CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
+          options_.range_data_inserter_options()
+              .tsdf_range_data_inserter_options_2d()
+              .truncation_distance(),
+          options_.range_data_inserter_options()
+              .tsdf_range_data_inserter_options_2d()
+              .maximum_weight(),
+          &conversion_tables_);
+    default:
+      LOG(FATAL) << "Unknown GridType.";
+  }
 }
 
 void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {

cartographer/mapping/2d/submap_2d.h

@@ -47,7 +47,7 @@ class Submap2D : public Submap {
   explicit Submap2D(const proto::Submap2D& proto,
                     ValueConversionTables* conversion_tables);
 
-  proto::Submap ToProto(bool include_probability_grid_data) const override;
+  proto::Submap ToProto(bool include_grid_data) const override;
   void UpdateFromProto(const proto::Submap& proto) override;
 
   void ToResponseProto(const transform::Rigid3d& global_submap_pose,

cartographer/mapping/2d/tsdf_2d.cc

@@ -30,15 +30,12 @@ TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
           truncation_distance, max_weight, conversion_tables_)),
       weight_cells_(
           limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells,
-          value_converter_->getUnknownWeightValue()) {
+          value_converter_->getUnknownWeightValue()) {}
-  *mutable_grid_type() = GridType::TSDF;
-}
 
 TSDF2D::TSDF2D(const proto::Grid2D& proto,
                ValueConversionTables* conversion_tables)
     : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
   CHECK(proto.has_tsdf_2d());
-  *mutable_grid_type() = GridType::TSDF;
   value_converter_ = absl::make_unique<TSDValueConverter>(
       proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(),
       conversion_tables_);
@@ -70,6 +67,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
   *weight_cell = value_converter_->WeightToValue(weight);
 }
 
+GridType TSDF2D::GetGridType() const { return GridType::TSDF; }
+
 float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {
   if (limits().Contains(cell_index)) {
     return value_converter_->ValueToTSD(

cartographer/mapping/2d/tsdf_2d.h

@@ -37,15 +37,16 @@ class TSDF2D : public Grid2D {
 
   void SetCell(const Eigen::Array2i& cell_index, const float tsd,
                const float weight);
+  GridType GetGridType() const override;
   float GetTSD(const Eigen::Array2i& cell_index) const;
   float GetWeight(const Eigen::Array2i& cell_index) const;
   std::pair<float, float> GetTSDAndWeight(
       const Eigen::Array2i& cell_index) const;
 
-  virtual void GrowLimits(const Eigen::Vector2f& point) override;
+  void GrowLimits(const Eigen::Vector2f& point) override;
   proto::Grid2D ToProto() const override;
-  virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
+  std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
-  virtual bool DrawToSubmapTexture(
+  bool DrawToSubmapTexture(
       proto::SubmapQuery::Response::SubmapTexture* const texture,
       transform::Rigid3d local_pose) const override;
   bool CellIsUpdated(const Eigen::Array2i& cell_index) const;

cartographer/mapping/2d/tsdf_2d_test.cc

@@ -48,7 +48,7 @@ TEST(TSDF2DTest, ProtoConstructor) {
 
   ValueConversionTables conversion_tables;
   TSDF2D grid(proto, &conversion_tables);
-  EXPECT_EQ(grid.grid_type(), GridType::TSDF);
+  EXPECT_EQ(grid.GetGridType(), GridType::TSDF);
   EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
 }
 
@@ -56,7 +56,7 @@ TEST(TSDF2DTest, ConstructorGridType) {
   ValueConversionTables conversion_tables;
   TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
               10.0f, &conversion_tables);
-  EXPECT_EQ(tsdf.grid_type(), GridType::TSDF);
+  EXPECT_EQ(tsdf.GetGridType(), GridType::TSDF);
 }
 
 TEST(TSDF2DTest, ToProto) {

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

@@ -26,6 +26,7 @@
 #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h"
 #include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h"
 #include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h"
+#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
 #include "glog/logging.h"
@@ -70,12 +71,23 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                                    initial_pose_estimate.rotation().angle()};
   ceres::Problem problem;
   CHECK_GT(options_.occupied_space_weight(), 0.);
-  problem.AddResidualBlock(
+  if (grid.GetGridType() == GridType::PROBABILITY_GRID) {
-      CreateOccupiedSpaceCostFunction2D(
+    problem.AddResidualBlock(
-          options_.occupied_space_weight() /
+        CreateOccupiedSpaceCostFunction2D(
-              std::sqrt(static_cast<double>(point_cloud.size())),
+            options_.occupied_space_weight() /
-          point_cloud, grid),
+                std::sqrt(static_cast<double>(point_cloud.size())),
-      nullptr /* loss function */, ceres_pose_estimate);
+            point_cloud, grid),
+        nullptr /* loss function */, ceres_pose_estimate);
+  } else if (grid.GetGridType() == GridType::TSDF) {
+    problem.AddResidualBlock(
+        CreateTSDFMatchCostFunction2D(
+            options_.occupied_space_weight() /
+                std::sqrt(static_cast<double>(point_cloud.size())),
+            point_cloud, static_cast<const TSDF2D&>(grid)),
+        nullptr /* loss function */, ceres_pose_estimate);
+  } else {
+    LOG(FATAL) << "Unknown GridType.";
+  }
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
       TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(

cartographer/mapping/map_builder_test.cc

@@ -18,6 +18,7 @@
 
 #include "cartographer/common/config.h"
 #include "cartographer/io/proto_stream.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/internal/testing/test_helpers.h"
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"
@@ -34,7 +35,8 @@ constexpr double kDuration = 4.;         // Seconds.
 constexpr double kTimeStep = 0.1;        // Seconds.
 constexpr double kTravelDistance = 1.2;  // Meters.
 
-class MapBuilderTest : public ::testing::Test {
+template <class T>
+class MapBuilderTestBase : public T {
  protected:
   void SetUp() override {
     // Global SLAM optimization is not executed.
@@ -70,6 +72,25 @@ class MapBuilderTest : public ::testing::Test {
     map_builder_options_.set_use_trajectory_builder_3d(true);
   }
 
+  void SetOptionsToTSDF2D() {
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_submaps_options()
+        ->mutable_range_data_inserter_options()
+        ->set_range_data_inserter_type(
+            proto::RangeDataInserterOptions::TSDF_INSERTER_2D);
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_submaps_options()
+        ->mutable_grid_options_2d()
+        ->set_grid_type(proto::GridOptions2D::TSDF);
+    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+        ->mutable_ceres_scan_matcher_options()
+        ->set_occupied_space_weight(10.0);
+    map_builder_options_.mutable_pose_graph_options()
+        ->mutable_constraint_builder_options()
+        ->mutable_ceres_scan_matcher_options()
+        ->set_occupied_space_weight(50.0);
+  }
+
   void SetOptionsEnableGlobalOptimization() {
     map_builder_options_.mutable_pose_graph_options()
         ->set_optimize_every_n_nodes(3);
@@ -111,7 +132,26 @@ class MapBuilderTest : public ::testing::Test {
   std::vector<::cartographer::transform::Rigid3d> local_slam_result_poses_;
 };
 
-TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
+class MapBuilderTest : public MapBuilderTestBase<::testing::Test> {};
+class MapBuilderTestByGridType
+    : public MapBuilderTestBase<::testing::TestWithParam<GridType>> {};
+class MapBuilderTestByGridTypeAndDimensions
+    : public MapBuilderTestBase<
+          ::testing::TestWithParam<std::pair<GridType, int /* dimensions */>>> {
+};
+INSTANTIATE_TEST_CASE_P(MapBuilderTestByGridType, MapBuilderTestByGridType,
+                        ::testing::Values(GridType::PROBABILITY_GRID,
+                                          GridType::TSDF));
+INSTANTIATE_TEST_CASE_P(
+    MapBuilderTestByGridTypeAndDimensions,
+    MapBuilderTestByGridTypeAndDimensions,
+    ::testing::Values(std::make_pair(GridType::PROBABILITY_GRID, 2),
+                      std::make_pair(GridType::PROBABILITY_GRID, 3),
+                      std::make_pair(GridType::TSDF, 2)));
+
+TEST_P(MapBuilderTestByGridTypeAndDimensions, TrajectoryAddFinish) {
+  if (GetParam().second == 3) SetOptionsTo3D();
+  if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D();
   BuildMapBuilder();
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
       {kRangeSensorId}, trajectory_builder_options_,
@@ -124,21 +164,8 @@ TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
   EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
 }
 
-TEST_F(MapBuilderTest, TrajectoryAddFinish3D) {
+TEST_P(MapBuilderTestByGridType, LocalSlam2D) {
-  SetOptionsTo3D();
+  if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
-  BuildMapBuilder();
-  int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      {kRangeSensorId}, trajectory_builder_options_,
-      nullptr /* GetLocalSlamResultCallbackForSubscriptions */);
-  EXPECT_EQ(1, map_builder_->num_trajectory_builders());
-  EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr);
-  EXPECT_TRUE(map_builder_->pose_graph() != nullptr);
-  map_builder_->FinishTrajectory(trajectory_id);
-  map_builder_->pose_graph()->RunFinalOptimization();
-  EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
-}
-
-TEST_F(MapBuilderTest, LocalSlam2D) {
   BuildMapBuilder();
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
       {kRangeSensorId}, trajectory_builder_options_,
@@ -187,7 +214,8 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
               0.1 * kTravelDistance);
 }
 
-TEST_F(MapBuilderTest, GlobalSlam2D) {
+TEST_P(MapBuilderTestByGridType, GlobalSlam2D) {
+  if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
   SetOptionsEnableGlobalOptimization();
   BuildMapBuilder();
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
@@ -268,7 +296,8 @@ TEST_F(MapBuilderTest, GlobalSlam3D) {
               0.1 * kTravelDistance);
 }
 
-TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) {
+TEST_P(MapBuilderTestByGridType, DeleteFinishedTrajectory2D) {
+  if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
   SetOptionsEnableGlobalOptimization();
   BuildMapBuilder();
   int trajectory_id = CreateTrajectoryWithFakeData();
@@ -309,60 +338,57 @@ TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) {
       0);
 }
 
-TEST_F(MapBuilderTest, SaveLoadState) {
+TEST_P(MapBuilderTestByGridTypeAndDimensions, SaveLoadState) {
-  for (int dimensions : {2, 3}) {
+  if (GetParam().second == 3) SetOptionsTo3D();
-    if (dimensions == 3) {
+  if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D();
-      SetOptionsTo3D();
+  trajectory_builder_options_.mutable_trajectory_builder_2d_options()
-    }
+      ->set_use_imu_data(true);
-    trajectory_builder_options_.mutable_trajectory_builder_2d_options()
+  BuildMapBuilder();
-        ->set_use_imu_data(true);
+  int trajectory_id = map_builder_->AddTrajectoryBuilder(
-    BuildMapBuilder();
+      {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_,
-    int trajectory_id = map_builder_->AddTrajectoryBuilder(
+      GetLocalSlamResultCallback());
-        {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_,
+  TrajectoryBuilderInterface* trajectory_builder =
-        GetLocalSlamResultCallback());
+      map_builder_->GetTrajectoryBuilder(trajectory_id);
-    TrajectoryBuilderInterface* trajectory_builder =
+  const auto measurements = testing::GenerateFakeRangeMeasurements(
-        map_builder_->GetTrajectoryBuilder(trajectory_id);
+      kTravelDistance, kDuration, kTimeStep);
-    const auto measurements = testing::GenerateFakeRangeMeasurements(
+  for (const auto& measurement : measurements) {
-        kTravelDistance, kDuration, kTimeStep);
+    trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
-    for (const auto& measurement : measurements) {
+    trajectory_builder->AddSensorData(
-      trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
+        kIMUSensorId.id,
-      trajectory_builder->AddSensorData(
+        sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
-          kIMUSensorId.id,
+                        Eigen::Vector3d::Zero()});
-          sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
-                          Eigen::Vector3d::Zero()});
-    }
-    map_builder_->FinishTrajectory(trajectory_id);
-    map_builder_->pose_graph()->RunFinalOptimization();
-    int num_constraints = map_builder_->pose_graph()->constraints().size();
-    int num_nodes =
-        map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
-            trajectory_id);
-    EXPECT_GT(num_constraints, 0);
-    EXPECT_GT(num_nodes, 0);
-    // TODO(gaschler): Consider using in-memory to avoid side effects.
-    const std::string filename = "temp-SaveLoadState.pbstream";
-    io::ProtoStreamWriter writer(filename);
-    map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
-    writer.Close();
-
-    // Reset 'map_builder_'.
-    BuildMapBuilder();
-    io::ProtoStreamReader reader(filename);
-    auto trajectory_remapping =
-        map_builder_->LoadState(&reader, false /* load_frozen_state */);
-    map_builder_->pose_graph()->RunFinalOptimization();
-    EXPECT_EQ(num_constraints,
-              map_builder_->pose_graph()->constraints().size());
-    ASSERT_EQ(trajectory_remapping.size(), 1);
-    int new_trajectory_id = trajectory_remapping.begin()->second;
-    EXPECT_EQ(
-        num_nodes,
-        map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
-            new_trajectory_id));
   }
+  map_builder_->FinishTrajectory(trajectory_id);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  int num_constraints = map_builder_->pose_graph()->constraints().size();
+  int num_nodes =
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          trajectory_id);
+  EXPECT_GT(num_constraints, 0);
+  EXPECT_GT(num_nodes, 0);
+  // TODO(gaschler): Consider using in-memory to avoid side effects.
+  const std::string filename = "temp-SaveLoadState.pbstream";
+  io::ProtoStreamWriter writer(filename);
+  map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
+  writer.Close();
+
+  // Reset 'map_builder_'.
+  BuildMapBuilder();
+  io::ProtoStreamReader reader(filename);
+  auto trajectory_remapping =
+      map_builder_->LoadState(&reader, false /* load_frozen_state */);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_EQ(num_constraints, map_builder_->pose_graph()->constraints().size());
+  ASSERT_EQ(trajectory_remapping.size(), 1);
+  int new_trajectory_id = trajectory_remapping.begin()->second;
+  EXPECT_EQ(
+      num_nodes,
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          new_trajectory_id));
 }
 
-TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) {
+TEST_P(MapBuilderTestByGridType, LocalizationOnFrozenTrajectory2D) {
+  if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
   BuildMapBuilder();
   int temp_trajectory_id = CreateTrajectoryWithFakeData();
   map_builder_->pose_graph()->RunFinalOptimization();

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

@@ -20,6 +20,7 @@ message GridOptions2D {
   enum GridType {
     INVALID_GRID = 0;
     PROBABILITY_GRID = 1;
+    TSDF = 2;
   }
 
   GridType grid_type = 1;

cartographer/mapping/submaps.h

@@ -62,7 +62,7 @@ class Submap {
       : local_pose_(local_submap_pose) {}
   virtual ~Submap() {}
 
-  virtual proto::Submap ToProto(bool include_probability_grid_data) const = 0;
+  virtual proto::Submap ToProto(bool include_grid_data) const = 0;
   virtual void UpdateFromProto(const proto::Submap& proto) = 0;
 
   // Fills data into the 'response'.

configuration_files/trajectory_builder_2d.lua

@@ -82,10 +82,10 @@ TRAJECTORY_BUILDER_2D = {
           num_normal_samples = 4,
           sample_radius = 0.5,
         },
-        project_sdf_distance_to_scan_normal = false,
+        project_sdf_distance_to_scan_normal = true,
         update_weight_range_exponent = 0,
-        update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,
+        update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5,
-        update_weight_distance_cell_to_hit_kernel_bandwith = 0,
+        update_weight_distance_cell_to_hit_kernel_bandwith = 0.5,
       },
     },
   },