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

@@ -15,13 +15,18 @@
 #ifndef BELUGA_MOTION_HPP
 #define BELUGA_MOTION_HPP
 
+#include <beluga/motion/ackerman_drive_model.hpp>
 #include <beluga/motion/differential_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 (AckermannDriveModel)
  */
 
 /**
@@ -61,6 +66,7 @@
  * - beluga::DifferentialDriveModel
  * - beluga::OmnidirectionalDriveModel
  * - beluga::StationaryModel
+ * - beluga::AckermannDriveModel
  */
 
 #endif

beluga/include/beluga/motion/ackerman_drive_model.hpp

@@ -0,0 +1,293 @@
+// 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_ACKERMAN_DRIVE_MODEL_HPP
+#define BELUGA_MOTION_ACKERMAN_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)
+  double phi;  ///< Steering angle (rad)
+};
+
+/// Parameters to construct a AckermannDriveModel instance.
+/**
+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3, particularly table 5.3.
+ */
+struct AckermannDriveModelParam {
+  /// 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;
+
+  /// Threshold for distinguishing between straight-line and circular motion.
+  /**
+   * Below this threshold (~0.57 degrees), motion is treated as straight-line to avoid
+   * numerical instabilities in radius calculations for nearly-zero angular velocities.
+   */
+  static constexpr double small_angle_threshold = 0.01;
+
+  /// Length of the robot (meters).
+  /**
+   * See \cite Localization and Mapping in Local Occupancy Grid Maps: Simulation
+   * in Ackerman model mobile robot by Ronald A. Cardenas , Jasper W. Huanay
+   * and Ivan Calle
+   */
+  double wheelbase;
+};
+
+/// 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 AckermannDriveModel {
+  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 = AckermannDriveModelParam;
+
+  /// Constructs a AckermannDriveModel instance.
+  /**
+   * \param params Parameters to configure this instance.
+   *  See beluga::AckermannDriveModelParam for details.
+   */
+  explicit AckermannDriveModel(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;
+
+    const auto time = timestamped.timestamp;
+    const auto previous_time = previous_timestamped.timestamp;
+    const auto delta_time = std::chrono::duration<double>(time - previous_time);
+    return sampling_fn_2d(pose, previous_pose, delta_time);
+  }
+
+ private:
+  using control_type_2d = std::tuple<Sophus::SE2d, Sophus::SE2d>;
+
+  [[nodiscard]] auto sampling_fn_2d(
+      const Sophus::SE2d& pose,
+      const Sophus::SE2d& previous_pose,
+      std::chrono::duration<double> delta_time) const {
+    // Calculate velocities from poses
+    const auto velocity = calculate_velocities(pose, previous_pose, delta_time);
+
+    // Velocity noise parameters (following velocity motion model from Probabilistic Robotics)
+    // Use temporary distributions to safely extract param_type objects
+    const auto linear_velocity_distribution = std::normal_distribution<double>{
+        velocity.v, std::sqrt(
+                        params_.translation_noise_from_translation * velocity.v * velocity.v +
+                        params_.translation_noise_from_rotation * velocity.w * velocity.w)};
+    const auto linear_velocity_params = linear_velocity_distribution.param();
+
+    const auto steering_angle_distribution = std::normal_distribution<double>{
+        velocity.phi, std::sqrt(
+                          params_.rotation_noise_from_translation * velocity.v * velocity.v +
+                          params_.rotation_noise_from_rotation * velocity.phi * velocity.phi)};
+    const auto steering_angle_params = steering_angle_distribution.param();
+
+    // TODO: check if velocity.w is necessary to calculate gamma_params
+    // Additional orientation noise (gamma_hat) using rotation parameters
+    const auto gamma_distribution = std::normal_distribution<double>{
+        0.0,  // zero mean
+        std::sqrt(
+            params_.orientation_noise_from_translation * velocity.v * velocity.v +
+            params_.orientation_noise_from_rotation * velocity.w * velocity.w)};
+    const auto gamma_params = gamma_distribution.param();
+
+    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 phi_hat = distribution(gen, steering_angle_params);
+      const auto gamma_hat = distribution(gen, gamma_params);
+
+      const auto omega_hat = v_hat * std::tan(phi_hat) / params_.wheelbase;
+      // 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,
+      std::chrono::duration<double> delta_time) const {
+    const double delta_t_sec = delta_time.count();
+
+    // 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_t_sec;
+
+    // Determine direction sign (forward/backward motion)
+    const auto relative_transform = previous_pose.inverse() * pose;
+    const double dx = relative_transform.translation().x();
+    const double sign = (dx >= 0.0) ? 1.0 : -1.0;
+
+    // Linear velocity calculation
+    double linear_velocity = 0.0;
+
+    if (std::abs(angle_change) > params_.small_angle_threshold) {
+      // 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_t_sec;
+
+    } else {
+      // Straight line motion: v = distance / time
+      linear_velocity = sign * chord_distance / delta_t_sec;
+    }
+
+    double steering_angle = 0.0;
+
+    if (std::abs(linear_velocity) > params_.small_angle_threshold) {
+      const double ratio = params_.wheelbase * angular_velocity / linear_velocity;
+      // TODO: use Taylor series for small angles to avoid atan?
+      steering_angle = std::atan(ratio);
+    }
+
+    return Velocity{linear_velocity, angular_velocity, steering_angle};
+  }
+  /// 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,
+      std::chrono::duration<double> delta_time) const {
+    const double delta_t_sec = delta_time.count();
+
+    const auto current_theta = state.so2().log();
+
+    Sophus::SE2d new_pose;
+
+    if (std::abs(omega_hat) < params_.small_angle_threshold) {
+      // Nearly straight line motion
+      const auto translation =
+          Eigen::Vector2d{v_hat * delta_t_sec * std::cos(current_theta), v_hat * delta_t_sec * std::sin(current_theta)};
+      const auto new_theta = current_theta + gamma_hat * delta_t_sec;
+      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_t_sec);
+      const auto dy = (v_hat / omega_hat) * std::cos(current_theta) -
+                      (v_hat / omega_hat) * std::cos(current_theta + omega_hat * delta_t_sec);
+      const auto translation = Eigen::Vector2d{dx, dy};
+      const auto new_theta = current_theta + omega_hat * delta_t_sec + gamma_hat * delta_t_sec;
+      new_pose = Sophus::SE2d{Sophus::SO2d{new_theta}, state.translation() + translation};
+    }
+
+    return new_pose;
+  }
+
+  param_type params_;
+};
+
+/// Alias for a 2D Ackerman drive model, for convenience.
+using AckermannDriveModel2d = AckermannDriveModel<Sophus::SE2d>;
+}  // 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;
 
@@ -100,7 +101,9 @@ class OmnidirectionalDriveModel {
    */
   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& [pose_stamped, previous_pose_stamped] = action;
+    const state_type& pose = pose_stamped;
+    const state_type& previous_pose = previous_pose_stamped;
 
     const auto translation = pose.translation() - previous_pose.translation();
     const double distance = translation.norm();

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.