Base ConstraintBuilder2D on correspondence cost function (#1088)

- Base `ConstraintBuilder2D` and `FastCorrelativeScanMatcher2D` on correspondence cost function instead of probabilities
- No changes in the options of `ConstraintBuilder2D` and `FastCorrelativeScanMatcher2D`
- Add properties `min/max_correspondence_cost` to `Grid2D` and `proto::Grid2D`
- Provide backwards compatibility for `proto::Grid2D`
 - Step towards [RFC 0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)

cartographer/mapping/2d/grid_2d.cc

@@ -20,11 +20,16 @@
 namespace cartographer {
 namespace mapping {
 
-Grid2D::Grid2D(const MapLimits& limits)
+Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
+               float max_correspondence_cost)
     : limits_(limits),
       correspondence_cost_cells_(
           limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
-          kUnknownCorrespondenceValue) {}
+          kUnknownCorrespondenceValue),
+      min_correspondence_cost_(min_correspondence_cost),
+      max_correspondence_cost_(max_correspondence_cost) {
+  CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
+}
 
 Grid2D::Grid2D(const proto::Grid2D& proto)
     : limits_(proto.limits()), correspondence_cost_cells_() {
@@ -39,6 +44,19 @@ Grid2D::Grid2D(const proto::Grid2D& proto)
     CHECK_LE(cell, std::numeric_limits<uint16>::max());
     correspondence_cost_cells_.push_back(cell);
   }
+  if (proto.min_correspondence_cost() == 0.f &&
+      proto.max_correspondence_cost() == 0.f) {
+    LOG(WARNING)
+        << "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
+           "are initialized with 0 indicating an older version of the "
+           "protobuf format. Loading default values.";
+    min_correspondence_cost_ = kMinCorrespondenceCost;
+    max_correspondence_cost_ = kMaxCorrespondenceCost;
+  } else {
+    min_correspondence_cost_ = proto.min_correspondence_cost();
+    max_correspondence_cost_ = proto.max_correspondence_cost();
+  }
+  CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
 }
 
 // Finishes the update sequence.
@@ -51,21 +69,14 @@ void Grid2D::FinishUpdate() {
   }
 }
 
-// Sets the probability of the cell at 'cell_index' to the given
+// Returns the correspondence cost of the cell with 'cell_index'.
-// 'probability'. Only allowed if the cell was unknown before.
-void Grid2D::SetCorrespondenceCost(const Eigen::Array2i& cell_index,
-                                   const float correspondence_cost) {
-  LOG(FATAL) << "Not implemented";
-}
-
-// Returns the probability of the cell with 'cell_index'.
 float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const {
   if (!limits().Contains(cell_index)) return kMaxCorrespondenceCost;
   return ValueToCorrespondenceCost(
       correspondence_cost_cells()[ToFlatIndex(cell_index)]);
 }
 
-// Returns true if the probability at the specified index is known.
+// Returns true if the correspondence cost at the specified index is known.
 bool Grid2D::IsKnown(const Eigen::Array2i& cell_index) const {
   return limits_.Contains(cell_index) &&
          correspondence_cost_cells_[ToFlatIndex(cell_index)] !=
@@ -136,6 +147,8 @@ proto::Grid2D Grid2D::ToProto() const {
     box->set_min_x(known_cells_box().min().x());
     box->set_min_y(known_cells_box().min().y());
   }
+  result.set_min_correspondence_cost(min_correspondence_cost_);
+  result.set_max_correspondence_cost(max_correspondence_cost_);
   return result;
 }
 

cartographer/mapping/2d/grid_2d.h

