Add recovery probability estimator (#295)

Related to #279.

Signed-off-by: Nahuel Espinosa <nespinosa@ekumenlabs.com>
Co-authored-by: Michel Hidalgo <michel@ekumenlabs.com>

beluga/include/beluga/algorithm.hpp

@@ -26,5 +26,6 @@
 #include <beluga/algorithm/exponential_filter.hpp>
 #include <beluga/algorithm/raycasting.hpp>
 #include <beluga/algorithm/spatial_hash.hpp>
+#include <beluga/algorithm/thrun_recovery_probability_estimator.hpp>
 
 #endif

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_;
   }

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 <beluga/algorithm/exponential_filter.hpp>
+#include <beluga/type_traits/particle_traits.hpp>
+#include <beluga/views/particles.hpp>
+
+#include <range/v3/numeric/accumulate.hpp>
+
+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 <class Range>
+  constexpr double operator()(Range&& range) {
+    static_assert(ranges::sized_range<Range>);
+    static_assert(beluga::is_particle_range_v<Range>);
+    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<double>(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<double>::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

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

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 <gmock/gmock.h>
+
+#include <beluga/algorithm/thrun_recovery_probability_estimator.hpp>
+
+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<std::tuple<int, beluga::Weight>>{}), 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<std::tuple<int, beluga::Weight>>{{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<std::tuple<int, beluga::Weight>>{{5, 1.0}, {6, 2.0}, {7, 3.0}};
+  ASSERT_EQ(estimator(input), 0.0);
+
+  input = std::vector<std::tuple<int, beluga::Weight>>{{5, 0.5}, {6, 1.0}, {7, 1.5}};
+  ASSERT_NEAR(estimator(input), 0.33, 0.01);
+
+  input = std::vector<std::tuple<int, beluga::Weight>>{{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<std::tuple<double, double, double>> {};
+
+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<std::tuple<int, beluga::Weight>>{{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

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()()};