Add new resampling policies

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}
+}

beluga/include/beluga/algorithm/particle_filter.hpp

@@ -18,14 +18,19 @@
 #include <execution>
 #include <utility>
 
-#include <beluga/algorithm/sampling.hpp>
-#include <beluga/tuple_vector.hpp>
-#include <beluga/type_traits.hpp>
 #include <ciabatta/ciabatta.hpp>
 #include <range/v3/algorithm/copy.hpp>
 #include <range/v3/algorithm/transform.hpp>
 #include <range/v3/view/const.hpp>
 
+#include <beluga/algorithm/sampling.hpp>
+#include <beluga/resampling_policies/resample_interval_policy.hpp>
+#include <beluga/resampling_policies/resample_on_motion_policy.hpp>
+#include <beluga/resampling_policies/resampling_policies_poller.hpp>
+#include <beluga/resampling_policies/selective_resampling_policy.hpp>
+#include <beluga/tuple_vector.hpp>
+#include <beluga/type_traits.hpp>
+
 /**
  * \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<BaselineGeneration>::template mixin,
                  ciabatta::curry<NaiveGeneration>::template mixin,
                  ciabatta::curry<FixedResampling>::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<BaselineGeneration>::template mixin,
       ciabatta::curry<NaiveGeneration>::template mixin,
       ciabatta::curry<FixedResampling>::template mixin,
+      ciabatta::
+          curry<ResamplingPoliciesPoller, ResampleOnMotionPolicy, ResampleIntervalPolicy, SelectiveResamplingPolicy>::
+              mixin,
       MotionModel,
       SensorModel>::mixin;
 };
@@ -321,6 +339,11 @@ struct AMCL : public ciabatta::mixin<
                   ciabatta::curry<BaselineGeneration>::template mixin,
                   ciabatta::curry<AdaptiveGeneration>::template mixin,
                   ciabatta::curry<KldResampling>::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<BaselineGeneration>::template mixin,
       ciabatta::curry<AdaptiveGeneration>::template mixin,
       ciabatta::curry<KldResampling>::template mixin,
+      ciabatta::
+          curry<ResamplingPoliciesPoller, ResampleOnMotionPolicy, ResampleIntervalPolicy, SelectiveResamplingPolicy>::
+              template mixin,
       MotionModel,
       SensorModel>::mixin;
 };

beluga/include/beluga/beluga.hpp

@@ -22,6 +22,7 @@
 
 #include <beluga/algorithm.hpp>
 #include <beluga/motion.hpp>
+#include <beluga/resampling_policies.hpp>
 #include <beluga/sensor.hpp>
 #include <beluga/tuple_vector.hpp>
 #include <beluga/type_traits.hpp>

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<update_type> with the latest motion update
+ *   received through motion_update().
  */
 
 #endif

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<update_type> latest_motion_update() const { return last_pose_; }
+
  private:
   using DistributionParam = typename std::normal_distribution<double>::param_type;
 
   DifferentialDriveModelParam params_;
-  std::optional<Sophus::SE2d> last_pose_;
+  std::optional<update_type> last_pose_;
 
   DistributionParam first_rotation_params_{0.0, 0.0};
   DistributionParam second_rotation_params_{0.0, 0.0};

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<update_type> latest_motion_update() const { return std::nullopt; }
 };
 
 }  // namespace beluga

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 <beluga/resampling_policies/resample_interval_policy.hpp>
+#include <beluga/resampling_policies/resample_on_motion_policy.hpp>
+#include <beluga/resampling_policies/resampling_policies_poller.hpp>
+#include <beluga/resampling_policies/selective_resampling_policy.hpp>
+
+/**
+ * \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

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 <optional>
+
+/**
+ * \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 <typename Concrete>
+  [[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

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 <optional>
+
+#include <sophus/se2.hpp>
+
+/**
+ * \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 <typename Concrete>
+  [[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<motion_event> latest_resample_pose_;
+};
+
+}  // namespace beluga
+
+#endif