From 1311a4f2fb3396e650203a763db89c07de42e415 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Tue, 28 Mar 2023 22:00:28 -0300 Subject: [PATCH] fixup! Integrate sensor probabilities accross resampling iterations --- .../beluga/algorithm/particle_filter.hpp | 21 ++++++------ beluga/include/beluga/algorithm/sampling.hpp | 21 ++++++------ .../beluga/sensor/likelihood_field_model.hpp | 14 +++++--- .../test/beluga/algorithm/test_sampling.cpp | 8 ++--- .../sensor/test_likelihood_field_model.cpp | 32 +++++++++---------- beluga/test/benchmark/benchmark_sampling.cpp | 6 ++-- 6 files changed, 54 insertions(+), 48 deletions(-) diff --git a/beluga/include/beluga/algorithm/particle_filter.hpp b/beluga/include/beluga/algorithm/particle_filter.hpp index 888d29607..3b8b6729c 100644 --- a/beluga/include/beluga/algorithm/particle_filter.hpp +++ b/beluga/include/beluga/algorithm/particle_filter.hpp @@ -19,8 +19,6 @@ #include #include -#include - #include /** @@ -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,7 +185,6 @@ class BootstrapParticleFilter : public Mixin { template void sample_impl(ExecutionPolicy&& policy) { auto states = this->self().states() | ranges::views::common; - // update particle states, conserve their weight unchanged std::transform( std::forward(policy), std::begin(states), std::end(states), std::begin(states), [this](const auto& state) { return this->self().apply_motion(state, generator_); }); @@ -192,13 +192,14 @@ class BootstrapParticleFilter : public Mixin { template 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(policy), std::begin(particles), std::end(particles), - std::begin(this->self().weights()), particle_weight_updater); + std::forward(policy), std::begin(states), std::end(states), std::begin(weights), + std::begin(weights), particle_weight_updater); } }; diff --git a/beluga/include/beluga/algorithm/sampling.hpp b/beluga/include/beluga/algorithm/sampling.hpp index 48fe4e361..e338ee36f 100644 --- a/beluga/include/beluga/algorithm/sampling.hpp +++ b/beluga/include/beluga/algorithm/sampling.hpp @@ -127,7 +127,7 @@ namespace beluga { * The return type is `decltype(first())`. */ template -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(first), second = std::forward(second), &generator, distribution = std::bernoulli_distribution{probability}]() mutable { return distribution(generator) ? first() : second(); @@ -135,8 +135,9 @@ auto random_select(Function1&& first, Function2&& second, RandomNumberGenerator& } /// 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 -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; auto new_particle = particle; @@ -303,7 +304,7 @@ class NaiveSampler : public Mixin { template [[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) | - 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); }) | diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index 153b903c7..b97641588 100644 --- a/beluga/include/beluga/sensor/likelihood_field_model.hpp +++ b/beluga/include/beluga/sensor/likelihood_field_model.hpp @@ -119,6 +119,7 @@ class LikelihoodFieldModel : public Mixin { template explicit LikelihoodFieldModel(const param_type& params, const OccupancyGrid& grid, Args&&... rest) : Mixin(std::forward(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{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 free_cells_; std::vector likelihood_field_; diff --git a/beluga/test/beluga/algorithm/test_sampling.cpp b/beluga/test/beluga/algorithm/test_sampling.cpp index 99cde8485..fb14d99ec 100644 --- a/beluga/test/beluga/algorithm/test_sampling.cpp +++ b/beluga/test/beluga/algorithm/test_sampling.cpp @@ -37,9 +37,9 @@ class RandomSelectWithParam : public ::testing::TestWithParam {}; 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(ranges::count(output, 1)); auto two_count = static_cast(ranges::count(output, 2)); ASSERT_NEAR(probability, static_cast(one_count) / static_cast(one_count + two_count), 0.01); @@ -66,7 +66,7 @@ TEST(RandomSample, Functional) { auto values = std::array{1, 2, 3, 4}; auto weights = std::array{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); } diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp index cfc5167e1..0e6f52e60 100644 --- a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp +++ b/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>{{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>{{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>{{-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>{{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>{{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>{{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>{{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>{{-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>{{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>{{-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>{{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 diff --git a/beluga/test/benchmark/benchmark_sampling.cpp b/beluga/test/benchmark/benchmark_sampling.cpp index 363bfa554..3cbbeb78a 100644 --- a/beluga/test/benchmark/benchmark_sampling.cpp +++ b/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) | ranges::views::take(container_size) | ranges::views::common;