Add TSDF support for RealTimeCorrelativeScanMatcher. (#1389)

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

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

@@ -75,12 +75,9 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
   transform::Rigid2d initial_ceres_pose = pose_prediction;
 
   if (options_.use_online_correlative_scan_matching()) {
-    CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
-             proto::GridOptions2D_GridType_PROBABILITY_GRID);
     const double score = real_time_correlative_scan_matcher_.Match(
         pose_prediction, filtered_gravity_aligned_point_cloud,
-        *static_cast<const ProbabilityGrid*>(matching_submap->grid()),
+        *matching_submap->grid(), &initial_ceres_pose);
-        &initial_ceres_pose);
     kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
   }
 

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

@@ -71,22 +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.);
-  if (grid.GetGridType() == GridType::PROBABILITY_GRID) {
+  switch (grid.GetGridType()) {
-    problem.AddResidualBlock(
+    case 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),
-  } else if (grid.GetGridType() == GridType::TSDF) {
+          nullptr /* loss function */, ceres_pose_estimate);
-    problem.AddResidualBlock(
+      break;
-        CreateTSDFMatchCostFunction2D(
+    case GridType::TSDF:
-            options_.occupied_space_weight() /
+      problem.AddResidualBlock(
-                std::sqrt(static_cast<double>(point_cloud.size())),
+          CreateTSDFMatchCostFunction2D(
-            point_cloud, static_cast<const TSDF2D&>(grid)),
+              options_.occupied_space_weight() /
-        nullptr /* loss function */, ceres_pose_estimate);
+                  std::sqrt(static_cast<double>(point_cloud.size())),
-  } else {
+              point_cloud, static_cast<const TSDF2D&>(grid)),
-    LOG(FATAL) << "Unknown GridType.";
+          nullptr /* loss function */, ceres_pose_estimate);
+      break;
   }
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(

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

@@ -25,6 +25,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/tsdf_2d.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
@@ -32,6 +33,48 @@
 namespace cartographer {
 namespace mapping {
 namespace scan_matching {
+namespace {
+
+float ComputeCandidateScore(const TSDF2D& tsdf,
+                            const DiscreteScan2D& discrete_scan,
+                            int x_index_offset, int y_index_offset) {
+  float candidate_score = 0.f;
+  float summed_weight = 0.f;
+  for (const Eigen::Array2i& xy_index : discrete_scan) {
+    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
+                                           xy_index.y() + y_index_offset);
+    const std::pair<float, float> tsd_and_weight =
+        tsdf.GetTSDAndWeight(proposed_xy_index);
+    const float normalized_tsd_score =
+        (tsdf.GetMaxCorrespondenceCost() - std::abs(tsd_and_weight.first)) /
+        tsdf.GetMaxCorrespondenceCost();
+    const float weight = tsd_and_weight.second;
+    candidate_score += normalized_tsd_score * weight;
+    summed_weight += weight;
+  }
+  if (summed_weight == 0.f) return 0.f;
+  candidate_score /= summed_weight;
+  CHECK_GE(candidate_score, 0.f);
+  return candidate_score;
+}
+
+float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
+                            const DiscreteScan2D& discrete_scan,
+                            int x_index_offset, int y_index_offset) {
+  float candidate_score = 0.f;
+  for (const Eigen::Array2i& xy_index : discrete_scan) {
+    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
+                                           xy_index.y() + y_index_offset);
+    const float probability =
+        probability_grid.GetProbability(proposed_xy_index);
+    candidate_score += probability;
+  }
+  candidate_score /= static_cast<float>(discrete_scan.size());
+  CHECK_GT(candidate_score, 0.f);
+  return candidate_score;
+}
+
+}  // namespace
 
 RealTimeCorrelativeScanMatcher2D::RealTimeCorrelativeScanMatcher2D(
     const proto::RealTimeCorrelativeScanMatcherOptions& options)
@@ -73,8 +116,7 @@ RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
 
 double RealTimeCorrelativeScanMatcher2D::Match(
     const transform::Rigid2d& initial_pose_estimate,
-    const sensor::PointCloud& point_cloud,
+    const sensor::PointCloud& point_cloud, const Grid2D& grid,
-    const ProbabilityGrid& probability_grid,
     transform::Rigid2d* pose_estimate) const {
   CHECK_NOTNULL(pose_estimate);
 
@@ -85,18 +127,17 @@ double RealTimeCorrelativeScanMatcher2D::Match(
           initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
   const SearchParameters search_parameters(
       options_.linear_search_window(), options_.angular_search_window(),
-      rotated_point_cloud, probability_grid.limits().resolution());
+      rotated_point_cloud, grid.limits().resolution());
 
   const std::vector<sensor::PointCloud> rotated_scans =
       GenerateRotatedScans(rotated_point_cloud, search_parameters);
   const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
-      probability_grid.limits(), rotated_scans,
+      grid.limits(), rotated_scans,
       Eigen::Translation2f(initial_pose_estimate.translation().x(),
                            initial_pose_estimate.translation().y()));
   std::vector<Candidate2D> candidates =
       GenerateExhaustiveSearchCandidates(search_parameters);
-  ScoreCandidates(probability_grid, discrete_scans, search_parameters,
+  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
-                  &candidates);
 
   const Candidate2D& best_candidate =
       *std::max_element(candidates.begin(), candidates.end());
@@ -108,29 +149,29 @@ double RealTimeCorrelativeScanMatcher2D::Match(
 }
 
 void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
-    const ProbabilityGrid& probability_grid,
+    const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
-    const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters,
     std::vector<Candidate2D>* const candidates) const {
   for (Candidate2D& candidate : *candidates) {
-    candidate.score = 0.f;
+    switch (grid.GetGridType()) {
-    for (const Eigen::Array2i& xy_index :
+      case GridType::PROBABILITY_GRID:
-         discrete_scans[candidate.scan_index]) {
+        candidate.score = ComputeCandidateScore(
-      const Eigen::Array2i proposed_xy_index(
+            static_cast<const ProbabilityGrid&>(grid),
-          xy_index.x() + candidate.x_index_offset,
+            discrete_scans[candidate.scan_index], candidate.x_index_offset,
-          xy_index.y() + candidate.y_index_offset);
+            candidate.y_index_offset);
-      const float probability =
+        break;
-          probability_grid.GetProbability(proposed_xy_index);
+      case GridType::TSDF:
-      candidate.score += probability;
+        candidate.score = ComputeCandidateScore(
+            static_cast<const TSDF2D&>(grid),
+            discrete_scans[candidate.scan_index], candidate.x_index_offset,
+            candidate.y_index_offset);
+        break;
     }
-    candidate.score /=
-        static_cast<float>(discrete_scans[candidate.scan_index].size());
     candidate.score *=
         std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
                                    options_.translation_delta_cost_weight() +
                                std::abs(candidate.orientation) *
                                    options_.rotation_delta_cost_weight()));
-    CHECK_GT(candidate.score, 0.f);
   }
 }
 

cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h

@@ -41,7 +41,7 @@
 #include <vector>
 
 #include "Eigen/Core"
-#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
 #include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h"
 
@@ -60,20 +60,19 @@ class RealTimeCorrelativeScanMatcher2D {
   RealTimeCorrelativeScanMatcher2D& operator=(
       const RealTimeCorrelativeScanMatcher2D&) = delete;
 
-  // Aligns 'point_cloud' within the 'probability_grid' given an
+  // Aligns 'point_cloud' within the 'grid' given an
   // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
   // returns the score.
   double Match(const transform::Rigid2d& initial_pose_estimate,
-               const sensor::PointCloud& point_cloud,
+               const sensor::PointCloud& point_cloud, const Grid2D& grid,
-               const ProbabilityGrid& probability_grid,
                transform::Rigid2d* pose_estimate) const;
 
   // Computes the score for each Candidate2D in a collection. The cost is
-  // computed as the sum of probabilities, different from the Ceres
+  // computed as the sum of probabilities or normalized TSD values, different
-  // CostFunctions: http://ceres-solver.org/modeling.html
+  // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html
   //
   // Visible for testing.
-  void ScoreCandidates(const ProbabilityGrid& probability_grid,
+  void ScoreCandidates(const Grid2D& grid,
                        const std::vector<DiscreteScan2D>& discrete_scans,
                        const SearchParameters& search_parameters,
                        std::vector<Candidate2D>* candidates) const;

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

@@ -24,6 +24,8 @@
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/tsdf_2d.h"
+#include "cartographer/mapping/2d/tsdf_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"
@@ -34,12 +36,66 @@ namespace mapping {
 namespace scan_matching {
 namespace {
 
+proto::RealTimeCorrelativeScanMatcherOptions
+CreateRealTimeCorrelativeScanMatcherTestOptions2D() {
+  auto parameter_dictionary = common::MakeDictionary(
+      "return {"
+      "linear_search_window = 0.6, "
+      "angular_search_window = 0.16, "
+      "translation_delta_cost_weight = 0., "
+      "rotation_delta_cost_weight = 0., "
+      "}");
+  return CreateRealTimeCorrelativeScanMatcherOptions(
+      parameter_dictionary.get());
+}
+
 class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
  protected:
-  RealTimeCorrelativeScanMatcherTest()
+  RealTimeCorrelativeScanMatcherTest() {
-      : probability_grid_(
+    point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
-            MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)),
+    point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
-            &conversion_tables_) {
+    point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
+    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
+    real_time_correlative_scan_matcher_ =
+        absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
+            CreateRealTimeCorrelativeScanMatcherTestOptions2D());
+  }
+
+  void SetUpTSDF() {
+    grid_ = absl::make_unique<TSDF2D>(
+        MapLimits(0.05, Eigen::Vector2d(0.3, 0.5), CellLimits(20, 20)), 0.3,
+        1.0, &conversion_tables_);
+    {
+      auto parameter_dictionary = common::MakeDictionary(R"text(
+      return {
+        truncation_distance = 0.3,
+        maximum_weight = 10.,
+        update_free_space = false,
+        normal_estimation_options = {
+          num_normal_samples = 4,
+          sample_radius = 0.5,
+        },
+        project_sdf_distance_to_scan_normal = true,
+        update_weight_range_exponent = 0,
+        update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5,
+        update_weight_distance_cell_to_hit_kernel_bandwith = 0.5,
+      })text");
+      range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(
+          CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()));
+    }
+    range_data_inserter_->Insert(
+        sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}},
+        grid_.get());
+    grid_->FinishUpdate();
+  }
+
+  void SetUpProbabilityGrid() {
+    grid_ = absl::make_unique<ProbabilityGrid>(
+        MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)),
+        &conversion_tables_);
     {
       auto parameter_dictionary = common::MakeDictionary(
           "return { "
@@ -52,51 +108,31 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
               CreateProbabilityGridRangeDataInserterOptions2D(
                   parameter_dictionary.get()));
     }