@@ -27,23 +27,24 @@ namespace mapping {
 
 class Grid2D {
  public:
-  explicit Grid2D(const MapLimits& limits);
+  explicit Grid2D(const MapLimits& limits, float min_correspondence_cost,
+                  float max_correspondence_cost);
   explicit Grid2D(const proto::Grid2D& proto);
 
-  // Returns the limits of this ProbabilityGrid.
+  // Returns the limits of this Grid2D.
   const MapLimits& limits() const { return limits_; }
 
   // Finishes the update sequence.
   void FinishUpdate();
-
-  // Sets the correspondence cost of the cell at 'cell_index' to the given
-  // 'correspondence_cost'. Only allowed if the cell was unknown before.
-  void SetCorrespondenceCost(const Eigen::Array2i& cell_index,
-                             const float correspondence_cost);
-
   // Returns the correspondence cost of the cell with 'cell_index'.
   float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
 
+  // Returns the minimum possible correspondence cost.
+  float GetMinCorrespondenceCost() const { return min_correspondence_cost_; }
+
+  // Returns the maximum possible correspondence cost.
+  float GetMaxCorrespondenceCost() const { return max_correspondence_cost_; }
+
   // Returns true if the probability at the specified index is known.
   bool IsKnown(const Eigen::Array2i& cell_index) const;
 
@@ -80,6 +81,8 @@ class Grid2D {
  private:
   MapLimits limits_;
   std::vector<uint16> correspondence_cost_cells_;
+  float min_correspondence_cost_;
+  float max_correspondence_cost_;
   std::vector<int> update_indices_;
 
   // Bounding box of known cells to efficiently compute cropping limits.

cartographer/mapping/2d/probability_grid.cc

@@ -22,7 +22,8 @@
 namespace cartographer {
 namespace mapping {
 
-ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) : Grid2D(limits) {}
+ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
+    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost) {}
 
 ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto) : Grid2D(proto) {
   CHECK(proto.has_probability_grid_2d());

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

@@ -35,4 +35,6 @@ message Grid2D {
   oneof grid {
     ProbabilityGrid probability_grid_2d = 4;
   }
+  float min_correspondence_cost = 6;
+  float max_correspondence_cost = 7;
 }

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc

@@ -130,7 +130,7 @@ void ConstraintBuilder2D::WhenDone(
 }
 
 void ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-    const SubmapId& submap_id, const ProbabilityGrid* const submap,
+    const SubmapId& submap_id, const Grid2D* const grid,
     const std::function<void()>& work_item) {
   if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
       nullptr) {
@@ -139,18 +139,18 @@ void ConstraintBuilder2D::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
     submap_queued_work_items_[submap_id].push_back(work_item);
     if (submap_queued_work_items_[submap_id].size() == 1) {
       thread_pool_->Schedule(
-          [=]() { ConstructSubmapScanMatcher(submap_id, submap); });
+          [=]() { ConstructSubmapScanMatcher(submap_id, grid); });
     }
   }
 }
 
-void ConstraintBuilder2D::ConstructSubmapScanMatcher(
+void ConstraintBuilder2D::ConstructSubmapScanMatcher(const SubmapId& submap_id,
-    const SubmapId& submap_id, const ProbabilityGrid* const submap) {
+                                                     const Grid2D* const grid) {
   auto submap_scan_matcher =
       common::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
-          *submap, options_.fast_correlative_scan_matcher_options());
+          *grid, options_.fast_correlative_scan_matcher_options());
   common::MutexLocker locker(&mutex_);
-  submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
+  submap_scan_matchers_[submap_id] = {grid, std::move(submap_scan_matcher)};
   for (const std::function<void()>& work_item :
        submap_queued_work_items_[submap_id]) {
     thread_pool_->Schedule(work_item);
@@ -227,8 +227,8 @@ void ConstraintBuilder2D::ComputeConstraint(
   ceres::Solver::Summary unused_summary;
   ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                             constant_data->filtered_gravity_aligned_point_cloud,
-                            *submap_scan_matcher->probability_grid,
+                            *submap_scan_matcher->grid, &pose_estimate,
-                            &pose_estimate, &unused_summary);
+                            &unused_summary);
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h

