Add new resampling policies

Signed-off-by: Gerardo Puga <[email protected]>

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/resampling_rate_policies/resample_interval_policy.hpp>
+#include <beluga/algorithm/resampling_rate_policies/resample_on_motion_policy.hpp>
+#include <beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp>
+#include <beluga/algorithm/sampling.hpp>
+#include <beluga/runtime_dispatch/runtime_dispatch.hpp>
+#include <beluga/tuple_vector.hpp>
+#include <beluga/type_traits.hpp>
+
 /**
  * \file
  * \brief Implementation of particle filters.
@@ -236,7 +241,10 @@ struct BootstrapParticleFilter : public Mixin {
    * \ref ParticleResamplingPage "ParticleResampling" named requirements.
    */
   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 +293,10 @@ struct MCL : public ciabatta::mixin<
                  ciabatta::curry<BaselineGeneration>::template mixin,
                  ciabatta::curry<NaiveGeneration>::template mixin,
                  ciabatta::curry<FixedResampling>::template mixin,
+                 ciabatta::curry<ResampleOnMotionPolicy>::template mixin,
+                 ciabatta::curry<ResampleIntervalPolicy>::template mixin,
+                 ciabatta::curry<SelectiveResamplingPolicy>::template mixin,
+                 ciabatta::curry<RuntimeDispatch>::template mixin,
                  MotionModel,
                  SensorModel> {
   using ciabatta::mixin<
@@ -293,6 +305,10 @@ struct MCL : public ciabatta::mixin<
       ciabatta::curry<BaselineGeneration>::template mixin,
       ciabatta::curry<NaiveGeneration>::template mixin,
       ciabatta::curry<FixedResampling>::template mixin,
+      ciabatta::curry<ResampleOnMotionPolicy>::template mixin,
+      ciabatta::curry<ResampleIntervalPolicy>::template mixin,
+      ciabatta::curry<SelectiveResamplingPolicy>::template mixin,
+      ciabatta::curry<RuntimeDispatch>::template mixin,
       MotionModel,
       SensorModel>::mixin;
 };
@@ -321,6 +337,10 @@ struct AMCL : public ciabatta::mixin<
                   ciabatta::curry<BaselineGeneration>::template mixin,
                   ciabatta::curry<AdaptiveGeneration>::template mixin,
                   ciabatta::curry<KldResampling>::template mixin,
+                  ciabatta::curry<ResampleOnMotionPolicy>::template mixin,
+                  ciabatta::curry<ResampleIntervalPolicy>::template mixin,
+                  ciabatta::curry<SelectiveResamplingPolicy>::template mixin,
+                  ciabatta::curry<RuntimeDispatch>::template mixin,
                   MotionModel,
                   SensorModel> {
   using ciabatta::mixin<
@@ -329,6 +349,10 @@ struct AMCL : public ciabatta::mixin<
       ciabatta::curry<BaselineGeneration>::template mixin,
       ciabatta::curry<AdaptiveGeneration>::template mixin,
       ciabatta::curry<KldResampling>::template mixin,
+      ciabatta::curry<ResampleOnMotionPolicy>::template mixin,
+      ciabatta::curry<ResampleIntervalPolicy>::template mixin,
+      ciabatta::curry<SelectiveResamplingPolicy>::template mixin,
+      ciabatta::curry<RuntimeDispatch>::template mixin,
       MotionModel,
       SensorModel>::mixin;
 };

beluga/include/beluga/algorithm/resampling_rate_policies/resample_interval_policy.hpp

