Integrate sensor probabilities across resampling iterations

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -53,7 +53,7 @@ struct BaseParticleFilterInterface {
   /// Update the states of the particles.
   /**
    * This step generates a hyphotetical state based on the current
-   * particle state and controls.
+   * particle state and controls, while leaving their importance weights unchanged.
    */
   virtual void sample() = 0;
 
@@ -94,6 +94,10 @@ struct BaseParticleFilterInterface {
    * This step creates a new particle set drawing samples from the
    * current particle set. The probability of drawing each particle
    * is given by its importance weight.
+   *
+   * The resampling process can be inhibited by the resampling policies.
+   *
+   * If resampling is performed, the importance weights of all particles are reset to 1.0.
    */
   virtual void resample() = 0;
 };
@@ -189,9 +193,11 @@ class BootstrapParticleFilter : public Mixin {
   template <typename ExecutionPolicy>
   void importance_sample_impl(ExecutionPolicy&& policy) {
     auto states = this->self().states() | ranges::views::common;
+    auto weights = this->self().weights() | ranges::views::common;
     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(states), std::end(states), std::begin(weights),
+        std::begin(weights),
+        [this](const auto& state, auto weight) { return weight * this->self().importance_weight(state); });
   }
 };
 

beluga/include/beluga/algorithm/sampling.hpp

@@ -127,15 +127,17 @@ 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 make_random_selector(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 +149,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 make_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);
@@ -172,7 +174,7 @@ auto random_sample(const Range& samples, const Weights& weights, RandomNumberGen
  * hash according to the specified resolutions in each axis.
  */
 template <class Hasher>
-inline auto set_cluster(Hasher&& hasher) {
+inline auto make_clusterization_function(Hasher&& hasher) {
   return [hasher](auto&& particle) {
     auto new_particle = particle;
     cluster(new_particle) = hasher(state(particle));
@@ -249,7 +251,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 +260,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 +290,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 +299,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::make_multinomial_sampler(this->self().states(), this->self().weights(), gen));
   }
 };
 
@@ -359,7 +362,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 +377,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 = make_multinomial_sampler(this->self().states(), weights, gen);
+    return ranges::views::generate(make_random_selector(
+        std::move(create_random_state), std::move(sample_existing_state), gen, random_state_probability));
   }
 
  private:
@@ -538,7 +543,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_hasher)) |
+           ranges::views::transform(beluga::make_clusterization_function(parameters_.spatial_hasher)) |
            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

@@ -119,6 +119,7 @@ class LikelihoodFieldModel : public Mixin {
   template <class... Args>
   explicit LikelihoodFieldModel(const param_type& params, const OccupancyGrid& grid, Args&&... rest)
       : Mixin(std::forward<Args>(rest)...),
+        params_{params},
         grid_{grid},
         free_cells_{make_free_cells_vector(grid)},
         likelihood_field_{make_likelihood_field(params, grid)} {}
@@ -155,18 +156,21 @@ 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_};
+    // TODO(glpuga): Investigate why AMCL and QuickMCL both use this formula for the weight.
+    // See https://github.com/ekumenlabs/beluga/issues/153
     return std::transform_reduce(
-        points_.cbegin(), points_.cend(), 0.0, std::plus{},
+        points_.cbegin(), points_.cend(), 1.0, std::plus{},
         [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) {
+         cos_theta = transform.so2().unit_complex().x(), sin_theta = transform.so2().unit_complex().y(),
+         unknown_space_occupancy_prob = 1. / params_.max_laser_distance](const auto& point) {
           // Transform the end point of the laser to the global coordinate system.
           // Not using Eigen/Sophus because they make the routine x10 slower.
           // See `benchmark_likelihood_field_model.cpp` for reference.
           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;
+          const auto pz = index < likelihood_field_.size() ? likelihood_field_[index] : unknown_space_occupancy_prob;
+          return pz * pz * pz;
         });
   }
 
@@ -177,6 +181,7 @@ class LikelihoodFieldModel : public Mixin {
   }
 
  private:
+  param_type params_;
   OccupancyGrid grid_;
   std::vector<std::size_t> free_cells_;
   std::vector<double> likelihood_field_;

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