Integrate sensor probabilities accross resampling iterations

Signed-off-by: Gerardo Puga <[email protected]>

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -19,6 +19,8 @@
 #include <random>
 #include <utility>
 
+#include <beluga/type_traits/particle_traits.hpp>
+
 #include <range/v3/view/common.hpp>
 
 /**
@@ -170,6 +172,7 @@ class BootstrapParticleFilter : public Mixin {
   void resample() final {
     const auto resampling_vote_result = this->self().do_resampling_vote();
     if (resampling_vote_result) {
+      // create new particles from states and set their weight to 1.
       this->self().initialize_particles(
           this->self().generate_samples_from_particles(generator_) | this->self().take_samples());
     }
@@ -181,17 +184,21 @@ class BootstrapParticleFilter : public Mixin {
   template <typename ExecutionPolicy>
   void sample_impl(ExecutionPolicy&& policy) {
     auto states = this->self().states() | ranges::views::common;
+    // update particle states, conserve their weight unchanged
     std::transform(
         std::forward<ExecutionPolicy>(policy), std::begin(states), std::end(states), std::begin(states),
         [this](const auto& state) { return this->self().apply_motion(state, generator_); });
   }
 
   template <typename ExecutionPolicy>
   void importance_sample_impl(ExecutionPolicy&& policy) {
-    auto states = this->self().states() | ranges::views::common;
+    auto particles = this->self().particles() | ranges::views::common;
+    auto particle_weight_updater = [this](const auto& particle) {
+      return weight(particle) * this->self().importance_weight(state(particle));
+    };
     std::transform(
-        std::forward<ExecutionPolicy>(policy), std::begin(states), std::end(states), std::begin(this->self().weights()),
-        [this](const auto& state) { return this->self().importance_weight(state); });
+        std::forward<ExecutionPolicy>(policy), std::begin(particles), std::end(particles),
+        std::begin(this->self().weights()), particle_weight_updater);
   }
 };
 

beluga/include/beluga/algorithm/sampling.hpp

@@ -127,15 +127,16 @@ namespace beluga {
  *  The return type is `decltype(first())`.
  */
 template <class Function1, class Function2, class RandomNumberGenerator>