@@ -0,0 +1,54 @@
+// 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_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP
+#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP
+
+#include <optional>
+#include <utility>
+
+#include <ciabatta/ciabatta.hpp>
+
+namespace beluga {
+
+struct ResampleIntervalPolicyParam {
+  std::size_t resample_interval_count{1};
+};
+
+template <class Mixin>
+struct ResampleIntervalPolicy : public Mixin {
+ public:
+  using param_type = ResampleIntervalPolicyParam;
+
+  template <class... Args>
+  explicit ResampleIntervalPolicy(const param_type& configuration, Args&&... rest)
+      : Mixin(std::forward<Args>(rest)...), configuration_{configuration} {
+    if (configuration_.resample_interval_count > 1) {
+      this->self().register_sampling_voter([this]() { return do_resampling(); });
+    }
+  }
+
+ private:
+  param_type configuration_;
+  std::size_t filter_update_counter_{0};
+
+  [[nodiscard]] bool do_resampling() {
+    filter_update_counter_ = (filter_update_counter_ + 1) % configuration_.resample_interval_count;
+    return (filter_update_counter_ == 0);
+  }
+};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/algorithm/resampling_rate_policies/resample_on_motion_policy.hpp

@@ -0,0 +1,78 @@
+// 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_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP
+#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP
+
+#include <optional>
+#include <utility>
+
+#include <ciabatta/ciabatta.hpp>
+#include <sophus/se2.hpp>
+
+namespace beluga {
+
+struct ResampleOnMotionPolicyParam {
+  double update_min_d{0.};
+  double update_min_a{0.};
+};
+
+template <class Mixin>
+struct ResampleOnMotionPolicy : public Mixin {
+ public:
+  using param_type = ResampleOnMotionPolicyParam;
+  using motion_event = Sophus::SE2d;
+
+  template <class... Args>
+  explicit ResampleOnMotionPolicy(const param_type& configuration, Args&&... rest)
+      : Mixin(std::forward<Args>(rest)...), configuration_{configuration} {
+    this->self().register_sampling_voter([this]() { return do_resampling(); });
+  }
+
+ private:
+  param_type configuration_;
+  std::optional<motion_event> latest_resample_pose_;
+
+  [[nodiscard]] bool do_resampling() {
+    // 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'.
+    auto current_pose = this->self().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 travelled since the last time we did resample
+    if (must_do_resample) {
+      latest_resample_pose_ = current_pose;
+    }
+
+    return must_do_resample;
+  }
+};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/algorithm/resampling_rate_policies/selective_resampling_policy.hpp

@@ -0,0 +1,59 @@
+// 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_ALGORITHM_RESAMPLING_RATE_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP
+#define BELUGA_ALGORITHM_RESAMPLING_RATE_POLICIES_SELECTIVE_RESAMPLING_POLICY_HPP
+
+#include <utility>
+
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/view/transform.hpp>
+
+#include <ciabatta/ciabatta.hpp>
+
+namespace beluga {
+
+struct SelectiveResamplingPolicyParam {
+  bool selective_resampling{false};
+};
+
+template <class Mixin>
+struct SelectiveResamplingPolicy : public Mixin {
+ public:
+  using param_type = SelectiveResamplingPolicyParam;
+
+  template <class... Args>
+  explicit SelectiveResamplingPolicy(const param_type& configuration, Args&&... rest)
+      : Mixin(std::forward<Args>(rest)...), configuration_{configuration} {
+    // only enable this if it's configured to be used
+    if (configuration.selective_resampling) {
+      this->self().register_sampling_voter([this]() { return do_resampling(); });
+    }
+  }
+
+ private:
+  param_type configuration_;
+
+  [[nodiscard]] bool do_resampling() const {
+    const auto n_eff =
+        1. /
+        ranges::accumulate(this->self().weights() | ranges::views::transform([](const auto w) { return w * w; }), 0.);
+    const auto n = static_cast<double>(std::size(this->self().weights()));
+    return n_eff < n / 2.;
+  }
+};
+
+}  // namespace beluga
+
+#endif

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/runtime_dispatch/runtime_dispatch.hpp

@@ -0,0 +1,50 @@
+// 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_RUNTIME_DISPATCH_RUNTIME_DISPATCH_HPP
+#define BELUGA_RUNTIME_DISPATCH_RUNTIME_DISPATCH_HPP
+
+#include <functional>
+#include <vector>
+
+#include <ciabatta/ciabatta.hpp>
+
+namespace beluga {
+
+template <class Mixin>
+struct RuntimeDispatch : public Mixin {
+ public:
+  using resample_voter_function = std::function<bool()>;
+
+  template <class... Args>
+  explicit RuntimeDispatch(Args&&... rest) : Mixin(std::forward<Args>(rest)...) {}
+
+  [[nodiscard]] bool do_resampling_vote() {
+    // resampling voters are in cascade: if any of them votes "no", no resampling is done
+    return std::all_of(voters_.begin(), voters_.end(), [](const auto& predicate) { return predicate(); });
+  }
+
+  // do not split this function from the "protected" access clause. The method needs to be protected to
+  // prevent code that tries call register_sampling_voter() before this mixin's constructor
+  // has been executed from compiling.
+ protected:
+  void register_sampling_voter(resample_voter_function predicate) { voters_.push_back(std::move(predicate)); }
+
+ private:
+  std::vector<resample_voter_function> voters_;
+};
+
+}  // namespace beluga
+
+#endif

beluga/test/CMakeLists.txt

@@ -1,2 +1,3 @@
 add_subdirectory(benchmark)
 add_subdirectory(beluga)
+add_subdirectory(build_failure_tests)

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_rate_policies/test_resample_interval_policy.cpp
+  resampling_rate_policies/test_resample_on_motion_policy.cpp
+  resampling_rate_policies/test_selective_resampling_policy.cpp
+  runtime_dispatch/test_runtime_dispatch.cpp
   sensor/test_likelihood_field_model.cpp
   test_tuple_vector.cpp
   test_spatial_hash.cpp)