Migrate motion models to functional form (#321)

### Proposed changes

This patch migrates motion models to a functional form. Combined with
#320, this affords:

```c++
beluga::actions::propagate(std::execution::par, motion(control_action_window << odom))
```

when propagating particles. 

#### Type of change

- [ ] 🐛 Bugfix (change which fixes an issue)
- [x] 🚀 Feature (change which adds functionality)
- [ ] 📚 Documentation (change which fixes or extends documentation)

### Checklist

- [x] Lint and unit tests (if any) pass locally with my changes
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have added necessary documentation (if appropriate)
- [x] All commits have been signed for
[DCO](https://developercertificate.org/)

---------

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

beluga/include/beluga/actions/propagate.hpp

@@ -17,11 +17,13 @@
 
 #include <algorithm>
 #include <execution>
+#include <type_traits>
 
 #include <beluga/type_traits/particle_traits.hpp>
 #include <beluga/views/particles.hpp>
 
 #include <range/v3/action/action.hpp>
+#include <range/v3/utility/random.hpp>
 #include <range/v3/view/common.hpp>
 
 namespace beluga::actions {
@@ -34,47 +36,61 @@ struct propagate_base_fn {
   /**
    * \tparam ExecutionPolicy An [execution policy](https://en.cppreference.com/w/cpp/algorithm/execution_policy_tag_t).
    * \tparam Range An [input range](https://en.cppreference.com/w/cpp/ranges/input_range) of particles.
-   * \tparam Model A callable that can compute the new state from the previous state.
+   * \tparam StateSamplingFunction A callable that samples a posterior state given a prior state. Callables satisfying
+   * \ref StateSamplingFunctionPage are also supported. This will be bound to the default random number generator for
+   * ranges.
    * \param policy The execution policy to use.
    * \param range An existing range of particles to apply this action to.
-   * \param model A callable instance to compute the states from the previous states.
+   * \param fn A state sampling function instance.
    */
   template <
       class ExecutionPolicy,
       class Range,
-      class Model,
+      class StateSamplingFunction,
       std::enable_if_t<std::is_execution_policy_v<std::decay_t<ExecutionPolicy>>, int> = 0,
       std::enable_if_t<ranges::range<Range>, int> = 0>
-  constexpr auto operator()(ExecutionPolicy&& policy, Range& range, Model model) const -> Range& {
+  constexpr auto operator()(ExecutionPolicy&& policy, Range& range, StateSamplingFunction fn) const -> Range& {
     static_assert(beluga::is_particle_range_v<Range>);
     auto states = range | beluga::views::states | ranges::views::common;
+
+    auto unary_fn = [&]() {
+      using States = decltype(states);
+      using State = ranges::range_value_t<States>;
+      using Generator = decltype(ranges::detail::get_random_engine());
+      if constexpr (std::is_invocable_v<StateSamplingFunction, State, Generator>) {
+        return [fn = std::move(fn)](const State& state) { return fn(state, ranges::detail::get_random_engine()); };
+      } else {
+        return std::move(fn);
+      }
+    }();
+
     std::transform(
         policy,              // rvalue policies are not supported in some STL implementations
         std::begin(states),  //
         std::end(states),    //
         std::begin(states),  //
-        std::move(model));
+        std::move(unary_fn));
     return range;
   }
 
   /// Overload that re-orders arguments from a view closure.
   template <
       class Range,
-      class Model,
+      class StateSamplingFunction,
       class ExecutionPolicy,
       std::enable_if_t<ranges::range<Range>, int> = 0,
       std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0>
-  constexpr auto operator()(Range&& range, Model model, ExecutionPolicy policy) const -> Range& {
-    return (*this)(std::move(policy), std::forward<Range>(range), std::move(model));
+  constexpr auto operator()(Range&& range, StateSamplingFunction fn, ExecutionPolicy policy) const -> Range& {
+    return (*this)(std::move(policy), std::forward<Range>(range), std::move(fn));
   }
 
   /// Overload that returns a view closure to compose with other views.
   template <
-      class ExecutionPolicy,  //
-      class Model,            //
+      class ExecutionPolicy,        //
+      class StateSamplingFunction,  //
       std::enable_if_t<std::is_execution_policy_v<ExecutionPolicy>, int> = 0>
-  constexpr auto operator()(ExecutionPolicy policy, Model model) const {
-    return ranges::make_action_closure(ranges::bind_back(propagate_base_fn{}, std::move(model), std::move(policy)));
+  constexpr auto operator()(ExecutionPolicy policy, StateSamplingFunction fn) const {
+    return ranges::make_action_closure(ranges::bind_back(propagate_base_fn{}, std::move(fn), std::move(policy)));
   }
 };
 
@@ -84,27 +100,28 @@ struct propagate_fn : public propagate_base_fn {
 
   /// Overload that defines a default execution policy.
   template <
-      class Range,  //
-      class Model,  //
+      class Range,                  //
+      class StateSamplingFunction,  //
       std::enable_if_t<ranges::range<Range>, int> = 0>
-  constexpr auto operator()(Range&& range, Model model) const -> Range& {
-    return (*this)(std::execution::seq, std::forward<Range>(range), std::move(model));
+  constexpr auto operator()(Range&& range, StateSamplingFunction fn) const -> Range& {
+    return (*this)(std::execution::seq, std::forward<Range>(range), std::move(fn));
   }
 
   /// Overload that returns a view closure to compose with other views.
-  template <class Model>
-  constexpr auto operator()(Model model) const {
-    return ranges::make_action_closure(ranges::bind_back(propagate_fn{}, std::move(model)));
+  template <class StateSamplingFunction>
+  constexpr auto operator()(StateSamplingFunction fn) const {
+    return ranges::make_action_closure(ranges::bind_back(propagate_fn{}, std::move(fn)));
   }
 };
 
 }  // namespace detail
 
 /// [Range adaptor object](https://en.cppreference.com/w/cpp/named_req/RangeAdaptorObject) that
-/// can update the state in a particle range using a motion model.
+/// can update the state in a particle range using a state transition (or sampling) function.
 /**
- * This action updates particle states based on their current value and a state-transition function (or motion model).
- * Every other particle attribute (such as importance sampling weights) is left unchanged.
+ * This action updates particle states based on their current value and a state transition
+ * (or sampling) function. Every other particle attribute (such as importance sampling weights)
+ * is left unchanged.
  */
 inline constexpr detail::propagate_fn propagate;
 

beluga/include/beluga/localization.hpp

@@ -16,6 +16,7 @@
 #define BELUGA_MIXIN_LOCALIZATION_HPP
 
 #include <beluga/algorithm/estimation.hpp>
+#include <beluga/containers/circular_array.hpp>
 #include <beluga/mixin.hpp>
 #include <beluga/motion.hpp>
 #include <beluga/sensor.hpp>
@@ -95,14 +96,32 @@ using LaserLocalizationInterface2d = beluga::compose_interfaces<
 
 /// Wrapper to convert a motion model class into a mixin.
 template <typename Mixin, typename Model>
-struct MotionMixin : public Mixin, public Model {
-  using update_type = typename Model::update_type;
+class MotionMixin : public Mixin {
+ public:
+  using update_type = std::tuple_element_t<0, typename Model::control_type>;
+  using state_type = typename Model::state_type;
 
   template <typename... Args>
   constexpr explicit MotionMixin(Model model, Args&&... args)
-      : Model(std::move(model)), Mixin(static_cast<decltype(args)>(args)...) {}
-
-  void update_motion(const update_type& pose) { Model::update_motion(pose); }
+      : Mixin(static_cast<decltype(args)>(args)...), model_(std::move(model)) {}
+
+  template <class Generator>
+  [[nodiscard]] state_type apply_motion(const state_type& state, Generator& gen) {
+    if (state_sampling_function_.has_value()) {
+      return (*state_sampling_function_)(state, gen);
+    }
+    return state;
+  }
+
+  void update_motion(const update_type& pose) { state_sampling_function_.emplace(model_(control_window_ << pose)); }
+
+ private:
+  Model model_;
+  static constexpr auto kWindowSize = std::tuple_size_v<typename Model::control_type>;
+  RollingWindow<update_type, kWindowSize> control_window_;
+  using StateSamplingFunction =
+      decltype(std::declval<Model>()(std::declval<RollingWindow<update_type, kWindowSize>&>()));
+  std::optional<StateSamplingFunction> state_sampling_function_;
 };
 
 /// Wrapper to convert a sensor model class into a mixin.

beluga/include/beluga/motion.hpp

@@ -31,20 +31,31 @@
  * \section MotionModelRequirements Requirements
  * A type `T` satisfies the `MotionModel` requirements if the following is satisfied:
  * - `T::state_type` is a valid type, representing a particle state.
- * - `T::update_type` is a valid type, representing a motion model update.
+ * - `T::control_type` is a valid type, representing a control action
+ *    to condition the motion model.
  *
  * Given:
- * - An instance `p` of `T`.
  * - A possibly const instance `cp` of `T`.
- * - A possibly const instance of `T::update_type` `u`.
- * - A possibly const instance of `T::state_type` `s`.
+ * - A possibly const instance `u` of `T::control_type` (or convertible type `U`).
  *
  * Then:
- * - `p.update_motion(u)` will update the motion model with `u`.
- *   This will not actually update the particle states, but the update done here
- *   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(u)` returns a callable satisfying \ref StateSamplingFunctionPage for `T::state_type`.
+ *
+ * \section StateSamplingFunctionPage Beluga named requirements: StateSamplingFunction
+ * Requirements for a callable to sample a motion distribution when propagating
+ * particle states in a Beluga `ParticleFilter`.
+ *
+ * \section StateSamplingFunctionRequirements Requirements
+ * A type `F` satisfies the `StateSamplingFunction` requirements for some state type `S` if:
+ *
+ * Given:
+ * - A possibly const instance `fn` of `F`.
+ * - A possible const instance `s` of `S`.
+ * - An instance `e` satisfying [URNG](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator)
+ * requirements.
+ *
+ * Then:
+ * - `fn(s, e)` returns another sampled state `ss` of `S`.
  *
  * \section MotionModelLinks See also
  * - beluga::DifferentialDriveModel
@@ -54,7 +65,7 @@
 
 namespace beluga {
 
-/// Pure abstract class representing the odometry motion model interface.
+/// Pure abstract class representing the odometry motion model mixin interface.
 struct OdometryMotionModelInterface2d {
   /// Update type of the motion model.
   using update_type = Sophus::SE2d;

beluga/include/beluga/motion/differential_drive_model.hpp

@@ -17,6 +17,10 @@
 
 #include <optional>
 #include <random>
+#include <tuple>
+#include <type_traits>
+
+#include <beluga/type_traits/tuple_traits.hpp>
 
 #include <sophus/se2.hpp>
 #include <sophus/so2.hpp>
@@ -64,15 +68,15 @@ struct DifferentialDriveModelParam {
 
 /// Sampled odometry model for a differential drive.
 /**
- * This class implements OdometryMotionModelInterface2d and satisfies \ref MotionModelPage.
+ * This class satisfies \ref MotionModelPage.
  *
  * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.4.2.
  */
 class DifferentialDriveModel {
  public:
-  /// Update type of the motion model, same as the state_type in the odometry model.
-  using update_type = Sophus::SE2d;
-  /// State type of a particle.
+  /// Current and previous odometry estimates as motion model control action.
+  using control_type = std::tuple<Sophus::SE2d, Sophus::SE2d>;
+  /// 2D pose as motion model state (to match that of the particles).
   using state_type = Sophus::SE2d;
 
   /// Parameter type that the constructor uses to configure the motion model.
@@ -83,69 +87,56 @@ class DifferentialDriveModel {
    * \param params Parameters to configure this instance.
    *  See beluga::DifferentialDriveModelParam for details.
    */
-  template <class... Args>
   explicit DifferentialDriveModel(const param_type& params) : params_{params} {}
 
-  /// Applies the last motion update to the given particle state.
+  /// Computes a state sampling function conditioned on a given control action.
   /**
-   * \tparam Generator  A random number generator that must satisfy the
-   *  [UniformRandomBitGenerator](https://en.cppreference.com/w/cpp/named_req/UniformRandomBitGenerator)
-   *  requirements.
-   * \param state The state of the particle to which the motion will be applied.
-   * \param gen An uniform random bit generator object.
+   * \tparam Control A tuple-like container matching the model's `control_type`.
+   * \param action Control action to condition the motion model with.
+   * \return a callable satisfying \ref StateSamplingFunctionPage.
    */
-  template <class Generator>
-  [[nodiscard]] state_type apply_motion(const state_type& state, Generator& gen) const {
-    static thread_local auto distribution = std::normal_distribution<double>{};
-    const auto first_rotation = Sophus::SO2d{distribution(gen, first_rotation_params_)};
-    const auto translation = Eigen::Vector2d{distribution(gen, translation_params_), 0.0};
-    const auto second_rotation = Sophus::SO2d{distribution(gen, second_rotation_params_)};
-    return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} * Sophus::SE2d{second_rotation, translation};
-  }
-
-  /// \copydoc OdometryMotionModelInterface2d::update_motion(const Sophus::SE2d&)
-  void update_motion(const update_type& pose) {
-    if (last_pose_) {
-      const auto translation = pose.translation() - last_pose_.value().translation();
-      const double distance = translation.norm();
-      const double distance_variance = distance * distance;
-
-      const auto& previous_orientation = last_pose_.value().so2();
-      const auto& current_orientation = pose.so2();
-      const auto first_rotation =
-          distance > params_.distance_threshold
-              ? Sophus::SO2d{std::atan2(translation.y(), translation.x())} * previous_orientation.inverse()
-              : Sophus::SO2d{};
-      const auto second_rotation = current_orientation * previous_orientation.inverse() * first_rotation.inverse();
-
-      {
-        first_rotation_params_ = DistributionParam{
-            first_rotation.log(), std::sqrt(
-                                      params_.rotation_noise_from_rotation * rotation_variance(first_rotation) +
-                                      params_.rotation_noise_from_translation * distance_variance)};
-        translation_params_ = DistributionParam{
-            distance, std::sqrt(
-                          params_.translation_noise_from_translation * distance_variance +
-                          params_.translation_noise_from_rotation *
-                              (rotation_variance(first_rotation) + rotation_variance(second_rotation)))};
-        second_rotation_params_ = DistributionParam{
-            second_rotation.log(), std::sqrt(
-                                       params_.rotation_noise_from_rotation * rotation_variance(second_rotation) +
-                                       params_.rotation_noise_from_translation * distance_variance)};
-      }
-    }
-    last_pose_ = pose;
+  template <class Control, typename = common_tuple_type_t<Control, control_type>>
+  [[nodiscard]] auto operator()(Control&& action) const {
+    const auto& [pose, previous_pose] = action;
+
+    const auto translation = pose.translation() - previous_pose.translation();
+    const double distance = translation.norm();
+    const double distance_variance = distance * distance;
+
+    const auto& previous_orientation = previous_pose.so2();
+    const auto& current_orientation = pose.so2();
+    const auto heading_rotation = Sophus::SO2d{std::atan2(translation.y(), translation.x())};
+    const auto first_rotation =
+        distance > params_.distance_threshold ? heading_rotation * previous_orientation.inverse() : Sophus::SO2d{};
+    const auto second_rotation = current_orientation * previous_orientation.inverse() * first_rotation.inverse();
+
+    using DistributionParam = typename std::normal_distribution<double>::param_type;
+    const auto first_rotation_params = DistributionParam{
+        first_rotation.log(), std::sqrt(
+                                  params_.rotation_noise_from_rotation * rotation_variance(first_rotation) +
+                                  params_.rotation_noise_from_translation * distance_variance)};
+    const auto translation_params = DistributionParam{
+        distance, std::sqrt(
+                      params_.translation_noise_from_translation * distance_variance +
+                      params_.translation_noise_from_rotation *
+                          (rotation_variance(first_rotation) + rotation_variance(second_rotation)))};
+    const auto second_rotation_params = DistributionParam{
+        second_rotation.log(), std::sqrt(
+                                   params_.rotation_noise_from_rotation * rotation_variance(second_rotation) +
+                                   params_.rotation_noise_from_translation * distance_variance)};
+
+    return [=](const state_type& state, auto& gen) {
+      static thread_local auto distribution = std::normal_distribution<double>{};
+      const auto first_rotation = Sophus::SO2d{distribution(gen, first_rotation_params)};
+      const auto translation = Eigen::Vector2d{distribution(gen, translation_params), 0.0};
+      const auto second_rotation = Sophus::SO2d{distribution(gen, second_rotation_params)};
+      return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} *
+             Sophus::SE2d{second_rotation, translation};
+    };
   }
 
  private:
-  using DistributionParam = typename std::normal_distribution<double>::param_type;
-
-  DifferentialDriveModelParam params_;
-  std::optional<update_type> last_pose_;
-
-  DistributionParam first_rotation_params_{0.0, 0.0};
-  DistributionParam second_rotation_params_{0.0, 0.0};
-  DistributionParam translation_params_{0.0, 0.0};
+  param_type params_;
 
   static double rotation_variance(const Sophus::SO2d& rotation) {
     // Treat backward and forward motion symmetrically for the noise models.