fixup! Integrate sensor probabilities accross resampling iterations

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -19,8 +19,6 @@
 #include <random>
 #include <utility>
 
-#include <beluga/type_traits/particle_traits.hpp>
-
 #include <range/v3/view/common.hpp>
 
 /**
@@ -55,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;
 
@@ -96,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;
 };
@@ -172,7 +174,6 @@ 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());
     }
@@ -184,21 +185,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 particles = this->self().particles() | ranges::views::common;
-    auto particle_weight_updater = [this](const auto& particle) {
-      return weight(particle) * this->self().importance_weight(state(particle));
+    auto particle_weight_updater = [this](const auto& state, auto weight) {
+      return weight * this->self().importance_weight(state);
     };
+    auto states = this->self().states() | ranges::views::common;
+    auto weights = this->self().weights() | ranges::views::common;
     std::transform(
-        std::forward<ExecutionPolicy>(policy), std::begin(particles), std::end(particles),
-        std::begin(this->self().weights()), particle_weight_updater);
+        std::forward<ExecutionPolicy>(policy), std::begin(states), std::end(states), std::begin(weights),
+        std::begin(weights), particle_weight_updater);
   }
 };
 

beluga/include/beluga/algorithm/sampling.hpp

@@ -127,16 +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) {
+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();
   };
 }
 
 /// 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.
+/**
+ * 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,
@@ -152,7 +153,7 @@ auto random_select(Function1&& first, Function2&& second, RandomNumberGenerator&
  *  Its type is the same as the `Range` value type.
  */
 template <class Range, class Weights, class RandomNumberGenerator>
-auto build_multinomial_sampler(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 +173,7 @@ auto build_multinomial_sampler(const Range& samples, const Weights& weights, Ran
  *  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 build_clusterization_function(double resolution) {
+inline auto make_clusterization_function(double resolution) {
   return [resolution](auto&& particle) {
     using state_type = std::decay_t<decltype(state(particle))>;
     auto new_particle = particle;
@@ -303,7 +304,7 @@ class NaiveSampler : public Mixin {
   template <class Generator>
   [[nodiscard]] auto generate_samples_from_particles(Generator& gen) const {
     return ranges::views::generate(
-        beluga::build_multinomial_sampler(this->self().states(), this->self().weights(), gen));
+        beluga::make_multinomial_sampler(this->self().states(), this->self().weights(), gen));
   }
 };
 
@@ -377,9 +378,9 @@ class AdaptiveSampler : public Mixin {
     }
 
     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));
+    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:
@@ -536,7 +537,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::build_clusterization_function(parameters_.spatial_resolution)) |
+           ranges::views::transform(beluga::make_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/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,20 +156,22 @@ 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(
+    const auto pz = std::transform_reduce(
         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) {
+         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] : kAvgUnknownSpaceOccupancyProb;
+          return index < likelihood_field_.size() ? likelihood_field_[index] : unknown_space_occupancy_prob;
         });
+    // TODO(glpuga): Investigate why AMCL and QuickMCL use this formula for the weight.
+    // See https://github.com/ekumenlabs/beluga/issues/153
+    return pz * pz * pz;
   }
 
   /// \copydoc LaserSensorModelInterface2d::update_sensor(measurement_type&& points)
@@ -178,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/test/beluga/algorithm/test_sampling.cpp