@@ -105,7 +105,7 @@ class ConstraintBuilder2D {
 
  private:
   struct SubmapScanMatcher {
-    const ProbabilityGrid* probability_grid;
+    const Grid2D* grid;
     std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
         fast_correlative_scan_matcher;
   };
@@ -113,12 +113,11 @@ class ConstraintBuilder2D {
   // Either schedules the 'work_item', or if needed, schedules the scan matcher
   // construction and queues the 'work_item'.
   void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      const SubmapId& submap_id, const ProbabilityGrid* submap,
+      const SubmapId& submap_id, const Grid2D* grid,
       const std::function<void()>& work_item) REQUIRES(mutex_);
 
   // Constructs the scan matcher for a 'submap', then schedules its work items.
-  void ConstructSubmapScanMatcher(const SubmapId& submap_id,
+  void ConstructSubmapScanMatcher(const SubmapId& submap_id, const Grid2D* grid)
-                                  const ProbabilityGrid* submap)
       EXCLUDES(mutex_);
 
   // Returns the scan matcher for a submap, which has to exist.

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

@@ -25,7 +25,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
-#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
@@ -89,11 +89,13 @@ CreateFastCorrelativeScanMatcherOptions2D(
 }
 
 PrecomputationGrid2D::PrecomputationGrid2D(
-    const ProbabilityGrid& probability_grid, const CellLimits& limits,
+    const Grid2D& grid, const CellLimits& limits, const int width,
-    const int width, std::vector<float>* reusable_intermediate_grid)
+    std::vector<float>* reusable_intermediate_grid)
     : offset_(-width + 1, -width + 1),
       wide_limits_(limits.num_x_cells + width - 1,
                    limits.num_y_cells + width - 1),
+      min_score_(1.f - grid.GetMaxCorrespondenceCost()),
+      max_score_(1.f - grid.GetMinCorrespondenceCost()),
       cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) {
   CHECK_GE(width, 1);
   CHECK_GE(limits.num_x_cells, 1);
@@ -106,26 +108,26 @@ PrecomputationGrid2D::PrecomputationGrid2D(
   for (int y = 0; y != limits.num_y_cells; ++y) {
     SlidingWindowMaximum current_values;
     current_values.AddValue(
-        probability_grid.GetProbability(Eigen::Array2i(0, y)));
+        1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(0, y))));
     for (int x = -width + 1; x != 0; ++x) {
       intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
       if (x + width < limits.num_x_cells) {
-        current_values.AddValue(
+        current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
-            probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
+                                          Eigen::Array2i(x + width, y))));
       }
     }
     for (int x = 0; x < limits.num_x_cells - width; ++x) {
       intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
       current_values.RemoveValue(
-          probability_grid.GetProbability(Eigen::Array2i(x, y)));
+          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
-      current_values.AddValue(
+      current_values.AddValue(1.f - std::abs(grid.GetCorrespondenceCost(
-          probability_grid.GetProbability(Eigen::Array2i(x + width, y)));
+                                        Eigen::Array2i(x + width, y))));
     }
     for (int x = std::max(limits.num_x_cells - width, 0);
          x != limits.num_x_cells; ++x) {
       intermediate[x + width - 1 + y * stride] = current_values.GetMaximum();
       current_values.RemoveValue(
-          probability_grid.GetProbability(Eigen::Array2i(x, y)));
+          1.f - std::abs(grid.GetCorrespondenceCost(Eigen::Array2i(x, y))));
     }
     current_values.CheckIsEmpty();
   }
@@ -159,38 +161,37 @@ PrecomputationGrid2D::PrecomputationGrid2D(
 }
 
 uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
