From 91dc4182ee3624557ed8ca3ca2ea12e3fa716ebc Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Mon, 6 Mar 2023 11:33:09 -0300 Subject: [PATCH] Add new resampling policies (#119) Related to #57 - Refactors minimum motion check into a resampling policy - Adds a policy that implements the "resampling_interval" parameter. - Adds a policy that implements the "selective_resampling" parameters. Signed-off-by: Gerardo Puga --- beluga/docs/doxygen_cites.bib | 22 ++ .../beluga/algorithm/particle_filter.hpp | 36 ++- beluga/include/beluga/beluga.hpp | 1 + beluga/include/beluga/motion.hpp | 2 + .../motion/differential_drive_model.hpp | 7 +- .../beluga/motion/stationary_model.hpp | 6 + beluga/include/beluga/resampling_policies.hpp | 46 ++++ .../resample_interval_policy.hpp | 65 +++++ .../resample_on_motion_policy.hpp | 95 +++++++ .../resampling_policies_poller.hpp | 68 +++++ .../selective_resampling_policy.hpp | 75 ++++++ beluga/test/beluga/CMakeLists.txt | 4 + .../beluga/algorithm/test_particle_filter.cpp | 15 +- .../motion/test_differential_drive_model.cpp | 33 +++ .../test_resample_interval_policy.cpp | 67 +++++ .../test_resample_on_motion_policy.cpp | 245 ++++++++++++++++++ .../test_resampling_policies_poller.cpp | 152 +++++++++++ .../test_selective_resampling_policy.cpp | 109 ++++++++ beluga_amcl/src/amcl_node.cpp | 47 ++-- 19 files changed, 1069 insertions(+), 26 deletions(-) create mode 100644 beluga/include/beluga/resampling_policies.hpp create mode 100644 beluga/include/beluga/resampling_policies/resample_interval_policy.hpp create mode 100644 beluga/include/beluga/resampling_policies/resample_on_motion_policy.hpp create mode 100644 beluga/include/beluga/resampling_policies/resampling_policies_poller.hpp create mode 100644 beluga/include/beluga/resampling_policies/selective_resampling_policy.hpp create mode 100644 beluga/test/beluga/resampling_policies/test_resample_interval_policy.cpp create mode 100644 beluga/test/beluga/resampling_policies/test_resample_on_motion_policy.cpp create mode 100644 beluga/test/beluga/resampling_policies/test_resampling_policies_poller.cpp create mode 100644 beluga/test/beluga/resampling_policies/test_selective_resampling_policy.cpp diff --git a/beluga/docs/doxygen_cites.bib b/beluga/docs/doxygen_cites.bib index 9fa58e591..d60550933 100644 --- a/beluga/docs/doxygen_cites.bib +++ b/beluga/docs/doxygen_cites.bib @@ -8,3 +8,25 @@ @book{thrun2005probabilistic year={2005}, publisher={MIT Press} } + +@ARTICLE{grisetti2007selectiveresampling, + author={Grisetti, Giorgio and Stachniss, Cyrill and Burgard, Wolfram}, + journal={IEEE Transactions on Robotics}, + title={Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters}, + year={2007}, + volume={23}, + number={1}, + pages={34-46}, + doi={10.1109/TRO.2006.889486} +} + +@ARTICLE{tiacheng2015resamplingmethods, + author={Li, Tiancheng and Bolic, Miodrag and Djuric, Petar M.}, + journal={IEEE Signal Processing Magazine}, + title={Resampling Methods for Particle Filtering: Classification, implementation, and strategies}, + year={2015}, + volume={32}, + number={3}, + pages={70-86}, + doi={10.1109/MSP.2014.2330626} +} diff --git a/beluga/include/beluga/algorithm/particle_filter.hpp b/beluga/include/beluga/algorithm/particle_filter.hpp index 8b8e1474a..3bc5f39f1 100644 --- a/beluga/include/beluga/algorithm/particle_filter.hpp +++ b/beluga/include/beluga/algorithm/particle_filter.hpp @@ -18,14 +18,19 @@ #include #include -#include -#include -#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include + /** * \file * \brief Implementation of particle filters. @@ -85,6 +90,7 @@ * The following is satisfied: * - `p.particles()` is valid and returns a view to a container that satisfies the * \ref ParticleContainerPage "ParticleContainer" requirements. + * - `p.weights()` is valid and returns a view to a container to the weights of the particles. * - `p.sample()` updates the particle filter particles based on the last motion update. * - `p.importance_sample()` updates the particle filter particles weight. * - `p.resample()` updates the particle filter, generating new particles from the old ones @@ -233,10 +239,14 @@ struct BootstrapParticleFilter : public Mixin { /** * The update will be done based on the Mixin implementation of the * \ref ParticleSampledGenerationPage "ParticleSampledGeneration" and - * \ref ParticleResamplingPage "ParticleResampling" named requirements. + * \ref ParticleResamplingPage "ParticleResampling" named requirements + * and the active resampling policies. */ void resample() { - particles_ = initialize_container(this->self().generate_samples_from(particles_) | this->self().take_samples()); + const auto resampling_vote_result = this->self().do_resampling_vote(); + if (resampling_vote_result) { + particles_ = initialize_container(this->self().generate_samples_from(particles_) | this->self().take_samples()); + } } private: @@ -285,6 +295,11 @@ struct MCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry< + ResamplingPoliciesPoller, + ResampleOnMotionPolicy, + ResampleIntervalPolicy, + SelectiveResamplingPolicy>::template mixin, MotionModel, SensorModel> { using ciabatta::mixin< @@ -293,6 +308,9 @@ struct MCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta:: + curry:: + mixin, MotionModel, SensorModel>::mixin; }; @@ -321,6 +339,11 @@ struct AMCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta::curry< + ResamplingPoliciesPoller, + ResampleOnMotionPolicy, + ResampleIntervalPolicy, + SelectiveResamplingPolicy>::template mixin, MotionModel, SensorModel> { using ciabatta::mixin< @@ -329,6 +352,9 @@ struct AMCL : public ciabatta::mixin< ciabatta::curry::template mixin, ciabatta::curry::template mixin, ciabatta::curry::template mixin, + ciabatta:: + curry:: + template mixin, MotionModel, SensorModel>::mixin; }; diff --git a/beluga/include/beluga/beluga.hpp b/beluga/include/beluga/beluga.hpp index 54f6bb493..8b0968c78 100644 --- a/beluga/include/beluga/beluga.hpp +++ b/beluga/include/beluga/beluga.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include diff --git a/beluga/include/beluga/motion.hpp b/beluga/include/beluga/motion.hpp index a6d13da7a..0f64a39d6 100644 --- a/beluga/include/beluga/motion.hpp +++ b/beluga/include/beluga/motion.hpp @@ -44,6 +44,8 @@ * will be used in subsequent calls to the `apply_motion()` method. * - `cp.apply_motion(s)` returns a `T::state_type`, that is the result of applying the motion model * to `s` based on the updates. + * - `cp.latest_motion_update() returns a std::optional with the latest motion update + * received through motion_update(). */ #endif diff --git a/beluga/include/beluga/motion/differential_drive_model.hpp b/beluga/include/beluga/motion/differential_drive_model.hpp index 92c222770..62d88780c 100644 --- a/beluga/include/beluga/motion/differential_drive_model.hpp +++ b/beluga/include/beluga/motion/differential_drive_model.hpp @@ -110,7 +110,7 @@ class DifferentialDriveModel : public Mixin { * That is done by the particle filter using the apply_motion() method * provided by this class. * - * \param pose Last odometry udpate. + * \param pose Last odometry update. */ void update_motion(const update_type& pose) { if (last_pose_) { @@ -146,11 +146,14 @@ class DifferentialDriveModel : public Mixin { last_pose_ = pose; } + /// Recovers latest motion update. + [[nodiscard]] std::optional latest_motion_update() const { return last_pose_; } + private: using DistributionParam = typename std::normal_distribution::param_type; DifferentialDriveModelParam params_; - std::optional last_pose_; + std::optional last_pose_; DistributionParam first_rotation_params_{0.0, 0.0}; DistributionParam second_rotation_params_{0.0, 0.0}; diff --git a/beluga/include/beluga/motion/stationary_model.hpp b/beluga/include/beluga/motion/stationary_model.hpp index d03e9ad7b..85061a4b8 100644 --- a/beluga/include/beluga/motion/stationary_model.hpp +++ b/beluga/include/beluga/motion/stationary_model.hpp @@ -69,6 +69,12 @@ class StationaryModel : public Mixin { * For the stationary model, updates are ignored. */ void update_motion(const update_type&) {} + + /// Recovers latest motion update. + /** + * For the stationary model, we don't ever have motion updates. + */ + [[nodiscard]] std::optional latest_motion_update() const { return std::nullopt; } }; } // namespace beluga diff --git a/beluga/include/beluga/resampling_policies.hpp b/beluga/include/beluga/resampling_policies.hpp new file mode 100644 index 000000000..f50baafe5 --- /dev/null +++ b/beluga/include/beluga/resampling_policies.hpp @@ -0,0 +1,46 @@ +// Copyright 2022 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RESAMPLING_POLICIES_HPP +#define BELUGA_RESAMPLING_POLICIES_HPP + +#include +#include +#include +#include + +/** + * \file + * \brief Includes all resampling policy related headers. + */ + +/** + * \page ResamplingPolicyPage beluga named requirements: ResamplingPolicy + * Requirements for a resampling policy to be used a beluga `ParticleFilter`. + * + * \section ResamplingPolicyRequirements Requirements + * A type `T` satisfies the `ResamplingPolicy` requirements if the following is satisfied. + * + * Given: + * - An instance `p` of `T`. + * - An instance `c` of `C`, where is C a particle filter that meets the requirements listed in the policy. + * + * Then: + * - `p.do_resampling(c)` will return true if resampling must be done according to the policy, false otherwise. This + * function is called in cascade when multiple policies are installed in the filter, and a given filter's + * do_resampling() function may not be called during a given iteration if a previously queried policy has already voted + * "false" (short-circuit evaluation of policies). + * */ + +#endif diff --git a/beluga/include/beluga/resampling_policies/resample_interval_policy.hpp b/beluga/include/beluga/resampling_policies/resample_interval_policy.hpp new file mode 100644 index 000000000..aed22dc8e --- /dev/null +++ b/beluga/include/beluga/resampling_policies/resample_interval_policy.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RESAMPLING_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP +#define BELUGA_RESAMPLING_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP + +#include + +/** + * \file + * \brief Implementation of the resample interval algorithm for resampling. + */ + +namespace beluga { + +/// Parameters used to construct a ResampleIntervalPolicy instance. +struct ResampleIntervalPolicyParam { + /// Interval of calls to do_resampling() out of which only the last iteration will do resampling. + std::size_t resample_interval_count{1}; +}; + +/// Implementation of the Resample Interval algorithm for resampling. +/** + * ResampleIntervalPolicy is an implementation of the \ref ResamplingPolicyPage "ResamplingPolicy" named requirements. + * */ +struct ResampleIntervalPolicy { + public: + /// Parameter type that the constructor uses to configure the policy. + using param_type = ResampleIntervalPolicyParam; + + /// Constructs a ResampleIntervalPolicy instance. + /** + * \param configuration Policy configuration data. + */ + explicit ResampleIntervalPolicy(const param_type& configuration) : configuration_{configuration} {} + + /// Vote whether resampling must be done according to this policy. + /** + * \tparam Concrete Type representing the concrete implementation of the filter. + */ + template + [[nodiscard]] bool do_resampling([[maybe_unused]] Concrete& filter) { + filter_update_counter_ = (filter_update_counter_ + 1) % configuration_.resample_interval_count; + return (filter_update_counter_ == 0); + } + + private: + param_type configuration_; //< Policy configuration + std::size_t filter_update_counter_{0}; //< Current cycle phase +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/resampling_policies/resample_on_motion_policy.hpp b/beluga/include/beluga/resampling_policies/resample_on_motion_policy.hpp new file mode 100644 index 000000000..65b6bc503 --- /dev/null +++ b/beluga/include/beluga/resampling_policies/resample_on_motion_policy.hpp @@ -0,0 +1,95 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RESAMPLING_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP +#define BELUGA_RESAMPLING_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP + +#include + +#include + +/** + * \file + * \brief Implementation of the resample-on-motion algorithm for resampling. + */ + +namespace beluga { + +/// Parameters used to construct a ResampleOnMotionPolicy instance. +struct ResampleOnMotionPolicyParam { + /// Distance threshold along x an y (independently) to trigger a resample. + double update_min_d{0.}; + /// Angular threshold to trigger a resample. + double update_min_a{0.}; +}; + +/// Implementation of the Resample-On-Motion algorithm for resampling. +/** + * ResampleOnMotionPolicy is an implementation of the \ref ResamplingPolicyPage "ResamplingPolicy" named requirements. + * */ +struct ResampleOnMotionPolicy { + public: + /// Parameter type that the constructor uses to configure the policy. + using param_type = ResampleOnMotionPolicyParam; + /// Type used to exchange and store motion updates by the motion model. + using motion_event = Sophus::SE2d; + + /// Constructs a ResampleOnMotionPolicy instance. + /** + * \param configuration Policy configuration data. + */ + explicit ResampleOnMotionPolicy(const param_type& configuration) : configuration_{configuration} {} + + /// Vote whether resampling must be done according to this policy. + /** + * \tparam Concrete Type representing the concrete implementation of the filter. + * It must satisfy the \ref MotionModelPage MotionModel requirements. + */ + template + [[nodiscard]] bool do_resampling(Concrete& filter) { + // To avoid loss of diversity in the particle population, don't + // resample when the state is known to be static. + // Probabilistic Robotics \cite thrun2005probabilistic Chapter 4.2.4. + auto current_pose = filter.latest_motion_update(); + + // default to letting other policies decide + bool must_do_resample{true}; + + if (current_pose && latest_resample_pose_) { + // calculate relative transform between previous pose and the current one + const auto delta = latest_resample_pose_->inverse() * current_pose.value(); + + // only resample if movement is above thresholds + must_do_resample = // + std::abs(delta.translation().x()) > configuration_.update_min_d || + std::abs(delta.translation().y()) > configuration_.update_min_d || + std::abs(delta.so2().log()) > configuration_.update_min_a; + } + + // we always measure the distance traveled since the last time we did resample + if (must_do_resample) { + latest_resample_pose_ = current_pose; + } + + return must_do_resample; + } + + private: + param_type configuration_; + std::optional latest_resample_pose_; +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/resampling_policies/resampling_policies_poller.hpp b/beluga/include/beluga/resampling_policies/resampling_policies_poller.hpp new file mode 100644 index 000000000..23c877a82 --- /dev/null +++ b/beluga/include/beluga/resampling_policies/resampling_policies_poller.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RESAMPLING_POLICIES_RESAMPLING_POLICIES_POLLER_HPP +#define BELUGA_RESAMPLING_POLICIES_RESAMPLING_POLICIES_POLLER_HPP + +#include + +#include + +/** + * \file + * \brief Implementation of a resampling policies poller. + */ + +namespace beluga { + +/// Resampling policy poller. +/** + * \tparam Mixin The mixed-in type. + * \tparam Policies Zero or more resampling policies. They must satisfy the \ref ResamplingPolicyPage "ResamplingPolicy" + * named requirements. + */ +template +struct ResamplingPoliciesPoller : public Mixin { + /// Constructs a ResamplingPoliciesPoller instance. + /** + * \tparam ...Rest Arguments types for the remaining mixin constructors. + * \param policy_configs Configuration parameters for each of the installed resampling policies. + * \param ...rest arguments that are not used by this part of the mixin, but by others. + */ + template + explicit ResamplingPoliciesPoller(const typename Policies::param_type&... policy_configs, Rest&&... rest) + : Mixin(std::forward(rest)...), policies_{policy_configs...} {} + + /// Evaluate all configured resampling policies, return true only if no policy votes "no". + /** + * Evaluation of the policies is done one by one, in the order in which they are listed in the "Policies" template + * parameter pack. + * + * Evaluation is done with short-circuit evaluation, meaning that as soon as one of the policies votes "false" the + * remaining policies will not be queried. + * + * If no policies have been installed, then the function will default to always returning "true". + * */ + [[nodiscard]] bool do_resampling_vote() { + // resampling voters are queried in cascade, with short-circuit evaluation. Order matters! + return std::apply([this](auto&... policies) { return (policies.do_resampling(this->self()) && ...); }, policies_); + } + + private: + std::tuple policies_; +}; + +} // namespace beluga + +#endif diff --git a/beluga/include/beluga/resampling_policies/selective_resampling_policy.hpp b/beluga/include/beluga/resampling_policies/selective_resampling_policy.hpp new file mode 100644 index 000000000..5cad5791d --- /dev/null +++ b/beluga/include/beluga/resampling_policies/selective_resampling_policy.hpp @@ -0,0 +1,75 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_RESAMPLING_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP +#define BELUGA_RESAMPLING_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP + +#include +#include + +/** + * \file + * \brief Implementation of the selective resampling algorithm for resampling. + */ + +namespace beluga { + +/// Parameters used to construct a SelectiveResamplingPolicy instance. +struct SelectiveResamplingPolicyParam { + /// Enable/disable the SelectiveResampling feature. + bool enabled{false}; +}; + +/// Implementation of the Selective Resampling algorithm for resampling. +/** + * SelectiveResamplingPolicy is an implementation of the \ref ResamplingPolicyPage "ResamplingPolicy" named + * requirements. + * + * The algorithm is based in \cite grisetti2007selectiveresampling, according to the description given in + * \cite tiacheng2015resamplingmethods. + * */ +struct SelectiveResamplingPolicy { + public: + /// Parameter type that the constructor uses to configure the policy. + using param_type = SelectiveResamplingPolicyParam; + + /// Constructs a SelectiveResamplingPolicy instance. + /** + * \param configuration Policy configuration data. + */ + explicit SelectiveResamplingPolicy(const param_type& configuration) : configuration_{configuration} {} + + /// Vote whether resampling must be done according to this policy. + /** + * \tparam Concrete Type representing the concrete implementation of the filter. + * It must satisfy the \ref BaseParticleFilterPage "BaseParticleFilter" named requirements. + */ + template + [[nodiscard]] bool do_resampling(Concrete& filter) { + if (!configuration_.enabled) { + return true; + } + const auto n_eff = + 1. / ranges::accumulate(filter.weights() | ranges::views::transform([](const auto w) { return w * w; }), 0.); + const auto n = static_cast(std::size(filter.weights())); + return n_eff < n / 2.; + } + + private: + param_type configuration_; +}; + +} // namespace beluga + +#endif diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index bc8440a6e..eaa9e5f1c 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -8,6 +8,10 @@ ament_add_gmock(test_beluga algorithm/test_sampling.cpp motion/test_differential_drive_model.cpp random/test_multivariate_normal_distribution.cpp + resampling_policies/test_resample_interval_policy.cpp + resampling_policies/test_resample_on_motion_policy.cpp + resampling_policies/test_resampling_policies_poller.cpp + resampling_policies/test_selective_resampling_policy.cpp sensor/test_likelihood_field_model.cpp test_tuple_vector.cpp test_spatial_hash.cpp) diff --git a/beluga/test/beluga/algorithm/test_particle_filter.cpp b/beluga/test/beluga/algorithm/test_particle_filter.cpp index b2b43490f..cc6904566 100644 --- a/beluga/test/beluga/algorithm/test_particle_filter.cpp +++ b/beluga/test/beluga/algorithm/test_particle_filter.cpp @@ -49,6 +49,8 @@ class MockMixin : public ciabatta::mixin, Mixin> { return range | ranges::views::all; } + MOCK_METHOD(bool, do_resampling_vote, ()); + auto take_samples() const { return ranges::views::take_exactly(max_samples()); } }; @@ -64,6 +66,7 @@ TEST(BootstrapParticleFilter, Update) { auto filter = MockMixin(); EXPECT_CALL(filter, apply_motion(1.)).WillRepeatedly(testing::Return(2.)); EXPECT_CALL(filter, importance_weight(2.)).WillRepeatedly(testing::Return(3.)); + EXPECT_CALL(filter, do_resampling_vote()).WillRepeatedly(testing::Return(true)); filter.update(); for (auto [state, weight] : filter.particles()) { ASSERT_EQ(state, 2.); @@ -74,9 +77,13 @@ TEST(BootstrapParticleFilter, Update) { template class MockMotionModel : public Mixin { public: + using update_type = Sophus::SE2d; + template explicit MockMotionModel(Args&&... args) : Mixin(std::forward(args)...) {} + [[nodiscard]] std::optional latest_motion_update() const { return std::nullopt; } + [[nodiscard]] double apply_motion(double state) { return state; } }; @@ -96,7 +103,9 @@ class MockSensorModel : public Mixin { TEST(MCL, InitializeFilter) { constexpr std::size_t kMaxSamples = 1'000; - auto filter = beluga::MCL{beluga::FixedResamplingParam{kMaxSamples}}; + auto filter = beluga::MCL{ + beluga::FixedResamplingParam{kMaxSamples}, beluga::ResampleOnMotionPolicyParam{}, + beluga::ResampleIntervalPolicyParam{}, beluga::SelectiveResamplingPolicyParam{}}; ASSERT_EQ(filter.particles().size(), kMaxSamples); } @@ -110,7 +119,9 @@ TEST(AMCL, InitializeFilter) { constexpr double kKldZ = 3.; auto filter = beluga::AMCL{ beluga::AdaptiveGenerationParam{kAlphaSlow, kAlphaFast}, - beluga::KldResamplingParam{kMinSamples, kMaxSamples, kSpatialResolution, kKldEpsilon, kKldZ}}; + beluga::KldResamplingParam{kMinSamples, kMaxSamples, kSpatialResolution, kKldEpsilon, kKldZ}, + beluga::ResampleOnMotionPolicyParam{}, beluga::ResampleIntervalPolicyParam{}, + beluga::SelectiveResamplingPolicyParam{}}; ASSERT_GE(filter.particles().size(), kMinSamples); } diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/beluga/test/beluga/motion/test_differential_drive_model.cpp index eb13ff123..0576ee805 100644 --- a/beluga/test/beluga/motion/test_differential_drive_model.cpp +++ b/beluga/test/beluga/motion/test_differential_drive_model.cpp @@ -154,4 +154,37 @@ TEST(DifferentialDriveModelSamples, RotateThirdQuadrant) { ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01); } +TEST(DifferentialDriveModelSamples, LatestMotionUpdateWorks) { + // tests that the latest motion update can be recovered + auto uut = MockMixin{beluga::DifferentialDriveModelParam{1.0, 0.0, 0.0, 0.0}}; + + auto make_motion_update = [](double x, double y, double phi) { + using Eigen::Vector2d; + using Sophus::SE2d; + using Sophus::SO2d; + return Sophus::SE2d{SO2d{phi}, Vector2d{x, y}}; + }; + + { + const auto lmu = uut.latest_motion_update(); + ASSERT_FALSE(lmu); + } + + { + const auto expected_motion_update = make_motion_update(0.1, 0.2, 0.3); + uut.update_motion(expected_motion_update); + const auto latest_motion_update = uut.latest_motion_update(); + ASSERT_TRUE(latest_motion_update); + ASSERT_THAT(expected_motion_update, testing::SE2Eq(latest_motion_update.value())); + } + + { + const auto expected_motion_update = make_motion_update(0.4, 0.5, 0.6); + uut.update_motion(expected_motion_update); + const auto latest_motion_update = uut.latest_motion_update(); + ASSERT_TRUE(latest_motion_update); + ASSERT_THAT(expected_motion_update, testing::SE2Eq(latest_motion_update.value())); + } +} + } // namespace diff --git a/beluga/test/beluga/resampling_policies/test_resample_interval_policy.cpp b/beluga/test/beluga/resampling_policies/test_resample_interval_policy.cpp new file mode 100644 index 000000000..a1e34f24e --- /dev/null +++ b/beluga/test/beluga/resampling_policies/test_resample_interval_policy.cpp @@ -0,0 +1,67 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +namespace beluga { + +namespace { + +struct UUT + : public ciabatta::mixin::template mixin> { + using ciabatta::mixin::template mixin>::mixin; +}; + +struct ResampleIntervalPolicyTests : public ::testing::Test {}; + +TEST_F(ResampleIntervalPolicyTests, Construction) { + // UUT does not blow up during contruction + [[maybe_unused]] UUT uut{ResampleIntervalPolicyParam{}}; +} + +class ResampleIntervalPolicyTestsWithParam : public testing::TestWithParam {}; + +TEST_P(ResampleIntervalPolicyTestsWithParam, ResampleEveryNthIteration) { + // uut allows resampling every N-th iteration, starting on the N-th one + const auto interval = GetParam(); + const auto periods = 10; + + ResampleIntervalPolicyParam config; + config.resample_interval_count = interval; + + UUT uut{config}; + + for (size_t i = 0; i < periods; ++i) { + // don't resample for the first N-1 iterations + for (size_t iteration = 0; iteration < interval - 1; ++iteration) { + ASSERT_FALSE(uut.do_resampling_vote()); + } + // then resample once + ASSERT_TRUE(uut.do_resampling_vote()); + } +} + +INSTANTIATE_TEST_SUITE_P( + ResampleEveryNthIterationInstance, + ResampleIntervalPolicyTestsWithParam, + testing::Values(3, 5, 10, 21)); + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/resampling_policies/test_resample_on_motion_policy.cpp b/beluga/test/beluga/resampling_policies/test_resample_on_motion_policy.cpp new file mode 100644 index 000000000..2465a784c --- /dev/null +++ b/beluga/test/beluga/resampling_policies/test_resample_on_motion_policy.cpp @@ -0,0 +1,245 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +namespace beluga { + +namespace { + +template +struct MockMotionModel : public Mixin { + using update_type = Sophus::SE2d; + + template + explicit MockMotionModel(Args&&... rest) : Mixin(std::forward(rest)...) {} + + void update_motion(const update_type& pose) { last_pose_ = pose; } + [[nodiscard]] std::optional latest_motion_update() const { return last_pose_; } + + private: + std::optional last_pose_; +}; + +struct UUT : public ciabatta::mixin< + UUT, + MockMotionModel, + ciabatta::curry::template mixin> { + using ciabatta::mixin< + UUT, + MockMotionModel, + ciabatta::curry::template mixin>::mixin; +}; + +struct ResampleOnMotionPolicyTests : public ::testing::Test { + auto make_motion_update_event(double x, double y, double phi) { + using Eigen::Vector2d; + using Sophus::SE2d; + using Sophus::SO2d; + return Sophus::SE2d{SO2d{phi}, Vector2d{x, y}}; + }; + + const double d_threshold = 0.4; + const double a_threshold = 0.2; +}; + +TEST_F(ResampleOnMotionPolicyTests, Construction) { + // tests uut does not blow up during contruction + [[maybe_unused]] UUT uut{ResampleOnMotionPolicyParam{}}; +} + +TEST_F(ResampleOnMotionPolicyTests, MotionUpdatesEnableResampling) { + // uut allows sampling only when motion values are above parameter thresholds + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // first motion update, still not enough data, will default to allowing sampling + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // second motion update, enough data, but we are not moving + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // still no motion + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // still no motion (thresholds are calculated independently for each axis) + uut.update_motion(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // back to origin + uut.update_motion(make_motion_update_event(0, 0, 0)); + + // motion above threshold for axis x + uut.update_motion(make_motion_update_event(d_threshold * 1.01, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // motion above threshold for axis y + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // motion above threshold for angular motion + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 1.01)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // no relative motion again + uut.update_motion(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 1.01)); + ASSERT_FALSE(uut.do_resampling_vote()); +} + +TEST_F(ResampleOnMotionPolicyTests, ThresholdsAreRelativeToLatestResamplingPose) { + // uut measures threshold from the latest pose where the policy agreed to do resampling + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // first motion update, still not enough data, will default to allowing sampling + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // second motion update, enough data, but we are not moving + uut.update_motion(make_motion_update_event(0, 0, 0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + const auto pivot_distance = d_threshold * 10.0; + const auto lower_bound = pivot_distance - 0.95 * d_threshold; + const auto upper_bound = pivot_distance + 0.95 * d_threshold; + + // move to 10x the threshold, forcing resampling to take place + uut.update_motion(make_motion_update_event(pivot_distance, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); + + // move to the lower bound, then forward to the upper bound, then back to the lower bound and finally back to the + // pivot. Despite two of the steps being almost 2x the length of the threshold, we are always at a distance of less + // than a threshold from the pivot where we last resampled, so none of the steps should cause additoinal resamples + // value, because we never get farther away from the pivot than the treshold value. + uut.update_motion(make_motion_update_event(pivot_distance, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(lower_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(upper_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(lower_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + uut.update_motion(make_motion_update_event(upper_bound, 0.0, 0.0)); + ASSERT_FALSE(uut.do_resampling_vote()); + + // resample when we move a full threshold from the pivot + uut.update_motion(make_motion_update_event(pivot_distance + d_threshold * 1.01, 0.0, 0.0)); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +TEST_F(ResampleOnMotionPolicyTests, RandomWalkingTest) { + // simulate making the uut go on a random walk, with some steps above threshold, and others below, + // to make sure it does not only work with the predictable poses (in particular rotations), that + // other more systematic tests above use + ResampleOnMotionPolicyParam params{}; + params.update_min_d = d_threshold; + params.update_min_a = a_threshold; + + UUT uut{params}; + + // when there's no motion data, policy should default to true + ASSERT_TRUE(uut.do_resampling_vote()); + + // list of pairs of relative motion from the previous pose, and the expected + // "do_resampling" value for that step + const auto relative_motions = { + std::make_tuple(make_motion_update_event(0, 0, 0), false), + // relative motions along x axis + std::make_tuple(make_motion_update_event(d_threshold * 0.99, 0.0, 0.0), false), + std::make_tuple(make_motion_update_event(d_threshold * 0.02, 0.0, 0.0), true), + std::make_tuple(make_motion_update_event(d_threshold * -0.99, 0.0, 0.0), false), + std::make_tuple(make_motion_update_event(d_threshold * -0.02, 0.0, 0.0), true), + // relative motions along y axis + std::make_tuple(make_motion_update_event(0.0, d_threshold * 0.99, 0.0), false), + std::make_tuple(make_motion_update_event(0.0, d_threshold * 0.02, 0.0), true), + std::make_tuple(make_motion_update_event(0.0, d_threshold * -0.99, 0.0), false), + std::make_tuple(make_motion_update_event(0.0, d_threshold * -0.02, 0.0), true), + // relative motions along theta axis + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * 0.99), false), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * 0.02), true), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * -0.99), false), + std::make_tuple(make_motion_update_event(0.0, 0.0, a_threshold * -0.02), true), + // random walk move and stop 1 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.40), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.10, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 2 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.30, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.50, d_threshold * 1.01, a_threshold * 0.30), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.70, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 3 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.70), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.30, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.01, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 4 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.80, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.50, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 5 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.30, a_threshold * 0.10), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.50, d_threshold * 1.01, a_threshold * 0.80), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.20, d_threshold * 0.40, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + // random walk move and stop 6 + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 0.99, a_threshold * 0.99), true), + std::make_tuple(make_motion_update_event(d_threshold * 1.01, d_threshold * 1.01, a_threshold * 0.50), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.40, d_threshold * 0.70, a_threshold * 1.01), true), + std::make_tuple(make_motion_update_event(d_threshold * 0.99, d_threshold * 0.99, a_threshold * 0.99), false), + }; + + auto current_pose = make_motion_update_event(0, 0, 0); + + // warm-up the internal state of the policy + uut.update_motion(current_pose); + ASSERT_TRUE(uut.do_resampling_vote()); + uut.update_motion(current_pose); + ASSERT_FALSE(uut.do_resampling_vote()); + + for (const auto& test_tuple : relative_motions) { + const auto relative_movement = std::get<0>(test_tuple); + const auto expected_resampling_flag = std::get<1>(test_tuple); + // apply the relative motion on top of the current pose and test the result + current_pose = current_pose * relative_movement; + uut.update_motion(current_pose); + ASSERT_EQ(expected_resampling_flag, uut.do_resampling_vote()); + } +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/resampling_policies/test_resampling_policies_poller.cpp b/beluga/test/beluga/resampling_policies/test_resampling_policies_poller.cpp new file mode 100644 index 000000000..a902e04c6 --- /dev/null +++ b/beluga/test/beluga/resampling_policies/test_resampling_policies_poller.cpp @@ -0,0 +1,152 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include + +#include + +namespace beluga { + +class ProxyVoterInterface { + public: + using Ptr = std::shared_ptr; + virtual ~ProxyVoterInterface() = default; + virtual bool current_vote_value() const = 0; +}; + +class MockDrivenProxyVoter : public ProxyVoterInterface { + public: + MOCK_METHOD(bool, current_vote_value, (), (const, override)); +}; + +struct ProxyVoterPolicyParam { + ProxyVoterInterface::Ptr proxy; +}; + +struct ProxyVoterPolicy { + using param_type = ProxyVoterPolicyParam; + + template + explicit ProxyVoterPolicy(ProxyVoterPolicyParam config) : config_{std::move(config)} {} + + template + [[nodiscard]] bool do_resampling([[maybe_unused]] Concrete& filter) { + return config_.proxy->current_vote_value(); + } + + private: + ProxyVoterPolicyParam config_; +}; + +namespace { + +struct UUT_WITH_POLICIES_INSTALLED + : public ciabatta::mixin< + UUT_WITH_POLICIES_INSTALLED, + ciabatta::curry:: + template mixin> { + using ciabatta::mixin< + UUT_WITH_POLICIES_INSTALLED, + ciabatta::curry::template mixin>:: + mixin; +}; + +template +using EventSubscriberCallback = std::function; + +struct ResamplingPoliciesPollerTests : public ::testing::Test {}; + +TEST_F(ResamplingPoliciesPollerTests, ResamplingPollerWithNPoliciesUsesShortCircuitEvaluation) { + using testing::InSequence; + using testing::Return; + + auto voter_1 = std::make_unique(); + auto voter_2 = std::make_unique(); + auto voter_3 = std::make_unique(); + + InSequence sec; + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(false)); + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_2, current_vote_value()).WillOnce(Return(false)); + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_2, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_3, current_vote_value()).WillOnce(Return(false)); + + // vote: true + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_2, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_3, current_vote_value()).WillOnce(Return(true)); + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_2, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_3, current_vote_value()).WillOnce(Return(false)); + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(true)); + EXPECT_CALL(*voter_2, current_vote_value()).WillOnce(Return(false)); + + // vote: false + EXPECT_CALL(*voter_1, current_vote_value()).WillOnce(Return(false)); + + UUT_WITH_POLICIES_INSTALLED uut{ + ProxyVoterPolicyParam{std::move(voter_1)}, ProxyVoterPolicyParam{std::move(voter_2)}, + ProxyVoterPolicyParam{std::move(voter_3)}}; + + const auto vote_results = { + false, /* false */ + false, /* true && false */ + false, /* true && true && false */ + true, /* true && true && true */ + false, /* true && true && false */ + false, /* true && false */ + false, /* false */ + }; + + for (const auto& iteration_result : vote_results) { + ASSERT_EQ(iteration_result, uut.do_resampling_vote()); + } +} + +TEST_F(ResamplingPoliciesPollerTests, ResamplingVoteWithNoPolicies) { + // Tests that if no policies have been registered, the vote is by default in favour of resampling + + struct UUT_WITH_NO_POLICIES_INSTALLED + : public ciabatta::mixin< + UUT_WITH_NO_POLICIES_INSTALLED, ciabatta::curry::template mixin> { + using ciabatta::mixin::template mixin>:: + mixin; + }; + + UUT_WITH_NO_POLICIES_INSTALLED uut{}; + + ASSERT_TRUE(uut.do_resampling_vote()); + ASSERT_TRUE(uut.do_resampling_vote()); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +} // namespace + +} // namespace beluga diff --git a/beluga/test/beluga/resampling_policies/test_selective_resampling_policy.cpp b/beluga/test/beluga/resampling_policies/test_selective_resampling_policy.cpp new file mode 100644 index 000000000..049d49b23 --- /dev/null +++ b/beluga/test/beluga/resampling_policies/test_selective_resampling_policy.cpp @@ -0,0 +1,109 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace beluga { + +namespace { + +template +struct MockMixin : public Mixin { + public: + std::vector weights_; + + using view_t = decltype(ranges::views::all(weights_)); + + [[nodiscard]] auto weights() const { return ranges::views::all(weights_) | ranges::views::const_; } + + void set_weights(std::vector v) { weights_ = std::move(v); } +}; + +struct UUT + : public ciabatta:: + mixin::template mixin, MockMixin> { + using ciabatta::mixin< + UUT, + ciabatta::curry::template mixin, + MockMixin>::mixin; +}; + +struct SelectiveResamplingPolicyTests : public ::testing::Test {}; + +TEST_F(SelectiveResamplingPolicyTests, Construction) { + // UUT does not blow up during contruction + [[maybe_unused]] UUT uut{SelectiveResamplingPolicyParam{}}; +} + +TEST_F(SelectiveResamplingPolicyTests, SelectiveResamplingOn) { + // uut allows resampling every N-th iteration, starting on the N-th one + + SelectiveResamplingPolicyParam config; + config.enabled = true; + UUT uut{config}; + + // very low n_eff, do resampling + uut.set_weights({1.0, 1.0, 1.0, 1.0, 1.0}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // very high n_eff, don't do resampling + uut.set_weights({0.1, 0.1, 0.1, 0.1, 0.1}); + ASSERT_FALSE(uut.do_resampling_vote()); + + // n_eff slightly below N/2, do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.30}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly above N/2, don't do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.25}); + ASSERT_FALSE(uut.do_resampling_vote()); +} + +TEST_F(SelectiveResamplingPolicyTests, SelectiveResamplingOff) { + // uut allows resampling every N-th iteration, starting on the N-th one + + SelectiveResamplingPolicyParam config; + config.enabled = false; + UUT uut{config}; + + // very low n_eff, do resampling + uut.set_weights({1.0, 1.0, 1.0, 1.0, 1.0}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // very high n_eff, don't do resampling + uut.set_weights({0.1, 0.1, 0.1, 0.1, 0.1}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly below N/2, do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.30}); + ASSERT_TRUE(uut.do_resampling_vote()); + + // n_eff slightly above N/2, don't do resampling + uut.set_weights({0.2828, 0.2828, 0.2828, 0.2828, 0.25}); + ASSERT_TRUE(uut.do_resampling_vote()); +} + +} // namespace + +} // namespace beluga diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index e548f027f..26f1fc8e4 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -163,15 +163,25 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = - "Number of filter updates required before resampling. " - "Values other than 1 are not supported in this implementation."; + "Number of filter updates required before resampling. "; descriptor.integer_range.resize(1); descriptor.integer_range[0].from_value = 1; - descriptor.integer_range[0].to_value = 1; + descriptor.integer_range[0].to_value = std::numeric_limits::max(); descriptor.integer_range[0].step = 1; declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "When set to true, will reduce the resampling rate when not needed and help " + "avoid particle deprivation. The resampling will only happen if the effective " + "number of particles (N_eff = 1/(sum(k_i^2))) is lower than half the current " + "number of particles."; + descriptor.read_only = true; + declare_parameter("selective_resampling", false, descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = @@ -579,6 +589,14 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) resampling_params.kld_epsilon = get_parameter("pf_err").as_double(); resampling_params.kld_z = get_parameter("pf_z").as_double(); + auto resample_on_motion_params = beluga::ResampleOnMotionPolicyParam{}; + resample_on_motion_params.update_min_d = get_parameter("update_min_d").as_double(); + resample_on_motion_params.update_min_a = get_parameter("update_min_a").as_double(); + + auto resample_interval_params = beluga::ResampleIntervalPolicyParam{}; + resample_interval_params.resample_interval_count = + static_cast(get_parameter("resample_interval").as_int()); + auto sensor_params = beluga::LikelihoodFieldModelParam{}; sensor_params.max_obstacle_distance = get_parameter("laser_likelihood_max_dist").as_double(); sensor_params.max_laser_distance = get_parameter("laser_max_range").as_double(); @@ -592,6 +610,9 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) motion_params.translation_noise_from_translation = get_parameter("alpha3").as_double(); motion_params.translation_noise_from_rotation = get_parameter("alpha4").as_double(); + auto selective_resampling_params = beluga::SelectiveResamplingPolicyParam{}; + selective_resampling_params.enabled = get_parameter("selective_resampling").as_bool(); + // Only when we get the first map we should use the parameters, not later. // TODO(ivanpauno): Intialize later maps from last known pose. const bool initialize_from_params = !particle_filter_ && @@ -600,6 +621,9 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) particle_filter_ = std::make_unique( generation_params, resampling_params, + resample_on_motion_params, + resample_interval_params, + selective_resampling_params, motion_params, sensor_params, OccupancyGrid{map}); @@ -722,20 +746,9 @@ void AmclNode::laser_callback( static_cast(get_parameter("laser_max_range").as_double()))); particle_filter_->importance_sample(exec_policy); const auto time3 = std::chrono::high_resolution_clock::now(); - { - const auto delta = odom_to_base_transform * last_odom_to_base_transform_.inverse(); - const bool has_moved_since_last_resample = - std::abs(delta.translation().x()) > get_parameter("update_min_d").as_double() || - std::abs(delta.translation().y()) > get_parameter("update_min_d").as_double() || - std::abs(delta.so2().log()) > get_parameter("update_min_a").as_double(); - if (has_moved_since_last_resample) { - // To avoid loss of diversity in the particle population, don't - // resample when the state is known to be static. - // See 'Probabilistic Robotics, Chapter 4.2.4'. - particle_filter_->resample(); - last_odom_to_base_transform_ = odom_to_base_transform; - } - } + + particle_filter_->resample(); + const auto time4 = std::chrono::high_resolution_clock::now(); RCLCPP_INFO_THROTTLE(