@@ -37,9 +37,9 @@ class RandomSelectWithParam : public ::testing::TestWithParam<double> {};
 TEST_P(RandomSelectWithParam, Functional) {
   const double probability = GetParam();
   auto generator = std::mt19937{std::random_device()()};
-  auto output =
-      ranges::views::generate(beluga::random_select([]() { return 1; }, []() { return 2; }, generator, probability)) |
-      ranges::views::take_exactly(1'000'000);
+  auto output = ranges::views::generate(
+                    beluga::make_random_selector([]() { return 1; }, []() { return 2; }, generator, probability)) |
+                ranges::views::take_exactly(1'000'000);
   auto one_count = static_cast<size_t>(ranges::count(output, 1));
   auto two_count = static_cast<size_t>(ranges::count(output, 2));
   ASSERT_NEAR(probability, static_cast<double>(one_count) / static_cast<double>(one_count + two_count), 0.01);
@@ -66,7 +66,7 @@ TEST(RandomSample, Functional) {
   auto values = std::array<int, 4>{1, 2, 3, 4};
   auto weights = std::array<double, 4>{0.1, 0.4, 0.3, 0.2};
   auto generator = std::mt19937{std::random_device()()};
-  auto output = ranges::views::generate(beluga::build_multinomial_sampler(values, weights, generator)) |
+  auto output = ranges::views::generate(beluga::make_multinomial_sampler(values, weights, generator)) |
                 ranges::views::take_exactly(samples);
   AssertWeights(output, values, weights);
 }

beluga/test/beluga/sensor/test_likelihood_field_model.cpp

@@ -102,11 +102,11 @@ TEST(LikelihoodFieldModel, LikelihoodField) {
     kResolution};
 
   const double expected_likelihood_field[] = {  // NOLINT(modernize-avoid-c-arrays)
-    0.025, 0.025, 0.025, 0.070, 1.020,
-    0.025, 0.027, 0.070, 1.020, 0.070,
-    0.025, 0.070, 1.020, 0.070, 0.025,
-    0.070, 1.020, 0.070, 0.027, 0.025,
-    1.020, 0.070, 0.025, 0.025, 0.025
+    0.025, 0.025, 0.025, 0.069, 1.022,
+    0.025, 0.027, 0.069, 1.022, 0.069,
+    0.025, 0.069, 1.022, 0.069, 0.025,
+    0.069, 1.022, 0.069, 0.027, 0.025,
+    1.022, 0.069, 0.025, 0.025, 0.025
   };
   // clang-format on
 
@@ -131,19 +131,19 @@ TEST(LikelihoodFieldModel, ImportanceWeight) {
   auto mixin = UUT{params, grid};
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{1.25, 1.25}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(grid.origin()), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{2.25, 2.25}});
-  ASSERT_NEAR(0.025, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(0.000, mixin.importance_weight(grid.origin()), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{-50.0, 50.0}});
-  ASSERT_NEAR(0.200, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(0.000, mixin.importance_weight(grid.origin()), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{1.20, 1.20}, {1.25, 1.25}, {1.30, 1.30}});
-  ASSERT_NEAR(1.060, mixin.importance_weight(grid.origin()), 0.01);
+  ASSERT_NEAR(1.220, mixin.importance_weight(grid.origin()), 0.01);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{0.0, 0.0}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{1.25, 1.25}}), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{1.25, 1.25}}), 0.003);
 }
 
 TEST(LikelihoodFieldModel, GridWithOffset) {
@@ -163,10 +163,10 @@ TEST(LikelihoodFieldModel, GridWithOffset) {
   auto mixin = UUT{params, grid};
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{4.5, 4.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(Sophus::SE2d{}), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(Sophus::SE2d{}), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{9.5, 9.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(grid.origin()), 0.003);
 }
 
 TEST(LikelihoodFieldModel, GridWithRotation) {
@@ -186,10 +186,10 @@ TEST(LikelihoodFieldModel, GridWithRotation) {
   auto mixin = UUT{params, grid};
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{-9.5, 9.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(Sophus::SE2d{}), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(Sophus::SE2d{}), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{9.5, 9.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(grid.origin()), 0.003);
 }
 
 TEST(LikelihoodFieldModel, GridWithRotationAndOffset) {
@@ -212,10 +212,10 @@ TEST(LikelihoodFieldModel, GridWithRotationAndOffset) {
   auto mixin = UUT{params, grid};
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{-4.5, 4.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(Sophus::SE2d{}), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(Sophus::SE2d{}), 0.003);
 
   mixin.update_sensor(std::vector<std::pair<double, double>>{{9.5, 9.5}});
-  ASSERT_NEAR(1.020, mixin.importance_weight(grid.origin()), 0.003);
+  ASSERT_NEAR(1.068, mixin.importance_weight(grid.origin()), 0.003);
 }
 
 }  // namespace

beluga/test/benchmark/benchmark_sampling.cpp

@@ -56,7 +56,7 @@ void BM_FixedResample(benchmark::State& state) {
   }
 
   for (auto _ : state) {
-    auto&& samples = ranges::views::generate(beluga::build_multinomial_sampler(
+    auto&& samples = ranges::views::generate(beluga::make_multinomial_sampler(
                          beluga::views::all(container), beluga::views::weights(container), generator)) |
                      ranges::views::take_exactly(container_size) | ranges::views::common;
     auto first = std::begin(beluga::views::all(new_container));
@@ -93,9 +93,9 @@ void BM_AdaptiveResample(benchmark::State& state) {
   double kld_z = 3.;
 
   for (auto _ : state) {
-    auto&& samples = ranges::views::generate(beluga::build_multinomial_sampler(
+    auto&& samples = ranges::views::generate(beluga::make_multinomial_sampler(
                          beluga::views::all(container), beluga::views::weights(container), generator)) |
-                     ranges::views::transform(beluga::build_clusterization_function(resolution)) |
+                     ranges::views::transform(beluga::make_clusterization_function(resolution)) |
                      ranges::views::take_while(
                          beluga::kld_condition(min_samples, kld_epsilon, kld_z), beluga::cluster<const Particle&>) |
                      ranges::views::take(container_size) | ranges::views::common;