From 5db2870b676ef7cd5402dd4f4cb9790078d2c2fe Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sat, 1 Apr 2023 10:13:22 -0300 Subject: [PATCH] Integrate sensor probabilities across resampling iterations (#149) Related to #57. - Initializes weight to 1 - Incorporates previous weight to the new weight calculation during the importance weight step. - Corrects importance_weight formula - Updates related tests - Minor documentation and testing updates. Signed-off-by: Gerardo Puga --- .../beluga/algorithm/particle_filter.hpp | 12 +- beluga/include/beluga/algorithm/sampling.hpp | 37 +++--- beluga/include/beluga/sensor.hpp | 2 +- .../beluga/sensor/likelihood_field_model.hpp | 13 ++- .../beluga/type_traits/particle_traits.hpp | 1 + .../beluga/algorithm/test_particle_filter.cpp | 105 ++++++++++++++++-- .../test/beluga/algorithm/test_sampling.cpp | 33 ++++-- .../sensor/test_likelihood_field_model.cpp | 32 +++--- beluga/test/benchmark/benchmark_sampling.cpp | 6 +- 9 files changed, 180 insertions(+), 61 deletions(-) diff --git a/beluga/include/beluga/algorithm/particle_filter.hpp b/beluga/include/beluga/algorithm/particle_filter.hpp index a8244205f..581f9f4f8 100644 --- a/beluga/include/beluga/algorithm/particle_filter.hpp +++ b/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 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(policy), std::begin(states), std::end(states), std::begin(this->self().weights()), - [this](const auto& state) { return this->self().importance_weight(state); }); + std::forward(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); }); } }; diff --git a/beluga/include/beluga/algorithm/sampling.hpp b/beluga/include/beluga/algorithm/sampling.hpp index 74bd5d11e..2860e64f4 100644 --- a/beluga/include/beluga/algorithm/sampling.hpp +++ b/beluga/include/beluga/algorithm/sampling.hpp @@ -127,15 +127,17 @@ namespace beluga { * The return type is `decltype(first())`. */ template -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(first), second = std::forward(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 -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 -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 explicit RandomStateGenerator(Args&&... args) : Mixin(std::forward(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 [[nodiscard]] auto generate_samples(Generator& gen) { @@ -288,7 +290,7 @@ class NaiveSampler : public Mixin { template explicit NaiveSampler(Args&&... args) : Mixin(std::forward(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 [[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 [[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) | - 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); }) | diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index e60130400..641064f3f 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/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. * diff --git a/beluga/include/beluga/sensor/likelihood_field_model.hpp b/beluga/include/beluga/sensor/likelihood_field_model.hpp index c949b1ca9..4eb1cd8d9 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,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{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 free_cells_; std::vector likelihood_field_; diff --git a/beluga/include/beluga/type_traits/particle_traits.hpp b/beluga/include/beluga/type_traits/particle_traits.hpp index b09353c09..5f93a3d96 100644 --- a/beluga/include/beluga/type_traits/particle_traits.hpp +++ b/beluga/include/beluga/type_traits/particle_traits.hpp @@ -258,6 +258,7 @@ template ::state_ty constexpr auto make_from_state(T&& value) { auto particle = Particle{}; state(particle) = std::forward(value); + weight(particle) = 1.0; return particle; } diff --git a/beluga/test/beluga/algorithm/test_particle_filter.cpp b/beluga/test/beluga/algorithm/test_particle_filter.cpp index c22da8903..5cd62670b 100644 --- a/beluga/test/beluga/algorithm/test_particle_filter.cpp +++ b/beluga/test/beluga/algorithm/test_particle_filter.cpp @@ -15,17 +15,35 @@ #include #include +#include #include + #include #include #include #include #include +namespace beluga { +template <> +struct particle_traits> { + template + static constexpr auto state(T&& particle) { + return particle.first; + } + + template + static constexpr auto weight(T&& particle) { + return particle.second; + } +}; +} // namespace beluga + namespace { using testing::Each; using testing::Return; +using testing::ReturnPointee; template 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 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, 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, Each(0.)); - filter.sample(); - EXPECT_THAT(filter.states() | ranges::to, Each(2.)); - EXPECT_THAT(filter.weights() | ranges::to, Each(1.)); - filter.importance_sample(); - EXPECT_THAT(filter.weights() | ranges::to, 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, Each(expected_initial_state)); + ASSERT_THAT(filter.weights() | ranges::to, 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, Each(expected_final_state)); + ASSERT_THAT(filter.weights() | ranges::to, Each(expected_initial_weight)); + + // updating particle weights, particles will have updated weights, but unchanged state + filter.importance_sample(); + ASSERT_THAT(filter.states() | ranges::to, Each(expected_final_state)); + ASSERT_THAT(filter.weights() | ranges::to, 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, Each(expected_initial_state)); + ASSERT_THAT(filter.weights() | ranges::to, 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, Each(expected_final_state)); + ASSERT_THAT(filter.weights() | ranges::to, Each(expected_initial_weight)); + + // updating particle weights, particles will have updated weights, but unchanged state + filter.importance_sample(); + ASSERT_THAT(filter.states() | ranges::to, Each(expected_final_state)); + ASSERT_THAT(filter.weights() | ranges::to, Each(expected_final_weight)); + + // resample, resampling will reset weights + filter.resample(); + + expected_initial_state = expected_final_state; + expected_final_state += motion_increment; + } } } // namespace diff --git a/beluga/test/beluga/algorithm/test_sampling.cpp b/beluga/test/beluga/algorithm/test_sampling.cpp index ecb4fce85..e9a2002fb 100644 --- a/beluga/test/beluga/algorithm/test_sampling.cpp +++ b/beluga/test/beluga/algorithm/test_sampling.cpp @@ -27,6 +27,7 @@ namespace { using testing::_; +using testing::DoubleEq; using testing::Each; using testing::Return; using testing::ReturnRef; @@ -36,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); @@ -65,8 +66,8 @@ 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::random_sample(values, weights, generator)) | ranges::views::take_exactly(samples); + auto output = ranges::views::generate(beluga::make_multinomial_sampler(values, weights, generator)) | + ranges::views::take_exactly(samples); AssertWeights(output, values, weights); } @@ -125,7 +126,7 @@ class MockStorage : public Mixin { template explicit MockStorage(Args&&... rest) : Mixin(std::forward(rest)...) {} - MOCK_METHOD(double, make_random_state, (std::mt19937&)); + MOCK_METHOD(int, make_random_state, (std::mt19937&)); MOCK_METHOD(const std::vector&, states, (), (const)); MOCK_METHOD(const std::vector&, weights, (), (const)); }; @@ -133,10 +134,10 @@ class MockStorage : public Mixin { TEST(RandomStateGenerator, InstantiateAndCall) { auto generator = std::mt19937{std::random_device()()}; auto instance = ciabatta::mixin{}; - EXPECT_CALL(instance, make_random_state(_)).WillRepeatedly(Return(1.5)); + EXPECT_CALL(instance, make_random_state(_)).WillRepeatedly(Return(5)); auto output = instance.generate_samples(generator) | ranges::views::take_exactly(50) | ranges::to; ASSERT_EQ(output.size(), 50); - ASSERT_THAT(output, Each(1.5)); + ASSERT_THAT(output, Each(5)); } TEST(NaiveSampler, Distribution) { @@ -211,6 +212,14 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple(0.5, 0.1, 0.08), std::make_tuple(0.5, 0.0, 0.10))); +TEST(FixedLimiter, TakeSamplesLeavesUnitWeight) { + auto instance = ciabatta::mixin{beluga::FixedLimiterParam{1'200}}; + auto output = ranges::views::generate([]() { return 1; }) | instance.take_samples() | + ranges::views::transform([](const auto& p) -> double { return std::get<1>(p); }) | + ranges::to; + ASSERT_THAT(output, Each(DoubleEq(1.0))); +} + TEST(FixedLimiter, TakeMaximum) { auto instance = ciabatta::mixin{beluga::FixedLimiterParam{1'200}}; auto output = ranges::views::generate([]() { return 1; }) | instance.take_samples() | ranges::to; @@ -226,6 +235,14 @@ struct MockStorageWithCluster : public Mixin { explicit MockStorageWithCluster(Args&&... rest) : Mixin(std::forward(rest)...) {} }; +TEST(KldLimiter, TakeSamplesLeavesUnitWeight) { + auto instance = ciabatta::mixin{beluga::FixedLimiterParam{1'200}}; + auto output = ranges::views::generate([]() { return 1; }) | instance.take_samples() | + ranges::views::transform([](const auto& p) -> double { return std::get<1>(p); }) | + ranges::to; + ASSERT_THAT(output, Each(DoubleEq(1.0))); +} + using state = std::pair; TEST(KldLimiter, TakeMaximum) { diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp index 01bc0ae76..8dae0a1a0 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(2.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(1.000, mixin.importance_weight(grid.origin()), 0.003); mixin.update_sensor(std::vector>{{-50.0, 50.0}}); - ASSERT_NEAR(0.000, mixin.importance_weight(grid.origin()), 0.003); + ASSERT_NEAR(1.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(3.060, mixin.importance_weight(grid.origin()), 0.01); + ASSERT_NEAR(4.205, 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(2.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(2.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(2.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(2.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(2.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(2.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(2.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 10dad64fe..f64006cbb 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::random_sample( + 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::random_sample( + auto&& samples = ranges::views::generate(beluga::make_multinomial_sampler( beluga::views::all(container), beluga::views::weights(container), generator)) | - ranges::views::transform(beluga::set_cluster(hasher)) | + ranges::views::transform(beluga::make_clusterization_function(hasher)) | ranges::views::take_while( beluga::kld_condition(min_samples, kld_epsilon, kld_z), beluga::cluster) | ranges::views::take(container_size) | ranges::views::common;