diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 45393c0..2a7df93 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/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. diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index ecd8404..cc1ef1c 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/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( - correspondence_cost_cells()[ToFlatIndex(cell_index)]); + return CorrespondenceCostToProbability(ValueToCorrespondenceCost( + correspondence_cost_cells()[ToFlatIndex(cell_index)])); } proto::Grid2D ProbabilityGrid::ToProto() const { proto::Grid2D result; diff --git a/cartographer/mapping/2d/probability_grid_test.cc b/cartographer/mapping/2d/probability_grid_test.cc index b93b7dc..c42ba3c 100644 --- a/cartographer/mapping/2d/probability_grid_test.cc +++ b/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), - ComputeLookupTableToApplyOdds(Odds(0.9))); + probability_grid.ApplyLookupTable( + 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), - ComputeLookupTableToApplyOdds(Odds(0.1))); + probability_grid.ApplyLookupTable( + 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), - ComputeLookupTableToApplyOdds(Odds(0.42))); + probability_grid.ApplyLookupTable( + 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), - ComputeLookupTableToApplyOdds(Odds(0.9))); + probability_grid.ApplyLookupTable( + 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), - ComputeLookupTableToApplyOdds(Odds(0.9))); + probability_grid.ApplyLookupTable( + Array2i(1, 1), + ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9))); EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42); } diff --git a/cartographer/mapping/2d/range_data_inserter_2d.cc b/cartographer/mapping/2d/range_data_inserter_2d.cc index 6e2dc0d..aa85884 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d.cc +++ b/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_( - ComputeLookupTableToApplyOdds(Odds(options.hit_probability()))), - miss_table_( - ComputeLookupTableToApplyOdds(Odds(options.miss_probability()))) {} + hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( + Odds(options.hit_probability()))), + miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( + Odds(options.miss_probability()))) {} void RangeDataInserter2D::Insert( const sensor::RangeData& range_data, diff --git a/cartographer/mapping/probability_values.cc b/cartographer/mapping/probability_values.cc index 32e45a4..cae2357 100644 --- a/cartographer/mapping/probability_values.cc +++ b/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]. -float SlowValueToProbability(const uint16 value) { +// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound]. +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) { - // Unknown cells have kMinProbability. - return kMinProbability; - } - const float kScale = (kMaxProbability - kMinProbability) / 32766.f; - return value * kScale + (kMinProbability - kScale); + if (value == unknown_value) return unknown_result; + const float kScale = (upper_bound - lower_bound) / 32766.f; + return value * kScale + (lower_bound - kScale); } -const std::vector* PrecomputeValueToProbability() { - std::vector* result = new std::vector; +std::unique_ptr> PrecomputeValueToBoundedFloat( + const uint16 unknown_value, const float unknown_result, + const float lower_bound, const float upper_bound) { + auto result = common::make_unique>(); // 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> PrecomputeValueToProbability() { + return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue, + kMinProbability, kMinProbability, + kMaxProbability); +} + +std::unique_ptr> PrecomputeValueToCorrespondenceCost() { + return PrecomputeValueToBoundedFloat( + kUnknownCorrespondenceValue, kMaxCorrespondenceCost, + kMinCorrespondenceCost, kMaxCorrespondenceCost); +} + } // namespace const std::vector* const kValueToProbability = - PrecomputeValueToProbability(); + PrecomputeValueToProbability().release(); + +const std::vector* const kValueToCorrespondenceCost = + PrecomputeValueToCorrespondenceCost().release(); std::vector ComputeLookupTableToApplyOdds(const float odds) { std::vector result; @@ -62,5 +82,22 @@ std::vector ComputeLookupTableToApplyOdds(const float odds) { return result; } +std::vector ComputeLookupTableToApplyCorrespondenceCostOdds( + float odds) { + std::vector 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 diff --git a/cartographer/mapping/probability_values.h b/cartographer/mapping/probability_values.h index d9a6e53..2ab57aa 100644 --- a/cartographer/mapping/probability_values.h +++ b/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 = - common::RoundToInt((ClampProbability(probability) - kMinProbability) * - (32766.f / (kMaxProbability - kMinProbability))) + - 1; - // DCHECK for performance. - DCHECK_GE(value, 1); - DCHECK_LE(value, 32767); - return value; + return BoundedFloatToValue(probability, kMinProbability, kMaxProbability); } extern const std::vector* const kValueToProbability; +extern const std::vector* 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 ComputeLookupTableToApplyOdds(float odds); +std::vector ComputeLookupTableToApplyCorrespondenceCostOdds(float odds); } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/probability_values_test.cc b/cartographer/mapping/probability_values_test.cc index 39c7286..328456a 100644 --- a/cartographer/mapping/probability_values_test.cc +++ b/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 probability_table = + ComputeLookupTableToApplyOdds(Odds(0.9f)); + std::vector 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(i_probability) / + static_cast(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 probability_table = + ComputeLookupTableToApplyOdds(Odds(0.55f)); + std::vector 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(i_probability) / + static_cast(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 probability_table = ComputeLookupTableToApplyOdds(0.3); + std::vector 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 \ No newline at end of file