Correspondence cost based probability grid (#1081)

- Store probabilities as correspondence costs
- Add conversion functions for correspondence cost values
- Step towards [RFC 0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)

cartographer/mapping/2d/grid_2d.h

@@ -37,11 +37,11 @@ class Grid2D {
   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.
+  // 'correspondence_cost'. Only allowed if the cell was unknown before.
   void SetCorrespondenceCost(const Eigen::Array2i& cell_index,
                              const float correspondence_cost);
 
-  // Returns the probability of the cell with 'cell_index'.
+  // Returns the correspondence cost of the cell with 'cell_index'.
   float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
 
   // Returns true if the probability at the specified index is known.

cartographer/mapping/2d/probability_grid.cc

@@ -35,7 +35,8 @@ void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
   uint16& cell =
       (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
   CHECK_EQ(cell, kUnknownProbabilityValue);
-  cell = ProbabilityToValue(probability);
+  cell =
+      CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
   mutable_known_cells_box()->extend(cell_index.matrix());
 }
 
@@ -64,8 +65,8 @@ bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
 // 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;
-  return ValueToProbability(
+  return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
-      correspondence_cost_cells()[ToFlatIndex(cell_index)]);
+      correspondence_cost_cells()[ToFlatIndex(cell_index)]));
 }
 proto::Grid2D ProbabilityGrid::ToProto() const {
   proto::Grid2D result;

cartographer/mapping/2d/probability_grid_test.cc

@@ -76,29 +76,34 @@ TEST(ProbabilityGridTest, ApplyOdds) {
 
   probability_grid.SetProbability(Array2i(1, 0), 0.5);
 
-  probability_grid.ApplyLookupTable(Array2i(1, 0),
+  probability_grid.ApplyLookupTable(
-                                    ComputeLookupTableToApplyOdds(Odds(0.9)));
+      Array2i(1, 0),
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
   probability_grid.FinishUpdate();
   EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
 
   probability_grid.SetProbability(Array2i(0, 1), 0.5);
-  probability_grid.ApplyLookupTable(Array2i(0, 1),
+  probability_grid.ApplyLookupTable(
-                                    ComputeLookupTableToApplyOdds(Odds(0.1)));
+      Array2i(0, 1),
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.1)));
   probability_grid.FinishUpdate();
   EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
 
   // Tests adding odds to an unknown cell.
-  probability_grid.ApplyLookupTable(Array2i(1, 1),
+  probability_grid.ApplyLookupTable(
-                                    ComputeLookupTableToApplyOdds(Odds(0.42)));
+      Array2i(1, 1),
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.42)));
   EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
 
   // Tests that further updates are ignored if FinishUpdate() isn't called.
