Add TSDF support for RealTimeCorrelativeScanMatcher. (#1389)

Continues #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()),
-        &initial_ceres_pose);
+        *matching_submap->grid(), &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) {
-    problem.AddResidualBlock(
-        CreateOccupiedSpaceCostFunction2D(
-            options_.occupied_space_weight() /
-                std::sqrt(static_cast<double>(point_cloud.size())),
-            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.";
+  switch (grid.GetGridType()) {
+    case GridType::PROBABILITY_GRID:
+      problem.AddResidualBlock(
+          CreateOccupiedSpaceCostFunction2D(
+              options_.occupied_space_weight() /
+                  std::sqrt(static_cast<double>(point_cloud.size())),
+              point_cloud, grid),
+          nullptr /* loss function */, ceres_pose_estimate);
+      break;
+    case 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);
+      break;
   }
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(

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

@@ -25,13 +25,56 @@
 #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"
 
 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 ProbabilityGrid& probability_grid,
+    const sensor::PointCloud& point_cloud, const Grid2D& 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,
-                  &candidates);
+  ScoreCandidates(grid, discrete_scans, search_parameters, &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 std::vector<DiscreteScan2D>& discrete_scans,
+    const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
     const SearchParameters& search_parameters,
     std::vector<Candidate2D>* const candidates) const {
   for (Candidate2D& candidate : *candidates) {
-    candidate.score = 0.f;
-    for (const Eigen::Array2i& xy_index :
-         discrete_scans[candidate.scan_index]) {
-      const Eigen::Array2i proposed_xy_index(
-          xy_index.x() + candidate.x_index_offset,
-          xy_index.y() + candidate.y_index_offset);
-      const float probability =
-          probability_grid.GetProbability(proposed_xy_index);
-      candidate.score += probability;
+    switch (grid.GetGridType()) {
+      case GridType::PROBABILITY_GRID:
+        candidate.score = ComputeCandidateScore(
+            static_cast<const ProbabilityGrid&>(grid),
+            discrete_scans[candidate.scan_index], candidate.x_index_offset,
+            candidate.y_index_offset);
+        break;
+      case GridType::TSDF:
+        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 ProbabilityGrid& probability_grid,
+               const sensor::PointCloud& point_cloud, const Grid2D& 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
-  // CostFunctions: http://ceres-solver.org/modeling.html
+  // computed as the sum of probabilities or normalized TSD values, different
+  // 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;