Add omnidirectional motion model (#120)

Closes #111.

This patch adds an omnidirectional drive motion model with its corresponding
tests. The implementation is based on the model described in #56.

Signed-off-by: Olmer Garcia-Bedoya <[email protected]>

beluga/include/beluga/motion/omnidirectional_drive_model.hpp

@@ -0,0 +1,184 @@
+// 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_MOTION_OMNIDIRECTIONAL_DRIVE_MODEL_HPP
+#define BELUGA_MOTION_OMNIDIRECTIONAL_DRIVE_MODEL_HPP
+
+#include <optional>
+#include <random>
+#include <shared_mutex>
+
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+
+/**
+ * \file
+ * \brief Implementation of an omnidirectional drive odometry motion model.
+ */
+
+namespace beluga {
+
+/// Parameters to construct an OmnidirectionalDriveModel instance.
+struct OmnidirectionalDriveModelParam {
+  /// Rotational noise from rotation
+  /**
+   * How much rotational noise is generated by the relative rotation between the last two odometry updates.
+   * Also known as `alpha1`.
+   */
+  double rotation_noise_from_rotation;
+  /// Rotational noise from translation
+  /**
+   * How much rotational noise is generated by the relative translation between the last two odometry updates.
+   * Also known as `alpha2`.
+   */
+  double rotation_noise_from_translation;
+  /// Translational noise from translation
+  /**
+   * How much translational in longitudinal noise is generated by the relative translation between
+   * the last two odometry updates.
+   * Also known as `alpha3`.
+   */
+  double translation_noise_from_translation;
+  /// Translational noise from rotation
+  /**
+   * How much translational noise is generated by the relative rotation between the last two odometry updates.
+   * Also known as `alpha4`.
+   */
+  double translation_noise_from_rotation;
+  /// Translational strafe noise from translation
+  /**
+   * How much translational noise in strafe is generated by the relative translation between the last two
+   * odometry updates.
+   * Also known as `alpha5`.
+   */
+  double translation_strafe_noise_from_translation;
+
+  /// Distance threshold to detect in-place rotation.
+  double distance_threshold = 0.01;
+};
+
+/// Sampled odometry model for an omnidirectional drive.
+/**
+ * \tparam Mixin The mixed-in type.
+ */
+template <class Mixin>
+class OmnidirectionalDriveModel : public Mixin {
+ 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.
+  using state_type = Sophus::SE2d;
+
+  /// Parameter type that the constructor uses to configure the motion model.
+  using param_type = OmnidirectionalDriveModelParam;
+
+  /// Constructs an omnidirectionalDriveModel instance.
+  /**
+   * \tparam ...Args Arguments types for the remaining mixin constructors.
+   * \param params Parameters to configure this instance.
+   *  See beluga::OmnidirectionalDriveModelParam for details.
+   * \param ...args arguments that are not used by this part of the mixin, but by others.
+   */
+  template <class... Args>
+  explicit OmnidirectionalDriveModel(const param_type& params, Args&&... args)
+      : Mixin(std::forward<Args>(args)...), params_{params} {}
+
+  /// Applies the last update to the particle state given.
+  /**
+   * \param state The particle state to apply the motion to.
+   * \return The updated particle state.
+   */
+  [[nodiscard]] state_type apply_motion(const state_type& state) const {
+    static thread_local auto generator = std::mt19937{std::random_device()()};
+    static thread_local auto distribution = std::normal_distribution<double>{};
+    const auto lock = std::shared_lock<std::shared_mutex>{params_mutex_};
+    // This is an implementation based on the same set of parameters that is used in
+    // nav2's omni_motion_model. To simplify the implementation, the following
+    // variable substitutions were performed:
+    // - first_rotation = delta_bearing - previous_orientation
+    // - second_rotation = delta_rot_hat - first_rotation
+    const auto second_rotation = Sophus::SO2d{distribution(generator, rotation_params_)} * first_rotation_.inverse();
+    const auto translation = Eigen::Vector2d{
+        distribution(generator, translation_params_),
+        -1.0 * distribution(generator, strafe_params_),
+    };
+    return state * Sophus::SE2d{first_rotation_, Eigen::Vector2d{0.0, 0.0}} *
+           Sophus::SE2d{second_rotation, translation};
+  }
+
+  /// Updates the motion model.
+  /**
+   * This will not update particles.
+   * That is done by the particle filter using the apply_motion() method
+   * provided by this class.
+   *
+   * \param pose Last odometry udpate.
+   */
+  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 rotation = current_orientation * previous_orientation.inverse();
+
+      {
+        const auto lock = std::lock_guard<std::shared_mutex>{params_mutex_};
+        first_rotation_ =
+            distance > params_.distance_threshold
+                ? Sophus::SO2d{std::atan2(translation.y(), translation.x())} * previous_orientation.inverse()
+                : Sophus::SO2d{0.0};
+        const double rotation_variance_ = rotation_variance(rotation);
+        rotation_params_ = DistributionParam{
+            rotation.log(), std::sqrt(
+                                params_.rotation_noise_from_rotation * rotation_variance_ +
+                                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_)};
+        strafe_params_ = DistributionParam{
+            0.0, std::sqrt(
+                     params_.translation_strafe_noise_from_translation * distance_variance +
+                     params_.translation_noise_from_rotation * rotation_variance_)};
+      }
+    }
+    last_pose_ = pose;
+  }
+
+ private:
+  using DistributionParam = typename std::normal_distribution<double>::param_type;
+
+  OmnidirectionalDriveModelParam params_;
+  std::optional<Sophus::SE2d> last_pose_;
+
+  DistributionParam rotation_params_{0.0, 0.0};
+  DistributionParam strafe_params_{0.0, 0.0};
+  DistributionParam translation_params_{0.0, 0.0};
+  Sophus::SO2d first_rotation_;
+  mutable std::shared_mutex params_mutex_;
+
+  static double rotation_variance(const Sophus::SO2d& rotation) {
+    // Treat backward and forward motion symmetrically for the noise models.
+    const auto flipped_rotation = rotation * Sophus::SO2d{Sophus::Constants<double>::pi()};
+    const auto delta = std::min(std::abs(rotation.log()), std::abs(flipped_rotation.log()));
+    return delta * delta;
+  }
+};
+
+}  // namespace beluga
+
+#endif

