From fec3e3810cc1d645892eaa1a483106e7c25ce27a Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Sun, 7 Jan 2024 12:02:56 -0300 Subject: [PATCH 1/7] Add adaptive probability estimator Signed-off-by: Nahuel Espinosa --- beluga/include/beluga/algorithm.hpp | 1 + .../adaptive_probability_estimator.hpp | 100 ++++++++++++++++++ .../beluga/algorithm/exponential_filter.hpp | 6 +- beluga/test/beluga/CMakeLists.txt | 1 + .../test_adaptive_probability_estimator.cpp | 79 ++++++++++++++ beluga_system_tests/test/test_system_new.cpp | 16 ++- 6 files changed, 191 insertions(+), 12 deletions(-) create mode 100644 beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp create mode 100644 beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp diff --git a/beluga/include/beluga/algorithm.hpp b/beluga/include/beluga/algorithm.hpp index e98559f81..8ef811e3f 100644 --- a/beluga/include/beluga/algorithm.hpp +++ b/beluga/include/beluga/algorithm.hpp @@ -20,6 +20,7 @@ * \brief Includes all beluga algorithms. */ +#include #include #include #include diff --git a/beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp b/beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp new file mode 100644 index 000000000..557278d50 --- /dev/null +++ b/beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp @@ -0,0 +1,100 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ALGORITHM_ADAPTIVE_PROBABILITY_ESTIMATOR_HPP +#define BELUGA_ALGORITHM_ADAPTIVE_PROBABILITY_ESTIMATOR_HPP + +#include +#include +#include + +#include + +namespace beluga { + +/// Random particle probability estimator. +/** + * This class implements an estimator for what probability to use for injecting random + * particles (not sampled directly from the particle set) during the resampling step of a + * particle filter. + * + * The addition of random samples allows the filter to recover in case it converged to + * the wrong estimate, adding an additional level of robustness. + * + * This estimator averages the total weight of the particles and computes the ratio + * between a short-term and a long-term average over time. + * + * See Probabilistic Robotics \cite thrun2005probabilistic, Chapter 8.3.3. + */ +class AdaptiveProbabilityEstimator { + public: + /// Constructor. + /** + * \param alpha_slow Decay rate for the long-term average. + * \param alpha_fast Decay rate for the short-term average. + */ + constexpr AdaptiveProbabilityEstimator(double alpha_slow, double alpha_fast) noexcept + : slow_filter_{alpha_slow}, fast_filter_{alpha_fast} { + assert(0 < alpha_slow); + assert(alpha_slow < alpha_fast); + } + + /// Reset the internal state of the estimator. + /** + * It is recommended to reset the estimator after injecting random particles + * to avoid spiraling off into complete randomness. + */ + constexpr void reset() noexcept { + slow_filter_.reset(); + fast_filter_.reset(); + probability_ = 0.0; + } + + /// Update the estimation based on a particle range. + template + constexpr void update(Range&& range) { + static_assert(ranges::sized_range); + static_assert(beluga::is_particle_range_v); + const auto size = static_cast(range.size()); + + if (size == 0.0) { + reset(); + return; + } + + const double total_weight = ranges::accumulate(beluga::views::weights(range), 0.0); + const double average_weight = total_weight / size; + const double fast_average = fast_filter_(average_weight); + const double slow_average = slow_filter_(average_weight); + + if (slow_average == 0.0) { + probability_ = 0.0; + return; + } + + probability_ = std::clamp(1.0 - fast_average / slow_average, 0.0, 1.0); + } + + /// Returns the estimated random state probability to be used by the filter. + constexpr double operator()() const noexcept { return probability_; } + + private: + ExponentialFilter slow_filter_; + ExponentialFilter fast_filter_; + double probability_ = 0.0; +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/algorithm/exponential_filter.hpp b/beluga/include/beluga/algorithm/exponential_filter.hpp index 1f6de9956..6716382fe 100644 --- a/beluga/include/beluga/algorithm/exponential_filter.hpp +++ b/beluga/include/beluga/algorithm/exponential_filter.hpp @@ -29,16 +29,16 @@ class ExponentialFilter { /** * \param alpha The exponential filter smoothing factor. */ - explicit ExponentialFilter(double alpha) : alpha_{alpha} {} + constexpr explicit ExponentialFilter(double alpha) noexcept : alpha_{alpha} {} /// Resets the output of the exponential filter to zero. - void reset() { output_ = 0.; } + constexpr void reset() noexcept { output_ = 0.; } /// Updates the exponential filter output given an input. /** * \param input Next value to be exponentially filtered. */ - [[nodiscard]] double operator()(double input) { + [[nodiscard]] constexpr double operator()(double input) noexcept { output_ += (output_ == 0.) ? input : alpha_ * (input - output_); return output_; } diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 2a2dae32e..5cc2ea822 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -18,6 +18,7 @@ add_executable( actions/test_propagate.cpp actions/test_reweight.cpp algorithm/raycasting/test_bresenham.cpp + algorithm/test_adaptive_probability_estimator.cpp algorithm/test_distance_map.cpp algorithm/test_effective_sample_size.cpp algorithm/test_estimation.cpp diff --git a/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp b/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp new file mode 100644 index 000000000..6131d413a --- /dev/null +++ b/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp @@ -0,0 +1,79 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace { + +TEST(AdaptiveProbabilityEstimator, ProbabilityWithNoParticles) { + // Test the probability when the particle set is empty. + const double alpha_slow = 0.2; + const double alpha_fast = 0.4; + auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + estimator.update(std::vector>{}); + ASSERT_EQ(estimator(), 0.0); +} + +TEST(AdaptiveProbabilityEstimator, ProbabilityAfterUpdateAndReset) { + const double alpha_slow = 0.5; + const double alpha_fast = 1.0; + auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + + // Test the probability after updating the estimator with particle weights. + estimator.update(std::vector>{{5, 1.0}, {6, 2.0}, {7, 3.0}}); + ASSERT_EQ(estimator(), 0.0); + + estimator.update(std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}); + ASSERT_NEAR(estimator(), 0.33, 0.01); + + estimator.update(std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}); + ASSERT_NEAR(estimator(), 0.20, 0.01); + + // Test the probability after resetting the estimator. + estimator.reset(); + ASSERT_EQ(estimator(), 0.0); +} + +class AdaptiveProbabilityWithParam : public ::testing::TestWithParam> {}; + +TEST_P(AdaptiveProbabilityWithParam, Probabilities) { + const auto [initial_weight, final_weight, expected_probability] = GetParam(); + + const double alpha_slow = 0.001; + const double alpha_fast = 0.1; + auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + auto particles = std::vector>{{1, initial_weight}}; + + estimator.update(particles); + ASSERT_NEAR(estimator(), 0.0, 0.01); + + beluga::weight(particles.front()) = final_weight; + + estimator.update(particles); + ASSERT_NEAR(estimator(), expected_probability, 0.01); +} + +INSTANTIATE_TEST_SUITE_P( + AdaptiveProbability, + AdaptiveProbabilityWithParam, + testing::Values( + std::make_tuple(1.0, 1.5, 0.00), + std::make_tuple(1.0, 2.0, 0.00), + std::make_tuple(1.0, 0.5, 0.05), + std::make_tuple(0.5, 0.1, 0.08), + std::make_tuple(0.5, 0.0, 0.10))); + +} // namespace diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index d0654a952..c46c9966e 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -172,6 +172,8 @@ auto particle_filter_test( return ++count % params.resample_interval_count == 0; }; + auto adaptive_probability_estimator = beluga::AdaptiveProbabilityEstimator(params.alpha_slow, params.alpha_fast); + // Iteratively run the filter through all the data points. for (auto [measurement, odom, ground_truth] : datapoints) { if (!far_enough_to_update(odom)) { @@ -201,10 +203,7 @@ auto particle_filter_test( const double total_weight = ranges::accumulate(beluga::views::weights(particles), 0.0); particles |= beluga::actions::reweight([total_weight](auto) { return 1.0 / total_weight; }); // HACK - // TODO(nahuel): Implement adaptive probability estimator. - /** - * adaptive_probability_estimator.update(particles); - */ + adaptive_probability_estimator.update(particles); // TODO(nahuel): Sort out resampling policies. if (!should_resample()) { @@ -214,11 +213,10 @@ auto particle_filter_test( // Nav2 updates the filter estimates regardless of whether selective resampling actually resamples or not. if (!params.selective_resampling || beluga::effective_sample_size(particles) < static_cast(ranges::size(particles)) / 2.0) { - // TODO(nahuel): Implement adaptive probability estimator. - /** - * const auto random_state_probability = adaptive_probability_estimator(); - */ - const auto random_state_probability = 0.1; + const auto random_state_probability = adaptive_probability_estimator(); + if (random_state_probability > 0.0) { + adaptive_probability_estimator.reset(); + } auto make_random_particle = [&sensor] { static thread_local auto engine = std::mt19937{std::random_device()()}; From 57ebfb7e6f5cd2b773c47b414ac92a2ac160fc6f Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 22 Jan 2024 09:03:58 -0300 Subject: [PATCH 2/7] Add zero weight test Signed-off-by: Nahuel Espinosa --- .../algorithm/test_adaptive_probability_estimator.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp b/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp index 6131d413a..bc6d34efd 100644 --- a/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp +++ b/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp @@ -27,6 +27,15 @@ TEST(AdaptiveProbabilityEstimator, ProbabilityWithNoParticles) { ASSERT_EQ(estimator(), 0.0); } +TEST(AdaptiveProbabilityEstimator, ProbabilityWithZeroWeight) { + // Test the probability when the total weight is zero. + const double alpha_slow = 0.2; + const double alpha_fast = 0.4; + auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + estimator.update(std::vector>{{1, 0.0}, {2, 0.0}}); + ASSERT_EQ(estimator(), 0.0); +} + TEST(AdaptiveProbabilityEstimator, ProbabilityAfterUpdateAndReset) { const double alpha_slow = 0.5; const double alpha_fast = 1.0; From c14e81dc0e4307d021219a7533c2260725245789 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Wed, 24 Jan 2024 21:09:32 -0300 Subject: [PATCH 3/7] Rename probability estimator Signed-off-by: Nahuel Espinosa --- beluga/include/beluga/algorithm.hpp | 2 +- ... thrun_recovery_probability_estimator.hpp} | 8 +++---- beluga/test/beluga/CMakeLists.txt | 2 +- ..._thrun_recovery_probability_estimator.cpp} | 24 +++++++++---------- beluga_system_tests/test/test_system_new.cpp | 8 +++---- 5 files changed, 22 insertions(+), 22 deletions(-) rename beluga/include/beluga/algorithm/{adaptive_probability_estimator.hpp => thrun_recovery_probability_estimator.hpp} (92%) rename beluga/test/beluga/algorithm/{test_adaptive_probability_estimator.cpp => test_thrun_recovery_probability_estimator.cpp} (74%) diff --git a/beluga/include/beluga/algorithm.hpp b/beluga/include/beluga/algorithm.hpp index 8ef811e3f..773b4ec29 100644 --- a/beluga/include/beluga/algorithm.hpp +++ b/beluga/include/beluga/algorithm.hpp @@ -20,12 +20,12 @@ * \brief Includes all beluga algorithms. */ -#include #include #include #include #include #include #include +#include #endif diff --git a/beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp similarity index 92% rename from beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp rename to beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp index 557278d50..ebd890032 100644 --- a/beluga/include/beluga/algorithm/adaptive_probability_estimator.hpp +++ b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BELUGA_ALGORITHM_ADAPTIVE_PROBABILITY_ESTIMATOR_HPP -#define BELUGA_ALGORITHM_ADAPTIVE_PROBABILITY_ESTIMATOR_HPP +#ifndef BELUGA_ALGORITHM_THRUN_RECOVERY_PROBABILITY_ESTIMATOR_HPP +#define BELUGA_ALGORITHM_THRUN_RECOVERY_PROBABILITY_ESTIMATOR_HPP #include #include @@ -37,14 +37,14 @@ namespace beluga { * * See Probabilistic Robotics \cite thrun2005probabilistic, Chapter 8.3.3. */ -class AdaptiveProbabilityEstimator { +class ThrunRecoveryProbabilityEstimator { public: /// Constructor. /** * \param alpha_slow Decay rate for the long-term average. * \param alpha_fast Decay rate for the short-term average. */ - constexpr AdaptiveProbabilityEstimator(double alpha_slow, double alpha_fast) noexcept + constexpr ThrunRecoveryProbabilityEstimator(double alpha_slow, double alpha_fast) noexcept : slow_filter_{alpha_slow}, fast_filter_{alpha_fast} { assert(0 < alpha_slow); assert(alpha_slow < alpha_fast); diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 5cc2ea822..ef856e4b2 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -18,12 +18,12 @@ add_executable( actions/test_propagate.cpp actions/test_reweight.cpp algorithm/raycasting/test_bresenham.cpp - algorithm/test_adaptive_probability_estimator.cpp algorithm/test_distance_map.cpp algorithm/test_effective_sample_size.cpp algorithm/test_estimation.cpp algorithm/test_exponential_filter.cpp algorithm/test_raycasting.cpp + algorithm/test_thrun_recovery_probability_estimator.cpp mixin/test_particle_filter.cpp mixin/test_sampling.cpp mixin/test_storage.cpp diff --git a/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp similarity index 74% rename from beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp rename to beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp index bc6d34efd..1f5f24840 100644 --- a/beluga/test/beluga/algorithm/test_adaptive_probability_estimator.cpp +++ b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp @@ -14,32 +14,32 @@ #include -#include +#include namespace { -TEST(AdaptiveProbabilityEstimator, ProbabilityWithNoParticles) { +TEST(ThrunRecoveryProbabilityEstimator, ProbabilityWithNoParticles) { // Test the probability when the particle set is empty. const double alpha_slow = 0.2; const double alpha_fast = 0.4; - auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; estimator.update(std::vector>{}); ASSERT_EQ(estimator(), 0.0); } -TEST(AdaptiveProbabilityEstimator, ProbabilityWithZeroWeight) { +TEST(ThrunRecoveryProbabilityEstimator, ProbabilityWithZeroWeight) { // Test the probability when the total weight is zero. const double alpha_slow = 0.2; const double alpha_fast = 0.4; - auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; estimator.update(std::vector>{{1, 0.0}, {2, 0.0}}); ASSERT_EQ(estimator(), 0.0); } -TEST(AdaptiveProbabilityEstimator, ProbabilityAfterUpdateAndReset) { +TEST(ThrunRecoveryProbabilityEstimator, ProbabilityAfterUpdateAndReset) { const double alpha_slow = 0.5; const double alpha_fast = 1.0; - auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; // Test the probability after updating the estimator with particle weights. estimator.update(std::vector>{{5, 1.0}, {6, 2.0}, {7, 3.0}}); @@ -56,14 +56,14 @@ TEST(AdaptiveProbabilityEstimator, ProbabilityAfterUpdateAndReset) { ASSERT_EQ(estimator(), 0.0); } -class AdaptiveProbabilityWithParam : public ::testing::TestWithParam> {}; +class ThrunRecoveryProbabilityWithParam : public ::testing::TestWithParam> {}; -TEST_P(AdaptiveProbabilityWithParam, Probabilities) { +TEST_P(ThrunRecoveryProbabilityWithParam, Probabilities) { const auto [initial_weight, final_weight, expected_probability] = GetParam(); const double alpha_slow = 0.001; const double alpha_fast = 0.1; - auto estimator = beluga::AdaptiveProbabilityEstimator{alpha_slow, alpha_fast}; + auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; auto particles = std::vector>{{1, initial_weight}}; estimator.update(particles); @@ -76,8 +76,8 @@ TEST_P(AdaptiveProbabilityWithParam, Probabilities) { } INSTANTIATE_TEST_SUITE_P( - AdaptiveProbability, - AdaptiveProbabilityWithParam, + ThrunRecoveryProbability, + ThrunRecoveryProbabilityWithParam, testing::Values( std::make_tuple(1.0, 1.5, 0.00), std::make_tuple(1.0, 2.0, 0.00), diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index c46c9966e..b7d61f51c 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -172,7 +172,7 @@ auto particle_filter_test( return ++count % params.resample_interval_count == 0; }; - auto adaptive_probability_estimator = beluga::AdaptiveProbabilityEstimator(params.alpha_slow, params.alpha_fast); + auto probability_estimator = beluga::ThrunRecoveryProbabilityEstimator(params.alpha_slow, params.alpha_fast); // Iteratively run the filter through all the data points. for (auto [measurement, odom, ground_truth] : datapoints) { @@ -203,7 +203,7 @@ auto particle_filter_test( const double total_weight = ranges::accumulate(beluga::views::weights(particles), 0.0); particles |= beluga::actions::reweight([total_weight](auto) { return 1.0 / total_weight; }); // HACK - adaptive_probability_estimator.update(particles); + probability_estimator.update(particles); // TODO(nahuel): Sort out resampling policies. if (!should_resample()) { @@ -213,9 +213,9 @@ auto particle_filter_test( // Nav2 updates the filter estimates regardless of whether selective resampling actually resamples or not. if (!params.selective_resampling || beluga::effective_sample_size(particles) < static_cast(ranges::size(particles)) / 2.0) { - const auto random_state_probability = adaptive_probability_estimator(); + const auto random_state_probability = probability_estimator(); if (random_state_probability > 0.0) { - adaptive_probability_estimator.reset(); + probability_estimator.reset(); } auto make_random_particle = [&sensor] { From 51b6736ce2e6076f5bac2331c82af3390f404dfe Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Wed, 24 Jan 2024 21:10:33 -0300 Subject: [PATCH 4/7] Check for zero in its integer form Signed-off-by: Nahuel Espinosa --- .../beluga/algorithm/thrun_recovery_probability_estimator.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp index ebd890032..825e6e390 100644 --- a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp +++ b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp @@ -66,7 +66,7 @@ class ThrunRecoveryProbabilityEstimator { constexpr void update(Range&& range) { static_assert(ranges::sized_range); static_assert(beluga::is_particle_range_v); - const auto size = static_cast(range.size()); + const std::size_t size = range.size(); if (size == 0.0) { reset(); @@ -74,7 +74,7 @@ class ThrunRecoveryProbabilityEstimator { } const double total_weight = ranges::accumulate(beluga::views::weights(range), 0.0); - const double average_weight = total_weight / size; + const double average_weight = total_weight / static_cast(size); const double fast_average = fast_filter_(average_weight); const double slow_average = slow_filter_(average_weight); From ef24694aa0fc17a13b8766f0cf5d651d59caeef0 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Wed, 24 Jan 2024 21:12:43 -0300 Subject: [PATCH 5/7] Make the API consistent Signed-off-by: Nahuel Espinosa --- .../thrun_recovery_probability_estimator.hpp | 30 ++++++++----------- ...t_thrun_recovery_probability_estimator.cpp | 27 +++++++---------- beluga_system_tests/test/test_system_new.cpp | 3 +- 3 files changed, 25 insertions(+), 35 deletions(-) diff --git a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp index 825e6e390..0e5b308f8 100644 --- a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp +++ b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp @@ -27,10 +27,8 @@ namespace beluga { /** * This class implements an estimator for what probability to use for injecting random * particles (not sampled directly from the particle set) during the resampling step of a - * particle filter. - * - * The addition of random samples allows the filter to recover in case it converged to - * the wrong estimate, adding an additional level of robustness. + * particle filter. The inclusion of random samples enhances the filter's ability to recover + * in case it converges to an incorrect estimate, thereby adding an extra layer of robustness. * * This estimator averages the total weight of the particles and computes the ratio * between a short-term and a long-term average over time. @@ -58,19 +56,22 @@ class ThrunRecoveryProbabilityEstimator { constexpr void reset() noexcept { slow_filter_.reset(); fast_filter_.reset(); - probability_ = 0.0; } /// Update the estimation based on a particle range. + /** + * \param range A range containing particles. + * \return The estimated random state probability to be used by the particle filter. + */ template - constexpr void update(Range&& range) { + constexpr double operator()(Range&& range) { static_assert(ranges::sized_range); static_assert(beluga::is_particle_range_v); const std::size_t size = range.size(); - if (size == 0.0) { + if (size == 0) { reset(); - return; + return 0.0; } const double total_weight = ranges::accumulate(beluga::views::weights(range), 0.0); @@ -79,20 +80,15 @@ class ThrunRecoveryProbabilityEstimator { const double slow_average = slow_filter_(average_weight); if (slow_average == 0.0) { - probability_ = 0.0; - return; + return 0.0; } - probability_ = std::clamp(1.0 - fast_average / slow_average, 0.0, 1.0); + return std::clamp(1.0 - fast_average / slow_average, 0.0, 1.0); } - /// Returns the estimated random state probability to be used by the filter. - constexpr double operator()() const noexcept { return probability_; } - private: - ExponentialFilter slow_filter_; - ExponentialFilter fast_filter_; - double probability_ = 0.0; + ExponentialFilter slow_filter_; ///< Exponential filter for the long-term average. + ExponentialFilter fast_filter_; ///< Exponential filter for the short-term average. }; } // namespace beluga diff --git a/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp index 1f5f24840..62dfe8b08 100644 --- a/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp +++ b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp @@ -23,8 +23,7 @@ TEST(ThrunRecoveryProbabilityEstimator, ProbabilityWithNoParticles) { const double alpha_slow = 0.2; const double alpha_fast = 0.4; auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; - estimator.update(std::vector>{}); - ASSERT_EQ(estimator(), 0.0); + ASSERT_EQ(estimator(std::vector>{}), 0.0); } TEST(ThrunRecoveryProbabilityEstimator, ProbabilityWithZeroWeight) { @@ -32,8 +31,7 @@ TEST(ThrunRecoveryProbabilityEstimator, ProbabilityWithZeroWeight) { const double alpha_slow = 0.2; const double alpha_fast = 0.4; auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; - estimator.update(std::vector>{{1, 0.0}, {2, 0.0}}); - ASSERT_EQ(estimator(), 0.0); + ASSERT_EQ(estimator(std::vector>{{1, 0.0}, {2, 0.0}}), 0.0); } TEST(ThrunRecoveryProbabilityEstimator, ProbabilityAfterUpdateAndReset) { @@ -42,18 +40,17 @@ TEST(ThrunRecoveryProbabilityEstimator, ProbabilityAfterUpdateAndReset) { auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; // Test the probability after updating the estimator with particle weights. - estimator.update(std::vector>{{5, 1.0}, {6, 2.0}, {7, 3.0}}); - ASSERT_EQ(estimator(), 0.0); + auto input = std::vector>{{5, 1.0}, {6, 2.0}, {7, 3.0}}; + ASSERT_EQ(estimator(input), 0.0); - estimator.update(std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}); - ASSERT_NEAR(estimator(), 0.33, 0.01); + input = std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}; + ASSERT_NEAR(estimator(input), 0.33, 0.01); - estimator.update(std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}); - ASSERT_NEAR(estimator(), 0.20, 0.01); + input = std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}; + ASSERT_NEAR(estimator(input), 0.20, 0.01); - // Test the probability after resetting the estimator. estimator.reset(); - ASSERT_EQ(estimator(), 0.0); + ASSERT_EQ(estimator(input), 0.0); // Test the probability after resetting the estimator. } class ThrunRecoveryProbabilityWithParam : public ::testing::TestWithParam> {}; @@ -66,13 +63,11 @@ TEST_P(ThrunRecoveryProbabilityWithParam, Probabilities) { auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; auto particles = std::vector>{{1, initial_weight}}; - estimator.update(particles); - ASSERT_NEAR(estimator(), 0.0, 0.01); + ASSERT_NEAR(estimator(particles), 0.0, 0.01); beluga::weight(particles.front()) = final_weight; - estimator.update(particles); - ASSERT_NEAR(estimator(), expected_probability, 0.01); + ASSERT_NEAR(estimator(particles), expected_probability, 0.01); } INSTANTIATE_TEST_SUITE_P( diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index b7d61f51c..ebb6ac516 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -203,7 +203,7 @@ auto particle_filter_test( const double total_weight = ranges::accumulate(beluga::views::weights(particles), 0.0); particles |= beluga::actions::reweight([total_weight](auto) { return 1.0 / total_weight; }); // HACK - probability_estimator.update(particles); + const double random_state_probability = probability_estimator(particles); // TODO(nahuel): Sort out resampling policies. if (!should_resample()) { @@ -213,7 +213,6 @@ auto particle_filter_test( // Nav2 updates the filter estimates regardless of whether selective resampling actually resamples or not. if (!params.selective_resampling || beluga::effective_sample_size(particles) < static_cast(ranges::size(particles)) / 2.0) { - const auto random_state_probability = probability_estimator(); if (random_state_probability > 0.0) { probability_estimator.reset(); } From e74fabad85fb706b74e4989fa31f71ded34bc748 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Thu, 25 Jan 2024 09:28:50 -0300 Subject: [PATCH 6/7] Apply code review suggestion Co-authored-by: Michel Hidalgo Signed-off-by: Nahuel Espinosa --- .../beluga/algorithm/thrun_recovery_probability_estimator.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp index 0e5b308f8..18d5b7265 100644 --- a/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp +++ b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp @@ -79,7 +79,7 @@ class ThrunRecoveryProbabilityEstimator { const double fast_average = fast_filter_(average_weight); const double slow_average = slow_filter_(average_weight); - if (slow_average == 0.0) { + if (std::abs(slow_average) < std::numeric_limits::epsilon()) { return 0.0; } From 25c5037949067f4b5f6bc5b2f5bf890718b0c32d Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Thu, 25 Jan 2024 09:49:57 -0300 Subject: [PATCH 7/7] Lint Signed-off-by: Nahuel Espinosa --- beluga_system_tests/test/test_system_new.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/beluga_system_tests/test/test_system_new.cpp b/beluga_system_tests/test/test_system_new.cpp index e8839d03c..842599f4c 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -186,7 +186,6 @@ auto particle_filter_test( const double random_state_probability = probability_estimator(particles); if (should_resample(particles)) { - if (random_state_probability > 0.0) { probability_estimator.reset(); }