-  probability_grid.ApplyLookupTable(Array2i(1, 1),
+  probability_grid.ApplyLookupTable(
-                                    ComputeLookupTableToApplyOdds(Odds(0.9)));
+      Array2i(1, 1),
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
   EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
   probability_grid.FinishUpdate();
-  probability_grid.ApplyLookupTable(Array2i(1, 1),
+  probability_grid.ApplyLookupTable(
-                                    ComputeLookupTableToApplyOdds(Odds(0.9)));
+      Array2i(1, 1),
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
   EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
 }
 

cartographer/mapping/2d/range_data_inserter_2d.cc

@@ -47,10 +47,10 @@ proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D(
 RangeDataInserter2D::RangeDataInserter2D(
     const proto::RangeDataInserterOptions2D& options)
     : options_(options),
-      hit_table_(
+      hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
-          ComputeLookupTableToApplyOdds(Odds(options.hit_probability()))),
+          Odds(options.hit_probability()))),
-      miss_table_(
+      miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
-          ComputeLookupTableToApplyOdds(Odds(options.miss_probability()))) {}
+          Odds(options.miss_probability()))) {}
 
 void RangeDataInserter2D::Insert(
     const sensor::RangeData& range_data,

cartographer/mapping/probability_values.cc

@@ -16,39 +16,59 @@
 
 #include "cartographer/mapping/probability_values.h"
 
+#include "cartographer/common/make_unique.h"
+
 namespace cartographer {
 namespace mapping {
 
 namespace {
 
-// 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability].
+// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
-float SlowValueToProbability(const uint16 value) {
+float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value,
+                              const float unknown_result,
+                              const float lower_bound,
+                              const float upper_bound) {
   CHECK_GE(value, 0);
   CHECK_LE(value, 32767);
-  if (value == kUnknownProbabilityValue) {
+  if (value == unknown_value) return unknown_result;
-    // Unknown cells have kMinProbability.
+  const float kScale = (upper_bound - lower_bound) / 32766.f;
-    return kMinProbability;
+  return value * kScale + (lower_bound - kScale);
-  }
-  const float kScale = (kMaxProbability - kMinProbability) / 32766.f;
-  return value * kScale + (kMinProbability - kScale);
 }
 
-const std::vector<float>* PrecomputeValueToProbability() {
+std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(
-  std::vector<float>* result = new std::vector<float>;
+    const uint16 unknown_value, const float unknown_result,
+    const float lower_bound, const float upper_bound) {
+  auto result = common::make_unique<std::vector<float>>();
   // Repeat two times, so that both values with and without the update marker
   // can be converted to a probability.
   for (int repeat = 0; repeat != 2; ++repeat) {
     for (int value = 0; value != 32768; ++value) {
-      result->push_back(SlowValueToProbability(value));
+      result->push_back(SlowValueToBoundedFloat(
+          value, unknown_value, unknown_result, lower_bound, upper_bound));
     }
   }
   return result;
 }
 
+std::unique_ptr<std::vector<float>> PrecomputeValueToProbability() {
+  return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue,
+                                       kMinProbability, kMinProbability,
+                                       kMaxProbability);
+}
+
+std::unique_ptr<std::vector<float>> PrecomputeValueToCorrespondenceCost() {
+  return PrecomputeValueToBoundedFloat(
+      kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
+      kMinCorrespondenceCost, kMaxCorrespondenceCost);
+}
+
 }  // namespace
 
 const std::vector<float>* const kValueToProbability =
-    PrecomputeValueToProbability();
+    PrecomputeValueToProbability().release();
+
+const std::vector<float>* const kValueToCorrespondenceCost =
+    PrecomputeValueToCorrespondenceCost().release();
 
 std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
   std::vector<uint16> result;
@@ -62,5 +82,22 @@ std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
   return result;
 }
 
+std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
+    float odds) {
+  std::vector<uint16> result;
+  result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
+                       ProbabilityFromOdds(odds))) +
+                   kUpdateMarker);
+  for (int cell = 1; cell != 32768; ++cell) {
+    result.push_back(
+        CorrespondenceCostToValue(
+            ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
+                odds * Odds(CorrespondenceCostToProbability(
+                           (*kValueToCorrespondenceCost)[cell]))))) +
+        kUpdateMarker);
+  }
+  return result;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/probability_values.h