beluga/test/beluga/CMakeLists.txt

@@ -7,6 +7,7 @@ ament_add_gmock(test_beluga
   algorithm/test_particle_filter.cpp
   algorithm/test_sampling.cpp
   motion/test_differential_drive_model.cpp
+  motion/test_omnidirectional_drive_model.cpp
   random/test_multivariate_normal_distribution.cpp
   resampling_policies/test_resample_interval_policy.cpp
   resampling_policies/test_resample_on_motion_policy.cpp

beluga/test/beluga/motion/test_omnidirectional_drive_model.cpp

@@ -0,0 +1,157 @@
+// 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.
+
+#include <gmock/gmock.h>
+
+#include <beluga/motion/omnidirectional_drive_model.hpp>
+#include <ciabatta/ciabatta.hpp>
+#include <range/v3/view/common.hpp>
+#include <range/v3/view/generate.hpp>
+#include <range/v3/view/take_exactly.hpp>
+
+#include "../../utils/sophus_matchers.hpp"
+
+namespace {
+
+using Constants = Sophus::Constants<double>;
+using Eigen::Vector2d;
+using Sophus::SE2d;
+using Sophus::SO2d;
+
+template <template <class> class Mixin>
+class MockMixin : public ciabatta::mixin<MockMixin<Mixin>, Mixin> {
+ public:
+  using ciabatta::mixin<MockMixin<Mixin>, Mixin>::mixin;
+};
+
+class OmnidirectionalDriveModelTest : public ::testing::Test {
+ protected:
+  MockMixin<beluga::OmnidirectionalDriveModel> mixin_{
+      beluga::OmnidirectionalDriveModelParam{0.0, 0.0, 0.0, 0.0, 0.0}};  // No variance
+};
+
+TEST_F(OmnidirectionalDriveModelTest, NoUpdate) {
+  const auto pose = SE2d{SO2d{Constants::pi() / 3}, Vector2d{2.0, 5.0}};
+  ASSERT_THAT(mixin_.apply_motion(pose), testing::SE2Eq(pose));
+}
+
+TEST_F(OmnidirectionalDriveModelTest, OneUpdate) {
+  const auto pose = SE2d{SO2d{Constants::pi() / 3}, Vector2d{2.0, 5.0}};
+  mixin_.update_motion(SE2d{SO2d{Constants::pi()}, Vector2d{1.0, -2.0}});
+  ASSERT_THAT(mixin_.apply_motion(pose), testing::SE2Eq(pose));
+}
+
+TEST_F(OmnidirectionalDriveModelTest, Translate) {
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{1.0, 0.0}});
+  const auto result1 = mixin_.apply_motion(SE2d{SO2d{0.0}, Vector2d{2.0, 0.0}});
+  ASSERT_THAT(result1, testing::SE2Eq(SO2d{0.0}, Vector2d{3.0, 0.0}));
+  const auto result2 = mixin_.apply_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 3.0}});
+  ASSERT_THAT(result2, testing::SE2Eq(SO2d{0.0}, Vector2d{1.0, 3.0}));
+}
+
+TEST_F(OmnidirectionalDriveModelTest, RotateTranslate) {
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin_.update_motion(SE2d{SO2d{Constants::pi() / 2}, Vector2d{0.0, 1.0}});
+  const auto result1 = mixin_.apply_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  ASSERT_THAT(result1, testing::SE2Eq(SO2d{Constants::pi() / 2}, Vector2d{0.0, 1.0}));
+  const auto result2 = mixin_.apply_motion(SE2d{SO2d{-Constants::pi() / 2}, Vector2d{2.0, 3.0}});
+  ASSERT_THAT(result2, testing::SE2Eq(SO2d{0.0}, Vector2d{3.0, 3.0}));
+}
+
+TEST_F(OmnidirectionalDriveModelTest, Rotate) {
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin_.update_motion(SE2d{SO2d{Constants::pi() / 4}, Vector2d{0.0, 0.0}});
+  const auto result1 = mixin_.apply_motion(SE2d{SO2d{Constants::pi()}, Vector2d{0.0, 0.0}});
+  ASSERT_THAT(result1, testing::SE2Eq(SO2d{Constants::pi() * 5 / 4}, Vector2d{0.0, 0.0}));
+  const auto result2 = mixin_.apply_motion(SE2d{SO2d{-Constants::pi() / 2}, Vector2d{0.0, 0.0}});
+  ASSERT_THAT(result2, testing::SE2Eq(SO2d{-Constants::pi() / 4}, Vector2d{0.0, 0.0}));
+}
+
+TEST_F(OmnidirectionalDriveModelTest, Translate_Strafe) {
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 1.0}});
+  const auto result1 = mixin_.apply_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  ASSERT_THAT(result1, testing::SE2Eq(SO2d{0.0}, Vector2d{0.0, 1.0}));
+}
+
+template <class Range>
+auto get_statistics(Range&& range) {
+  const double size = static_cast<double>(std::distance(std::begin(range), std::end(range)));
+  const double sum = std::accumulate(std::begin(range), std::end(range), 0.0);
+  const double mean = sum / size;
+  const double squared_diff_sum =
+      std::transform_reduce(std::begin(range), std::end(range), 0.0, std::plus<>{}, [mean](double value) {
+        const double diff = value - mean;
+        return diff * diff;
+      });
+  const double stddev = std::sqrt(squared_diff_sum / size);
+  return std::pair{mean, stddev};
+}
+
+TEST(OmnidirectionalDriveModelSamples, Translate) {
+  const double alpha = 0.2;
+  const double origin = 5.0;
+  const double distance = 3.0;
+  auto mixin = MockMixin<beluga::OmnidirectionalDriveModel>{
+      beluga::OmnidirectionalDriveModelParam{0.0, 0.0, alpha, 0.0, 0.0}};  // Translation variance
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{distance, 0.0}});
+  auto view = ranges::view::generate([&mixin, origin]() {
+                return mixin.apply_motion(SE2d{SO2d{0.0}, Vector2d{origin, 0.0}}).translation().x();
+              }) |
+              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+  const auto [mean, stddev] = get_statistics(view);
+  ASSERT_NEAR(mean, origin + distance, 0.01);
+  ASSERT_NEAR(stddev, std::sqrt(alpha * distance * distance), 0.01);
+}
+
+TEST(OmnidirectionalDriveModelSamples, RotateFirstQuadrant) {
+  const double alpha = 0.2;
+  const double initial_angle = Constants::pi() / 6;
+  const double motion_angle = Constants::pi() / 4;
+  auto mixin =
+      MockMixin<beluga::OmnidirectionalDriveModel>{beluga::OmnidirectionalDriveModelParam{alpha, 0.0, 0.0, 0.0, 0.0}};
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin.update_motion(SE2d{SO2d{motion_angle}, Vector2d{0.0, 0.0}});
+  auto view = ranges::view::generate([&mixin, initial_angle]() {
+                return mixin.apply_motion(SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}).so2().log();
+              }) |
+              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+  const auto [mean, stddev] = get_statistics(view);
+  ASSERT_NEAR(mean, initial_angle + motion_angle, 0.01);
+  ASSERT_NEAR(stddev, std::sqrt(alpha * motion_angle * motion_angle), 0.01);
+}
+
+TEST(OmnidirectionalDriveModelSamples, RotateThirdQuadrant) {
+  const double alpha = 0.2;
+  const double initial_angle = Constants::pi() / 6;
+  const double motion_angle = -Constants::pi() * 3 / 4;
+  auto mixin =
+      MockMixin<beluga::OmnidirectionalDriveModel>{beluga::OmnidirectionalDriveModelParam{alpha, 0.0, 0.0, 0.0, 0.0}};
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin.update_motion(SE2d{SO2d{motion_angle}, Vector2d{0.0, 0.0}});
+  auto view = ranges::view::generate([&mixin, initial_angle]() {
+                return mixin.apply_motion(SE2d{SO2d{initial_angle}, Vector2d{0.0, 0.0}}).so2().log();
+              }) |
+              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+  const auto [mean, stddev] = get_statistics(view);
+  ASSERT_NEAR(mean, initial_angle + motion_angle, 0.01);
+
+  // Treat backward and forward motion symmetrically for the noise models.
+  const double flipped_angle = Constants::pi() + motion_angle;
+  ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01);
+}
+
+}  // namespace