-auto random_select(Function1 first, Function2 second, RandomNumberGenerator& generator, double probability) {
-  return [first = std::move(first), second = std::move(second), &generator,
+auto random_select(Function1&& first, Function2&& second, RandomNumberGenerator& generator, double probability) {
+  return [first = std::forward<Function1>(first), second = std::forward<Function2>(second), &generator,
           distribution = std::bernoulli_distribution{probability}]() mutable {
     return distribution(generator) ? first() : second();
   };
 }
 
-/// Picks a sample randomly from a range according to the weights of the samples.
-/**
+/// Returns multinomial resampler function.
+/** Returns a function that when called picks randomly a sample from the input range, with individual
+ *  probabilities given by the weights of each.
  * \tparam Range A [Range](https://en.cppreference.com/w/cpp/ranges/range) type, its iterator
  *  must be [random access](https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator).
  * \tparam Weights A [Range](https://en.cppreference.com/w/cpp/ranges/range) type,
@@ -147,11 +148,11 @@ auto random_select(Function1 first, Function2 second, RandomNumberGenerator& gen
  *  The size of the container must be the same as the size of samples.
  *  For a sample `samples[i]`, its weight is `weights[i]`.
  * \param generator The random number generator used.
- * \return The picked sample.
+ * \return The sampler function.
  *  Its type is the same as the `Range` value type.
  */
 template <class Range, class Weights, class RandomNumberGenerator>
-auto random_sample(const Range& samples, const Weights& weights, RandomNumberGenerator& generator) {
+auto build_multinomial_sampler(const Range& samples, const Weights& weights, RandomNumberGenerator& generator) {
   auto weights_begin = std::begin(weights);
   auto weights_end = std::end(weights);
   using difference_type = decltype(weights_end - weights_begin);
@@ -171,7 +172,7 @@ auto random_sample(const Range& samples, const Weights& weights, RandomNumberGen
  *  After the returned object is applied to a particle `p`, \c cluster(p) will be updated
  *  with the calculated spatial hash according to the specified resolution.
  */
-inline auto set_cluster(double resolution) {
+inline auto build_clusterization_function(double resolution) {
   return [resolution](auto&& particle) {
     using state_type = std::decay_t<decltype(state(particle))>;
     auto new_particle = particle;
@@ -249,7 +250,7 @@ class RandomStateGenerator : public Mixin {
   template <class... Args>
   explicit RandomStateGenerator(Args&&... args) : Mixin(std::forward<Args>(args)...) {}
 
-  /// Generates new random states.
+  /// Generates new random state samples.
   /**
    * The states are generated according to the `make_random_state()` method
    * provided by the mixin.
@@ -258,7 +259,7 @@ class RandomStateGenerator : public Mixin {
    *  [UniformRandomBitGenerator](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator)
    *  requirements.
    * \param gen An uniform random bit generator object.
-   * \return A range view that can generate random states.
+   * \return A range view that sources a sequence of random states.
    */
   template <class Generator>
   [[nodiscard]] auto generate_samples(Generator& gen) {
@@ -288,7 +289,7 @@ class NaiveSampler : public Mixin {
   template <class... Args>
   explicit NaiveSampler(Args&&... args) : Mixin(std::forward<Args>(args)...) {}
 
-  /// Generates new samples from the current particles.
+  /// Returns a view that sources a sequence of with-replacement samples from the existing set.
   /**
    * The new states are generated according to the `states()` and `weights()` methods
    * provided by the mixin.
@@ -297,11 +298,12 @@ class NaiveSampler : public Mixin {
    *  [UniformRandomBitGenerator](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator)
    *  requirements.
    * \param gen An uniform random bit generator object.
-   * \return A range view that can generate samples from the current set of particles.
+   * \return A range view that consists of with-replacement samples from the existing set.
    */
   template <class Generator>
   [[nodiscard]] auto generate_samples_from_particles(Generator& gen) const {
-    return ranges::views::generate(beluga::random_sample(this->self().states(), this->self().weights(), gen));
+    return ranges::views::generate(
+        beluga::build_multinomial_sampler(this->self().states(), this->self().weights(), gen));
   }
 };
 
@@ -359,7 +361,8 @@ class AdaptiveSampler : public Mixin {
    *  [UniformRandomBitGenerator](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator)
    *  requirements.
    * \param gen An uniform random bit generator object.
-   * \return A range view that can generate samples from the current set of particles.
+   * \return A range view that consists of a mixture of with-replacement samples from the existing set and brand new
+   * random samples.
    */
   template <class Generator>
   [[nodiscard]] auto generate_samples_from_particles(Generator& gen) {
@@ -373,9 +376,10 @@ class AdaptiveSampler : public Mixin {
       slow_filter_.reset();
     }
 
-    return ranges::views::generate(random_select(
-        [this, &gen]() { return this->self().make_random_state(gen); },
-        random_sample(this->self().states(), weights, gen), gen, random_state_probability));
+    auto create_random_state = [this, &gen] { return this->self().make_random_state(gen); };
+    auto sample_existing_state = build_multinomial_sampler(this->self().states(), weights, gen);
+    return ranges::views::generate(
+        random_select(std::move(create_random_state), std::move(sample_existing_state), gen, random_state_probability));
   }
 
  private:
@@ -532,7 +536,7 @@ class KldLimiter : public Mixin {
   [[nodiscard]] auto take_samples() const {
     using particle_type = typename Mixin::self_type::particle_type;
     return ranges::views::transform(beluga::make_from_state<particle_type>) |
-           ranges::views::transform(beluga::set_cluster(parameters_.spatial_resolution)) |
+           ranges::views::transform(beluga::build_clusterization_function(parameters_.spatial_resolution)) |
            ranges::views::take_while(
                beluga::kld_condition(parameters_.min_samples, parameters_.kld_epsilon, parameters_.kld_z),
                [](auto&& particle) { return cluster(particle); }) |

beluga/include/beluga/sensor.hpp

@@ -41,7 +41,7 @@
  * Then:
  * - `p.update_sensor(m)` will update the sensor model with `p`.
  *   This will not actually update the particle weights, but the update done here
- *   will be used in the nexts `importance_weight()` method calls.
+ *   will be used in the following `importance_weight()` method calls.
  * - `cp.importance_weight(s)` returns a `T::weight_type` instance representing the weight of the particle.
  * - `cp.make_random_state()` returns a `T::state_type` instance.
  *

beluga/include/beluga/sensor/likelihood_field_model.hpp

@@ -155,8 +155,9 @@ class LikelihoodFieldModel : public Mixin {
   [[nodiscard]] weight_type importance_weight(const state_type& state) const {
     const auto transform = grid_.origin().inverse() * state;
     const auto lock = std::shared_lock<std::shared_mutex>{points_mutex_};
+    constexpr auto kAvgUnknownSpaceOccupancyProb = 0.2;
     return std::transform_reduce(
-        points_.cbegin(), points_.cend(), 0.0, std::plus{},
+        points_.cbegin(), points_.cend(), 1.0, std::multiplies{},
         [this, x_offset = transform.translation().x(), y_offset = transform.translation().y(),
          cos_theta = transform.so2().unit_complex().x(),
          sin_theta = transform.so2().unit_complex().y()](const auto& point) {
@@ -166,7 +167,7 @@ class LikelihoodFieldModel : public Mixin {
           const auto x = point.first * cos_theta - point.second * sin_theta + x_offset;
           const auto y = point.first * sin_theta + point.second * cos_theta + y_offset;
           const auto index = grid_.index(x, y);
-          return index < likelihood_field_.size() ? likelihood_field_[index] : 0.0;
+          return index < likelihood_field_.size() ? likelihood_field_[index] : kAvgUnknownSpaceOccupancyProb;
         });
   }
 

beluga/include/beluga/type_traits/particle_traits.hpp

@@ -258,6 +258,7 @@ template <class Particle, class T = typename particle_traits<Particle>::state_ty
 constexpr auto make_from_state(T&& value) {
   auto particle = Particle{};
   state(particle) = std::forward<T>(value);
+  weight(particle) = 1.0;
   return particle;
 }
 

beluga/test/beluga/algorithm/test_particle_filter.cpp

@@ -15,17 +15,35 @@
 #include <gmock/gmock.h>
 
 #include <beluga/algorithm/particle_filter.hpp>
+#include <beluga/type_traits/particle_traits.hpp>
 #include <beluga/views.hpp>
+
 #include <ciabatta/ciabatta.hpp>
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/generate.hpp>
 #include <range/v3/view/take_exactly.hpp>
 #include <range/v3/view/transform.hpp>
 
+namespace beluga {
+template <>
+struct particle_traits<std::pair<double, double>> {
+  template <typename T>
+  static constexpr auto state(T&& particle) {
+    return particle.first;
+  }
+
+  template <typename T>
+  static constexpr auto weight(T&& particle) {
+    return particle.second;
+  }
+};
+}  // namespace beluga
+
 namespace {
 
 using testing::Each;
 using testing::Return;
+using testing::ReturnPointee;
 
 template <class Mixin>
 class MockMixin : public Mixin {
@@ -34,6 +52,8 @@ class MockMixin : public Mixin {
   MOCK_METHOD(double, importance_weight, (double state), (const));
   MOCK_METHOD(bool, do_resampling_vote, ());
 
+  auto particles() { return particles_ | ranges::views::common; }
+
   auto states() { return particles_ | beluga::views::elements<0>; }
 
   auto weights() { return particles_ | beluga::views::elements<1>; }
@@ -50,7 +70,8 @@ class MockMixin : public Mixin {
 
   template <class Generator>
   auto generate_samples_from_particles(Generator&&) const {
-    return ranges::views::generate([]() { return 0.0; });
+    // for the purpose of the test, return the existing states unchanged
+    return particles_ | beluga::views::elements<0> | ranges::views::common;
   }
 
   auto take_samples() const {
@@ -77,16 +98,80 @@ TEST(BootstrapParticleFilter, InitializeParticles) {
   EXPECT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.));
 }
 
-TEST(BootstrapParticleFilter, Update) {
+TEST(BootstrapParticleFilter, UpdateWithoutResampling) {
   auto filter = ParticleFilter{};
-  EXPECT_CALL(filter, apply_motion(0.)).WillRepeatedly(Return(2.));
-  EXPECT_CALL(filter, importance_weight(2.)).WillRepeatedly(Return(3.));
-  EXPECT_THAT(filter.states() | ranges::to<std::vector>, Each(0.));
-  filter.sample();
-  EXPECT_THAT(filter.states() | ranges::to<std::vector>, Each(2.));
-  EXPECT_THAT(filter.weights() | ranges::to<std::vector>, Each(1.));
-  filter.importance_sample();
-  EXPECT_THAT(filter.weights() | ranges::to<std::vector>, Each(3.));
+
+  const auto motion_increment = 2.0;
+  const auto weight_reduction_factor = 0.707;
+
+  auto expected_initial_state = 0.0;
+  auto expected_final_state = motion_increment;
+  auto expected_initial_weight = 1.0;
+  auto expected_final_weight = weight_reduction_factor;
+
+  EXPECT_CALL(filter, apply_motion(::testing::_)).WillRepeatedly(ReturnPointee(&expected_final_state));
+  EXPECT_CALL(filter, importance_weight(::testing::_)).WillRepeatedly(Return(weight_reduction_factor));
+  EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(Return(false));
+
+  for (auto iteration = 0; iteration < 5; ++iteration) {
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_initial_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+
+    // apply motion on all particles, particles will have updated states, but unchanged weight
+    filter.sample();
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+
+    // updating particle weights, particles will have updated weights, but unchanged state
+    filter.importance_sample();
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_final_weight));
+
+    // resample, but with sampling policy preventing decimation
+    filter.resample();
+
+    expected_initial_state = expected_final_state;
+    expected_initial_weight = expected_final_weight;
+    expected_final_state += motion_increment;
+    expected_final_weight *= weight_reduction_factor;
+  }
+}
+
+TEST(BootstrapParticleFilter, UpdateWithResampling) {
+  auto filter = ParticleFilter{};
+
+  const auto motion_increment = 2.0;
+  const auto weight_reduction_factor = 0.707;
+
+  auto expected_initial_state = 0.0;
+  auto expected_final_state = motion_increment;
+  const auto expected_initial_weight = 1.0;
+  const auto expected_final_weight = weight_reduction_factor;
+
+  EXPECT_CALL(filter, apply_motion(::testing::_)).WillRepeatedly(ReturnPointee(&expected_final_state));
+  EXPECT_CALL(filter, importance_weight(::testing::_)).WillRepeatedly(Return(weight_reduction_factor));
+  EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(Return(true));
+
+  for (auto iteration = 0; iteration < 4; ++iteration) {
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_initial_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+
+    // apply motion on all particles, particles will have updated states, but unchanged weight
+    filter.sample();
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_initial_weight));
+
+    // updating particle weights, particles will have updated weights, but unchanged state
+    filter.importance_sample();
+    ASSERT_THAT(filter.states() | ranges::to<std::vector>, Each(expected_final_state));
+    ASSERT_THAT(filter.weights() | ranges::to<std::vector>, Each(expected_final_weight));
+
+    // resample, resampling will reset weights
+    filter.resample();
+
+    expected_initial_state = expected_final_state;
+    expected_final_state += motion_increment;
+  }
 }
 
 }  // namespace