@@ -27,6 +27,24 @@
 namespace cartographer {
 namespace mapping {
 
+namespace {
+
+inline uint16 BoundedFloatToValue(const float float_value,
+                                  const float lower_bound,
+                                  const float upper_bound) {
+  const int value =
+      common::RoundToInt(
+          (common::Clamp(float_value, lower_bound, upper_bound) - lower_bound) *
+          (32766.f / (upper_bound - lower_bound))) +
+      1;
+  // DCHECK for performance.
+  DCHECK_GE(value, 1);
+  DCHECK_LE(value, 32767);
+  return value;
+}
+
+}  // namespace
+
 inline float Odds(float probability) {
   return probability / (1.f - probability);
 }
@@ -35,31 +53,47 @@ inline float ProbabilityFromOdds(const float odds) {
   return odds / (odds + 1.f);
 }
 
+inline float ProbabilityToCorrespondenceCost(const float probability) {
+  return 1.f - probability;
+}
+
+inline float CorrespondenceCostToProbability(const float correspondence_cost) {
+  return 1.f - correspondence_cost;
+}
+
 constexpr float kMinProbability = 0.1f;
 constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
 
 // Clamps probability to be in the range [kMinProbability, kMaxProbability].
 inline float ClampProbability(const float probability) {
   return common::Clamp(probability, kMinProbability, kMaxProbability);
 }
+// Clamps correspondece cost to be in the range [kMinCorrespondenceCost,
+// kMaxCorrespondenceCost].
+inline float ClampCorrespondenceCost(const float correspondence_cost) {
+  return common::Clamp(correspondence_cost, kMinCorrespondenceCost,
+                       kMaxCorrespondenceCost);
+}
 
 constexpr uint16 kUnknownProbabilityValue = 0;
-constexpr uint16 kUnknownCorrespondenceValue = 0;
+constexpr uint16 kUnknownCorrespondenceValue = kUnknownProbabilityValue;
 constexpr uint16 kUpdateMarker = 1u << 15;
 
+// Converts a correspondence_cost to a uint16 in the [1, 32767] range.
+inline uint16 CorrespondenceCostToValue(const float correspondence_cost) {
+  return BoundedFloatToValue(correspondence_cost, kMinCorrespondenceCost,
+                             kMaxCorrespondenceCost);
+}
+
 // Converts a probability to a uint16 in the [1, 32767] range.
 inline uint16 ProbabilityToValue(const float probability) {
-  const int value =
+  return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
-      common::RoundToInt((ClampProbability(probability) - kMinProbability) *
-                         (32766.f / (kMaxProbability - kMinProbability))) +
-      1;
-  // DCHECK for performance.
-  DCHECK_GE(value, 1);
-  DCHECK_LE(value, 32767);
-  return value;
 }
 
 extern const std::vector<float>* const kValueToProbability;
+extern const std::vector<float>* const kValueToCorrespondenceCost;
 
 // Converts a uint16 (which may or may not have the update marker set) to a
 // probability in the range [kMinProbability, kMaxProbability].
@@ -67,7 +101,46 @@ inline float ValueToProbability(const uint16 value) {
   return (*kValueToProbability)[value];
 }
 
+// Converts a uint16 (which may or may not have the update marker set) to a
+// correspondence cost in the range [kMinCorrespondenceCost,
+// kMaxCorrespondenceCost].
+inline float ValueToCorrespondenceCost(const uint16 value) {
+  return (*kValueToCorrespondenceCost)[value];
+}
+
+inline uint16 ProbabilityValueToCorrespondenceCostValue(
+    uint16 probability_value) {
+  if (probability_value == kUnknownProbabilityValue) {
+    return kUnknownCorrespondenceValue;
+  }
+  bool update_carry = false;
+  if (probability_value > kUpdateMarker) {
+    probability_value -= kUpdateMarker;
+    update_carry = true;
+  }
+  uint16 result = CorrespondenceCostToValue(
+      ProbabilityToCorrespondenceCost(ValueToProbability(probability_value)));
+  if (update_carry) result += kUpdateMarker;
+  return result;
+}
+
+inline uint16 CorrespondenceCostValueToProbabilityValue(
+    uint16 correspondence_cost_value) {
+  if (correspondence_cost_value == kUnknownCorrespondenceValue)
+    return kUnknownProbabilityValue;
+  bool update_carry = false;
+  if (correspondence_cost_value > kUpdateMarker) {
+    correspondence_cost_value -= kUpdateMarker;
+    update_carry = true;
+  }
+  uint16 result = ProbabilityToValue(CorrespondenceCostToProbability(
+      ValueToCorrespondenceCost(correspondence_cost_value)));
+  if (update_carry) result += kUpdateMarker;
+  return result;
+}
+
 std::vector<uint16> ComputeLookupTableToApplyOdds(float odds);
+std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(float odds);
 
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/probability_values_test.cc

@@ -30,6 +30,144 @@ TEST(ProbabilityValuesTest, OddsConversions) {
   EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6);
 }
 
+TEST(ProbabilityValuesTest, OddsConversionsCorrespondenceCost) {
+  EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds(
+                  CorrespondenceCostToProbability(kMaxCorrespondenceCost)))),
+              kMaxCorrespondenceCost, 1e-6);
+  EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(Odds(
+                  CorrespondenceCostToProbability(kMinCorrespondenceCost)))),
+              kMinCorrespondenceCost, 1e-6);
+  EXPECT_NEAR(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
+                  Odds(CorrespondenceCostToProbability(0.5)))),
+              0.5, 1e-6);
+}
+
+TEST(ProbabilityValuesTest,
+     ProbabilityValueToCorrespondenceCostValueConversions) {
+  for (uint16 i = 0; i < 32768; ++i) {
+    EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue(
+                  CorrespondenceCostValueToProbabilityValue(i)),
+              i);
+    EXPECT_EQ(CorrespondenceCostValueToProbabilityValue(
+                  ProbabilityValueToCorrespondenceCostValue(i)),
+              i);
+  }
+}
+
+TEST(ProbabilityValuesTest,
+     ProbabilityValueToCorrespondenceCostValueConversionsWithUpdateMarker) {
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_EQ(ProbabilityValueToCorrespondenceCostValue(
+                  CorrespondenceCostValueToProbabilityValue(i + kUpdateMarker)),
+              i + kUpdateMarker);
+    EXPECT_EQ(CorrespondenceCostValueToProbabilityValue(
+                  ProbabilityValueToCorrespondenceCostValue(i + kUpdateMarker)),
+              i + kUpdateMarker);
+  }
+}
+
+TEST(ProbabilityValuesTest, ConversionLookUpTable) {
+  EXPECT_NEAR(ValueToProbability(0), 1.f - ValueToCorrespondenceCost(0), 1e-6);
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_NEAR(ValueToProbability(i), ValueToCorrespondenceCost(i), 1e-6)
+        << " i " << i;
+  }
+}
+
+TEST(ProbabilityValuesTest, CellUpdate) {
+  std::vector<uint16> probability_table =
+      ComputeLookupTableToApplyOdds(Odds(0.9f));
+  std::vector<uint16> correspondence_table =
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9f));
+  uint16 cell_pg_pre_update = 0;
+  uint16 cell_cg_pre_update = 0;
+  uint16 cell_pg_post_update = probability_table[cell_pg_pre_update];
+  uint16 cell_cg_post_update = correspondence_table[cell_cg_pre_update];
+  float p_post = ValueToProbability(cell_pg_post_update);
+  float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
+  EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
+  int num_evaluations = 5000;
+  for (int i_probability = 0; i_probability < num_evaluations;
+       ++i_probability) {
+    float p = (static_cast<float>(i_probability) /
+               static_cast<float>(num_evaluations)) *
+                  (kMaxProbability - kMinProbability) +
+              kMinProbability;
+    cell_pg_pre_update = ProbabilityToValue(p);
+    cell_cg_pre_update =
+        CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p));
+    float p_value =
+        (common::Clamp(p, kMinProbability, kMaxProbability) - kMinProbability) *
+        (32766.f / (kMaxProbability - kMinProbability));
+    float cvalue = (common::Clamp(ProbabilityToCorrespondenceCost(p),
+                                  kMinProbability, kMaxProbability) -
+                    kMinProbability) *
+                   (32766.f / (kMaxProbability - kMinProbability));
+
+    EXPECT_NEAR(cell_pg_pre_update, 32768 - cell_cg_pre_update, 1)
+        << "p " << p << " p_value " << p_value << " cvalue " << cvalue
+        << " p_valuei " << common::RoundToInt(p_value) << " cvaluei "
+        << common::RoundToInt(cvalue);
+    cell_pg_post_update = probability_table[cell_pg_pre_update];
+    cell_cg_post_update = correspondence_table[cell_cg_pre_update];
+    p_post = ValueToProbability(cell_pg_post_update);
+    c_post = ValueToCorrespondenceCost(cell_cg_post_update);
+    EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
+        << "p " << p << " " << p_post - 1.f + c_post;
+  }
+}
+
+TEST(ProbabilityValuesTest, MultipleCellUpdate) {
+  std::vector<uint16> probability_table =
+      ComputeLookupTableToApplyOdds(Odds(0.55f));
+  std::vector<uint16> correspondence_table =
+      ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.55f));
+  uint16 cell_pg_post_update = probability_table[0];
+  uint16 cell_cg_post_update = correspondence_table[0];
+  float p_post = ValueToProbability(cell_pg_post_update);
+  float c_post = ValueToCorrespondenceCost(cell_cg_post_update);
+  EXPECT_NEAR(p_post, 1.f - c_post, 1e-6);
+  int num_evaluations = 5000;
+  for (int i_probability = 0; i_probability < num_evaluations;
+       ++i_probability) {
+    float p = (static_cast<float>(i_probability) /
+               static_cast<float>(num_evaluations)) *
+                  (kMaxProbability - kMinProbability) +
+              kMinProbability;
+    cell_pg_post_update = ProbabilityToValue(p) + kUpdateMarker;
+    cell_cg_post_update =
+        CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(p)) +
+        kUpdateMarker;
+    for (int i_update = 0; i_update < 20; ++i_update) {
+      cell_pg_post_update =
+          probability_table[cell_pg_post_update - kUpdateMarker];
+      cell_cg_post_update =
+          correspondence_table[cell_cg_post_update - kUpdateMarker];
+    }
+    p_post = ValueToProbability(cell_pg_post_update);
+    c_post = ValueToCorrespondenceCost(cell_cg_post_update);
+    EXPECT_NEAR(p_post, 1.f - c_post, 5e-5)
+        << "p " << p << " p_post " << p_post << " " << p_post - 1.f + c_post;
+  }
+}
+
+TEST(ProbabilityValuesTest, EqualityLookupTableToApplyOdds) {
+  std::vector<uint16> probability_table = ComputeLookupTableToApplyOdds(0.3);
+  std::vector<uint16> correspondence_table =
+      ComputeLookupTableToApplyCorrespondenceCostOdds(0.3);
+  for (int i = 0; i < 32768; ++i) {
+    EXPECT_NEAR(
+        probability_table[i],
+        CorrespondenceCostValueToProbabilityValue(
+            correspondence_table[ProbabilityValueToCorrespondenceCostValue(i)]),
+        1);
+    EXPECT_NEAR(
+        ProbabilityValueToCorrespondenceCostValue(
+            probability_table[CorrespondenceCostValueToProbabilityValue(i)]),
+        correspondence_table[i], 1);
+  }
+}
+
 }  // namespace
 }  // namespace mapping
-}  // namespace cartographer
+}  // namespace cartographer