-  const int cell_value =
+  const int cell_value = common::RoundToInt(
-      common::RoundToInt((probability - kMinProbability) *
+      (probability - min_score_) * (255.f / (max_score_ - min_score_)));
-                         (255.f / (kMaxProbability - kMinProbability)));
   CHECK_GE(cell_value, 0);
   CHECK_LE(cell_value, 255);
   return cell_value;
 }
 
 PrecomputationGridStack2D::PrecomputationGridStack2D(
-    const ProbabilityGrid& probability_grid,
+    const Grid2D& grid,
     const proto::FastCorrelativeScanMatcherOptions2D& options) {
   CHECK_GE(options.branch_and_bound_depth(), 1);
   const int max_width = 1 << (options.branch_and_bound_depth() - 1);
   precomputation_grids_.reserve(options.branch_and_bound_depth());
   std::vector<float> reusable_intermediate_grid;
-  const CellLimits limits = probability_grid.limits().cell_limits();
+  const CellLimits limits = grid.limits().cell_limits();
   reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
                                      limits.num_y_cells);
   for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
     const int width = 1 << i;
-    precomputation_grids_.emplace_back(probability_grid, limits, width,
+    precomputation_grids_.emplace_back(grid, limits, width,
                                        &reusable_intermediate_grid);
   }
 }
 
 FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
-    const ProbabilityGrid& probability_grid,
+    const Grid2D& grid,
     const proto::FastCorrelativeScanMatcherOptions2D& options)
     : options_(options),
-      limits_(probability_grid.limits()),
+      limits_(grid.limits()),
-      precomputation_grid_stack_(common::make_unique<PrecomputationGridStack2D>(
+      precomputation_grid_stack_(
-          probability_grid, options)) {}
+          common::make_unique<PrecomputationGridStack2D>(grid, options)) {}
 
 FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}
 
@@ -324,7 +325,7 @@ void FastCorrelativeScanMatcher2D::ScoreCandidates(
           xy_index.y() + candidate.y_index_offset);
       sum += precomputation_grid.GetValue(proposed_xy_index);
     }