-    point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
-    point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
     range_data_inserter_->Insert(
         sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
-        &probability_grid_);
+        grid_.get());
-    probability_grid_.FinishUpdate();
+    grid_->FinishUpdate();
-    {
-      auto parameter_dictionary = common::MakeDictionary(
-          "return {"
-          "linear_search_window = 0.6, "
-          "angular_search_window = 0.16, "
-          "translation_delta_cost_weight = 0., "
-          "rotation_delta_cost_weight = 0., "
-          "}");
-      real_time_correlative_scan_matcher_ =
-          absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
-              CreateRealTimeCorrelativeScanMatcherOptions(
-                  parameter_dictionary.get()));
-    }
   }
 
   ValueConversionTables conversion_tables_;
-  ProbabilityGrid probability_grid_;
+  std::unique_ptr<Grid2D> grid_;
-  std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
+  std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
   sensor::PointCloud point_cloud_;
   std::unique_ptr<RealTimeCorrelativeScanMatcher2D>
       real_time_correlative_scan_matcher_;
 };
 
 TEST_F(RealTimeCorrelativeScanMatcherTest,
-       ScorePerfectHighResolutionCandidate) {
+       ScorePerfectHighResolutionCandidateProbabilityGrid) {
+  SetUpProbabilityGrid();
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
-  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans =
-      probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
+      DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
   std::vector<Candidate2D> candidates;
   candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
   real_time_correlative_scan_matcher_->ScoreCandidates(
-      probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
+      *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
-      &candidates);
   EXPECT_EQ(0, candidates[0].scan_index);
   EXPECT_EQ(0, candidates[0].x_index_offset);
   EXPECT_EQ(0, candidates[0].y_index_offset);
@@ -105,16 +141,35 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
 }
 
 TEST_F(RealTimeCorrelativeScanMatcherTest,
-       ScorePartiallyCorrectHighResolutionCandidate) {
+       ScorePerfectHighResolutionCandidateTSDF) {
+  SetUpTSDF();
   const std::vector<sensor::PointCloud> scans =
       GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
-  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
+  const std::vector<DiscreteScan2D> discrete_scans =
-      probability_grid_.limits(), scans, Eigen::Translation2f::Identity());
+      DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
+  std::vector<Candidate2D> candidates;
+  candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
+  real_time_correlative_scan_matcher_->ScoreCandidates(
+      *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
+  EXPECT_EQ(0, candidates[0].scan_index);
+  EXPECT_EQ(0, candidates[0].x_index_offset);
+  EXPECT_EQ(0, candidates[0].y_index_offset);
+  // Every point should align perfectly.
+  EXPECT_NEAR(1.0, candidates[0].score, 1e-1);
+  EXPECT_LT(0.95, candidates[0].score);
+}
+
+TEST_F(RealTimeCorrelativeScanMatcherTest,
+       ScorePartiallyCorrectHighResolutionCandidateProbabilityGrid) {
+  SetUpProbabilityGrid();
+  const std::vector<sensor::PointCloud> scans =
+      GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
+  const std::vector<DiscreteScan2D> discrete_scans =
+      DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
   std::vector<Candidate2D> candidates;
   candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
   real_time_correlative_scan_matcher_->ScoreCandidates(
-      probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.),
+      *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
-      &candidates);
   EXPECT_EQ(0, candidates[0].scan_index);
   EXPECT_EQ(0, candidates[0].x_index_offset);
   EXPECT_EQ(1, candidates[0].y_index_offset);
@@ -123,6 +178,25 @@ TEST_F(RealTimeCorrelativeScanMatcherTest,
   EXPECT_GT(0.7, candidates[0].score);
 }
 
+TEST_F(RealTimeCorrelativeScanMatcherTest,
+       ScorePartiallyCorrectHighResolutionCandidateTSDF) {
+  SetUpTSDF();
+  const std::vector<sensor::PointCloud> scans =
+      GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.));
+  const std::vector<DiscreteScan2D> discrete_scans =
+      DiscretizeScans(grid_->limits(), scans, Eigen::Translation2f::Identity());
+  std::vector<Candidate2D> candidates;
+  candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
+  real_time_correlative_scan_matcher_->ScoreCandidates(
+      *grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates);
+  EXPECT_EQ(0, candidates[0].scan_index);
+  EXPECT_EQ(0, candidates[0].x_index_offset);
+  EXPECT_EQ(1, candidates[0].y_index_offset);
+  // 3 points should align perfectly.
+  EXPECT_LT(1.0 - 4. / (7. * 6.), candidates[0].score);
+  EXPECT_GT(1.0, candidates[0].score);
+}
+
 }  // namespace
 }  // namespace scan_matching
 }  // namespace mapping