Integrated velocity motion model

beluga/include/beluga/algorithm/amcl_core.hpp

@@ -87,6 +87,7 @@ class Amcl {
   using measurement_type = typename SensorModel::measurement_type;
   using state_type = typename SensorModel::state_type;
   using map_type = typename SensorModel::map_type;
+  using control_type = TimeStamped<state_type>;
   using spatial_hasher_type = spatial_hash<state_type>;
   using random_state_generator_type = RandomStateGenerator;
   using estimation_type = std::invoke_result_t<beluga::detail::estimate_fn, std::vector<state_type>>;
@@ -162,7 +163,7 @@ class Amcl {
    * \return An optional pair containing the estimated pose and covariance after the update,
    *         or std::nullopt if no update was performed.
    */
-  auto update(state_type control_action, measurement_type measurement) -> std::optional<estimation_type> {
+  auto update(control_type control_action, measurement_type measurement) -> std::optional<estimation_type> {
     if (particles_.empty()) {
       return std::nullopt;
     }
@@ -227,7 +228,7 @@ class Amcl {
 
   random_state_generator_type random_state_generator_;
 
-  beluga::RollingWindow<state_type, 2> control_action_window_;
+  beluga::RollingWindow<control_type, 2> control_action_window_;
 
   bool force_update_{true};
 };

beluga/include/beluga/motion.hpp

@@ -16,12 +16,17 @@
 #define BELUGA_MOTION_HPP
 
 #include <beluga/motion/differential_drive_model.hpp>
+#include <beluga/motion/differential_velocity_drive_model.hpp>
 #include <beluga/motion/omnidirectional_drive_model.hpp>
 #include <beluga/motion/stationary_model.hpp>
 
 /**
  * \file
  * \brief Includes all Beluga motion models.
+ *
+ * Motion models in Beluga include:
+ * - Position-based models: Use pose differences (DifferentialDriveModel, OmnidirectionalDriveModel, StationaryModel)
+ * - Velocity-based models: Use timestamped poses to calculate velocities (DifferentialVelocityDriveModel)
  */
 
 /**
@@ -61,6 +66,7 @@
  * - beluga::DifferentialDriveModel
  * - beluga::OmnidirectionalDriveModel
  * - beluga::StationaryModel
+ * - beluga::DifferentialVelocityDriveModel
  */
 
 #endif

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

@@ -0,0 +1,274 @@
+// Copyright 2022-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_MOTION_DIFFERENTIAL_VELOCITY_DRIVE_MODEL_HPP
+#define BELUGA_MOTION_DIFFERENTIAL_VELOCITY_DRIVE_MODEL_HPP
+
+#include <chrono>
+#include <random>
+#include <sophus/se3.hpp>
+#include <tuple>
+
+#include <beluga/type_traits/tuple_traits.hpp>
+#include <beluga/utility/time_stamped.hpp>
+
+#include <beluga/3d_embedding.hpp>
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+#include <type_traits>
+
+/**
+ * \file
+ * \brief Implementation of a velocity motion model.
+ */
+
+namespace beluga {
+
+/// Velocity components for differential drive motion model.
+struct Velocity {
+  double v;  ///< Linear velocity (m/s)
+  double w;  ///< Angular velocity (rad/s)
+};
+
+/// Parameters to construct a DifferentialVelocityDriveModel instance.
+/**
+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3, particularly table 5.3.
+ */
+struct DifferentialVelocityDriveModelParam {
+  /// Rotational noise from rotational velocity
+  /**
+   * How much rotational noise is generated by the rotational velocity.
+   * Also known as `alpha1 in the differential drive model param`.
+   */
+  double rotation_noise_from_rotation;
+  /// Rotational noise from translation velocity
+  /**
+   * How much rotational noise is generated by the linear velocity.
+   * Also known as `alpha2 in the differential drive model param`.
+   */
+  double rotation_noise_from_translation;
+  /// Translational noise from translation velocity
+  /**
+   * How much translational noise is generated by the linear velocity.
+   * Also known as `alpha3 in the differential drive model param`.
+   */
+  double translation_noise_from_translation;
+  /// Translational noise from rotational velocity
+  /**
+   * How much translational noise is generated by the rotational velocity.
+   * Also known as `alpha4 in the differential drive model param`.
+   */
+  double translation_noise_from_rotation;
+  /// Additional orientation noise from translational velocity
+  /**
+   * How much extra orientation noise is generated by the linear velocity.
+   * Also known as `alpha6`.
+   */
+  double orientation_noise_from_translation;
+  /// Additional orientation noise from rotational velocity
+  /**
+   * How much extra orientation noise is generated by the rotational velocity.
+   * Also known as `alpha7`.
+   */
+  double orientation_noise_from_rotation;
+};
+
+/// Sampled velocity model for a differential drive.
+/**
+ * Supports 2D and (flattened) 3D state types.
+ * This class satisfies \ref MotionModelPage.
+ *
+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3.
+ *
+ * \tparam StateType Type for particle's state. Either Sophus::SE2d or Sophus::SE3d.
+ */
+template <class StateType = Sophus::SE2d>
+class DifferentialVelocityDriveModel {
+  static_assert(
+      std::is_same_v<StateType, Sophus::SE2d> or std::is_same_v<StateType, Sophus::SE3d>,
+      "Velocity model only supports SE2 and SE3 state types.");
+
+ public:
+  /// 2D or flattened 3D pose as motion model state (to match that of the particles).
+  using state_type = StateType;
+
+  /// Time point type for motion model control actions.
+  using timestamped_state_type = TimeStamped<state_type>;
+
+  /// Current and previous pose estimates and time points as motion model control action.
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;
+
+  /// Parameter type that the constructor uses to configure the motion model.
+  using param_type = DifferentialVelocityDriveModelParam;
+
+  /// Constructs a DifferentialVelocityDriveModel instance.
+  /**
+   * \param params Parameters to configure this instance.
+   *  See beluga::DifferentialVelocityDriveModelParam for details.
+   */
+  explicit DifferentialVelocityDriveModel(const param_type& params) : params_{params} {}
+
+  /// Computes a state sampling function conditioned on a given control action.
+  /**
+   * \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 Control, typename = common_tuple_type_t<Control, control_type>>
+  [[nodiscard]] auto operator()(const Control& action) const {
+    const auto& [timestamped, previous_timestamped] = action;
+    const auto& pose = timestamped.value;
+    const auto& previous_pose = previous_timestamped.value;
+
+    auto time = timestamped.timestamp;
+    auto previous_time = previous_timestamped.timestamp;
+    const auto delta_time = std::chrono::duration<double>(time - previous_time).count();
+    if constexpr (std::is_same_v<state_type, Sophus::SE2d>) {
+      return sampling_fn_2d(pose, previous_pose, delta_time);
+    } else {
+      return sampling_fn_3d(pose, previous_pose, delta_time);
+    }
+  }
+
+ private:
+  using control_type_2d = std::tuple<Sophus::SE2d, Sophus::SE2d>;
+  using control_type_3d = std::tuple<Sophus::SE3d, Sophus::SE3d>;
+
+  [[nodiscard]] auto sampling_fn_3d(const Sophus::SE3d& pose, const Sophus::SE3d& previous_pose, double delta_time)
+      const {
+    const auto current_pose_2d = To2d(pose);
+    const auto previous_pose_pose_2d = To2d(previous_pose);
+    const auto two_d_sampling_fn = sampling_fn_2d(current_pose_2d, previous_pose_pose_2d, delta_time);
+    return [=](const state_type& state, auto& gen) { return To3d(two_d_sampling_fn(To2d(state), gen)); };
+  }
+
+  [[nodiscard]] auto sampling_fn_2d(const Sophus::SE2d& pose, const Sophus::SE2d& previous_pose, double delta_time)
+      const {
+    // Calculate velocities from poses
+    const auto velocity = calculate_velocities(pose, previous_pose, delta_time);
+
+    using DistributionParam = typename std::normal_distribution<double>::param_type;
+
+    // Velocity noise parameters (following velocity motion model from Probabilistic Robotics)
+    const auto linear_velocity_params = DistributionParam{
+        velocity.v, std::sqrt(
+                        params_.translation_noise_from_translation * std::abs(velocity.v) +
+                        params_.translation_noise_from_rotation * std::abs(velocity.w))};
+
+    const auto angular_velocity_params = DistributionParam{
+        velocity.w, std::sqrt(
+                        params_.rotation_noise_from_translation * std::abs(velocity.v) +
+                        params_.rotation_noise_from_rotation * std::abs(velocity.w))};
+
+    // Additional orientation noise (gamma_hat) using rotation parameters
+    const auto gamma_params = DistributionParam{
+        0.0,  // zero mean
+        std::sqrt(
+            params_.orientation_noise_from_translation * std::abs(velocity.v) +
+            params_.orientation_noise_from_rotation * std::abs(velocity.w))};
+
+    return [=](const auto& state, auto& gen) {
+      static thread_local auto distribution = std::normal_distribution<double>{};
+
+      // Sample noisy velocities
+      const auto v_hat = distribution(gen, linear_velocity_params);
+      const auto omega_hat = distribution(gen, angular_velocity_params);
+      const auto gamma_hat = distribution(gen, gamma_params);
+
+      // Apply velocity motion model
+      return apply_velocity_motion(state, v_hat, omega_hat, gamma_hat, delta_time);
+    };
+  }
+
+  /// Calculate linear and angular velocities from two poses and delta time
+  Velocity calculate_velocities(const Sophus::SE2d& pose, const Sophus::SE2d& previous_pose, double delta_time) const {
+    // Euclidean distance (chord length between poses)
+    const auto translation = pose.translation() - previous_pose.translation();
+    const double chord_distance = translation.norm();
+
+    // Angular velocity from orientation change
+    const auto angular_change = pose.so2() * previous_pose.so2().inverse();
+    const double angle_change = angular_change.log();
+    const double angular_velocity = angle_change / delta_time;
+
+    // Determine direction sign (forward/backward motion)
+    const auto forward_direction =
+        Eigen::Vector2d{std::cos(previous_pose.so2().log()), std::sin(previous_pose.so2().log())};
+    const double dot_product = translation.dot(forward_direction);
+    const double sign = (dot_product >= 0) ? 1.0 : -1.0;
+
+    // Linear velocity calculation
+    double linear_velocity = 0.0;
+
+    if (std::abs(angle_change) > std::numeric_limits<double>::epsilon()) {
+      // Circular motion: calculate radius from chord and angle
+      // For an arc: chord = 2r·sin(θ/2), therefore r = chord / (2·sin(θ/2))
+      const double radius = chord_distance / (2.0 * std::sin(std::abs(angle_change) / 2.0));
+
+      // Arc length: s = r · θ
+      const double arc_distance = radius * std::abs(angle_change);
+
+      // Linear velocity with direction sign
+      linear_velocity = sign * arc_distance / delta_time;
+
+    } else {
+      // Straight line motion: v = distance / time
+      linear_velocity = sign * chord_distance / delta_time;
+    }
+
+    return Velocity{linear_velocity, angular_velocity};
+  }
+  /// Apply velocity motion model to get new pose
+  Sophus::SE2d apply_velocity_motion(
+      const Sophus::SE2d& state,
+      double v_hat,
+      double omega_hat,
+      double gamma_hat,
+      double delta_time) const {
+    const auto current_theta = state.so2().log();
+
+    Sophus::SE2d new_pose;
+
+    if (std::abs(omega_hat) < std::numeric_limits<double>::epsilon()) {
+      // Nearly straight line motion
+      const auto translation =
+          Eigen::Vector2d{v_hat * delta_time * std::cos(current_theta), v_hat * delta_time * std::sin(current_theta)};
+      const auto new_theta = current_theta + gamma_hat * delta_time;
+      new_pose = Sophus::SE2d{Sophus::SO2d{new_theta}, state.translation() + translation};
+    } else {
+      // Circular motion (following velocity motion model equations)
+      const auto dx = -(v_hat / omega_hat) * std::sin(current_theta) +
+                      (v_hat / omega_hat) * std::sin(current_theta + omega_hat * delta_time);
+      const auto dy = (v_hat / omega_hat) * std::cos(current_theta) -
+                      (v_hat / omega_hat) * std::cos(current_theta + omega_hat * delta_time);
+      const auto translation = Eigen::Vector2d{dx, dy};
+      const auto new_theta = current_theta + omega_hat * delta_time + gamma_hat * delta_time;
+      new_pose = Sophus::SE2d{Sophus::SO2d{new_theta}, state.translation() + translation};
+    }
+
+    return new_pose;
+  }
+
+  param_type params_;
+};
+
+/// Alias for a 2D velocity drive model, for convenience.
+using VelocityDriveModel2d = DifferentialVelocityDriveModel<Sophus::SE2d>;
+
+/// Alias for a 3D velocity drive model, for convenience.
+using VelocityDriveModel3d = DifferentialVelocityDriveModel<Sophus::SE3d>;
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/motion/omnidirectional_drive_model.hpp

@@ -77,11 +77,12 @@ struct OmnidirectionalDriveModelParam {
  */
 class OmnidirectionalDriveModel {
  public:
-  /// 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;
 
+  /// Current and previous odometry estimates as motion model control action.
+  using control_type = std::tuple<state_type, state_type>;
+
   /// Parameter type that the constructor uses to configure the motion model.
   using param_type = OmnidirectionalDriveModelParam;
 
@@ -102,12 +103,12 @@ class OmnidirectionalDriveModel {
   [[nodiscard]] auto operator()(Control&& action) const {
     const auto& [pose, previous_pose] = action;
 
-    const auto translation = pose.translation() - previous_pose.translation();
+    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& previous_orientation = previous_pose->so2();
+    const auto& current_orientation = pose->so2();
     const auto rotation = current_orientation * previous_orientation.inverse();
 
     const auto heading_rotation = Sophus::SO2d{std::atan2(translation.y(), translation.x())};

beluga/include/beluga/motion/stationary_model.hpp

@@ -38,11 +38,10 @@ namespace beluga {
  */
 class StationaryModel {
  public:
-  /// 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;
-
+  /// Current and previous odometry estimates as motion model control action.
+  using control_type = std::tuple<state_type, state_type>;
   /// Computes a state sampling function conditioned on a given control action.
   /**
    * The updated state will be centered around `state` with some covariance.