-    candidate.score = PrecomputationGrid2D::ToProbability(
+    candidate.score = precomputation_grid.ToScore(
         sum / static_cast<float>(discrete_scans[candidate.scan_index].size()));
   }
   std::sort(candidates->begin(), candidates->end(),

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

@@ -30,10 +30,9 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/port.h"
-#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/2d/grid_2d.h"
 #include "cartographer/mapping/2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.pb.h"
 #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
-#include "cartographer/mapping/probability_values.h"
 #include "cartographer/sensor/point_cloud.h"
 
 namespace cartographer {
@@ -49,12 +48,11 @@ CreateFastCorrelativeScanMatcherOptions2D(
 // y0 <= y < y0.
 class PrecomputationGrid2D {
  public:
-  PrecomputationGrid2D(const ProbabilityGrid& probability_grid,
+  PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width,
-                       const CellLimits& limits, int width,
                        std::vector<float>* reusable_intermediate_grid);
 
   // Returns a value between 0 and 255 to represent probabilities between
-  // kMinProbability and kMaxProbability.
+  // min_score and max_score.
   int GetValue(const Eigen::Array2i& xy_index) const {
     const Eigen::Array2i local_xy_index = xy_index - offset_;
     // The static_cast<unsigned> is for performance to check with 2 comparisons
@@ -72,10 +70,9 @@ class PrecomputationGrid2D {
     return cells_[local_xy_index.x() + local_xy_index.y() * stride];
   }
 
-  // Maps values from [0, 255] to [kMinProbability, kMaxProbability].
+  // Maps values from [0, 255] to [min_score, max_score].
-  static float ToProbability(float value) {
+  float ToScore(float value) const {
-    return kMinProbability +
+    return min_score_ + value * ((max_score_ - min_score_) / 255.f);
-           value * ((kMaxProbability - kMinProbability) / 255.f);
   }
 
  private:
@@ -88,6 +85,9 @@ class PrecomputationGrid2D {
   // Size of the precomputation grid.
   const CellLimits wide_limits_;
 
+  const float min_score_;
+  const float max_score_;
+
   // Probabilites mapped to 0 to 255.
   std::vector<uint8> cells_;
 };
@@ -95,7 +95,7 @@ class PrecomputationGrid2D {
 class PrecomputationGridStack2D {
  public:
   PrecomputationGridStack2D(
-      const ProbabilityGrid& probability_grid,
+      const Grid2D& grid,
       const proto::FastCorrelativeScanMatcherOptions2D& options);
 
   const PrecomputationGrid2D& Get(int index) {
@@ -112,7 +112,7 @@ class PrecomputationGridStack2D {
 class FastCorrelativeScanMatcher2D {
  public:
   FastCorrelativeScanMatcher2D(
-      const ProbabilityGrid& probability_grid,
+      const Grid2D& grid,
       const proto::FastCorrelativeScanMatcherOptions2D& options);
   ~FastCorrelativeScanMatcher2D();
 
@@ -120,7 +120,7 @@ class FastCorrelativeScanMatcher2D {
   FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) =
       delete;
 
-  // Aligns 'point_cloud' within the 'probability_grid' given an
+  // Aligns 'point_cloud' within the 'grid' given an
   // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
   // is possible, true is returned, and 'score' and 'pose_estimate' are updated
   // with the result.
@@ -128,7 +128,7 @@ class FastCorrelativeScanMatcher2D {
              const sensor::PointCloud& point_cloud, float min_score,
              float* score, transform::Rigid2d* pose_estimate) const;
 
-  // Aligns 'point_cloud' within the full 'probability_grid', i.e., not
+  // Aligns 'point_cloud' within the full 'grid', i.e., not
   // restricted to the configured search window. If a score above 'min_score'
   // (excluding equality) is possible, true is returned, and 'score' and
   // 'pose_estimate' are updated with the result.

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

@@ -41,13 +41,17 @@ TEST(PrecomputationGridTest, CorrectValues) {
   std::uniform_int_distribution<int> distribution(0, 255);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
+  std::vector<float> reusable_intermediate_grid;
+  PrecomputationGrid2D precomputation_grid_dummy(
+      probability_grid, probability_grid.limits().cell_limits(), 1,
+      &reusable_intermediate_grid);
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
     probability_grid.SetProbability(
-        xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
+        xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
   }
 
-  std::vector<float> reusable_intermediate_grid;
+  reusable_intermediate_grid.clear();
   for (const int width : {1, 2, 3, 8}) {
     PrecomputationGrid2D precomputation_grid(
         probability_grid, probability_grid.limits().cell_limits(), width,
@@ -62,9 +66,9 @@ TEST(PrecomputationGridTest, CorrectValues) {
               probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
         }
       }
-      EXPECT_NEAR(max_score,
+      EXPECT_NEAR(
-                  PrecomputationGrid2D::ToProbability(
+          max_score,
-                      precomputation_grid.GetValue(xy_index)),
+          precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
           1e-4);
     }
   }
@@ -75,13 +79,17 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
   std::uniform_int_distribution<int> distribution(0, 255);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
+  std::vector<float> reusable_intermediate_grid;
+  PrecomputationGrid2D precomputation_grid_dummy(
+      probability_grid, probability_grid.limits().cell_limits(), 1,
+      &reusable_intermediate_grid);
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
     probability_grid.SetProbability(
-        xy_index, PrecomputationGrid2D::ToProbability(distribution(prng)));
+        xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
   }
 
-  std::vector<float> reusable_intermediate_grid;
+  reusable_intermediate_grid.clear();
   for (const int width : {1, 2, 3, 8, 200}) {
     PrecomputationGrid2D precomputation_grid(
         probability_grid, probability_grid.limits().cell_limits(), width,
@@ -96,9 +104,9 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) {
               probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
         }
       }
-      EXPECT_NEAR(max_score,
+      EXPECT_NEAR(
-                  PrecomputationGrid2D::ToProbability(
+          max_score,
-                      precomputation_grid.GetValue(xy_index)),
+          precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
           1e-4);
     }
   }