diff --git a/beluga/include/beluga/algorithm.hpp b/beluga/include/beluga/algorithm.hpp index e98559f81..773b4ec29 100644 --- a/beluga/include/beluga/algorithm.hpp +++ b/beluga/include/beluga/algorithm.hpp @@ -26,5 +26,6 @@ #include #include #include +#include #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/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp new file mode 100644 index 000000000..18d5b7265 --- /dev/null +++ b/beluga/include/beluga/algorithm/thrun_recovery_probability_estimator.hpp @@ -0,0 +1,96 @@ +// 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_THRUN_RECOVERY_PROBABILITY_ESTIMATOR_HPP +#define BELUGA_ALGORITHM_THRUN_RECOVERY_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 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. + * + * See Probabilistic Robotics \cite thrun2005probabilistic, Chapter 8.3.3. + */ +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 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); + } + + /// 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(); + } + + /// 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 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) { + reset(); + return 0.0; + } + + const double total_weight = ranges::accumulate(beluga::views::weights(range), 0.0); + 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); + + if (std::abs(slow_average) < std::numeric_limits::epsilon()) { + return 0.0; + } + + return std::clamp(1.0 - fast_average / slow_average, 0.0, 1.0); + } + + private: + ExponentialFilter slow_filter_; ///< Exponential filter for the long-term average. + ExponentialFilter fast_filter_; ///< Exponential filter for the short-term average. +}; + +} // namespace beluga + +#endif diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index b1f5d2d1f..bdec36ca7 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -23,6 +23,7 @@ add_executable( 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_thrun_recovery_probability_estimator.cpp b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp new file mode 100644 index 000000000..62dfe8b08 --- /dev/null +++ b/beluga/test/beluga/algorithm/test_thrun_recovery_probability_estimator.cpp @@ -0,0 +1,83 @@ +// 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(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::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; + ASSERT_EQ(estimator(std::vector>{}), 0.0); +} + +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::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; + ASSERT_EQ(estimator(std::vector>{{1, 0.0}, {2, 0.0}}), 0.0); +} + +TEST(ThrunRecoveryProbabilityEstimator, ProbabilityAfterUpdateAndReset) { + const double alpha_slow = 0.5; + const double alpha_fast = 1.0; + auto estimator = beluga::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; + + // Test the probability after updating the estimator with particle weights. + auto input = std::vector>{{5, 1.0}, {6, 2.0}, {7, 3.0}}; + ASSERT_EQ(estimator(input), 0.0); + + input = std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}; + ASSERT_NEAR(estimator(input), 0.33, 0.01); + + input = std::vector>{{5, 0.5}, {6, 1.0}, {7, 1.5}}; + ASSERT_NEAR(estimator(input), 0.20, 0.01); + + estimator.reset(); + ASSERT_EQ(estimator(input), 0.0); // Test the probability after resetting the estimator. +} + +class ThrunRecoveryProbabilityWithParam : public ::testing::TestWithParam> {}; + +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::ThrunRecoveryProbabilityEstimator{alpha_slow, alpha_fast}; + auto particles = std::vector>{{1, initial_weight}}; + + ASSERT_NEAR(estimator(particles), 0.0, 0.01); + + beluga::weight(particles.front()) = final_weight; + + ASSERT_NEAR(estimator(particles), expected_probability, 0.01); +} + +INSTANTIATE_TEST_SUITE_P( + ThrunRecoveryProbability, + ThrunRecoveryProbabilityWithParam, + 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 12392533c..842599f4c 100644 --- a/beluga_system_tests/test/test_system_new.cpp +++ b/beluga_system_tests/test/test_system_new.cpp @@ -152,6 +152,8 @@ auto particle_filter_test( should_resample = should_resample && beluga::policies::on_effective_size_drop; } + 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) { if (!should_update(odom)) { @@ -181,17 +183,12 @@ 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); - */ + const double random_state_probability = probability_estimator(particles); if (should_resample(particles)) { - // TODO(nahuel): Implement adaptive probability estimator. - /** - * const auto random_state_probability = adaptive_probability_estimator(); - */ - const auto random_state_probability = 0.1; + if (random_state_probability > 0.0) { + probability_estimator.reset(); + } auto make_random_particle = [&sensor] { static thread_local auto engine = std::mt19937{std::random_device()()};