Add differential motion model

beluga/include/beluga/motion.h

@@ -14,4 +14,5 @@
 
 #pragma once
 
+#include <beluga/motion/differential_drive_model.h>
 #include <beluga/motion/stationary_model.h>

beluga/include/beluga/motion/differential_drive_model.h

@@ -0,0 +1,105 @@
+// Copyright 2022 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.
+
+#pragma once
+
+#include <optional>
+#include <random>
+#include <shared_mutex>
+
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+
+namespace beluga {
+
+struct DifferentialDriveModelParam {
+  double rotation_noise_from_rotation;        // alpha1
+  double rotation_noise_from_translation;     // alpha2
+  double translation_noise_from_translation;  // alpha3
+  double translation_noise_from_rotation;     // alpha4
+  double distance_threshold = 0.01;           // distance threshold to detect in-place rotation
+};
+
+template <class Mixin>
+class DifferentialDriveModel : public Mixin {
+ public:
+  using DistributionParam = typename std::normal_distribution<double>::param_type;
+
+  template <class... Args>
+  explicit DifferentialDriveModel(const DifferentialDriveModelParam& params, Args&&... args)
+      : Mixin(std::forward<Args>(args)...), params_{params} {}
+
+  [[nodiscard]] Sophus::SE2d apply_motion(const Sophus::SE2d& 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_};
+    const auto first_rotation = Sophus::SO2d{distribution(generator, first_rotation_params_)};
+    const auto translation = Eigen::Vector2d{distribution(generator, translation_params_), 0.0};
+    const auto second_rotation = Sophus::SO2d{distribution(generator, second_rotation_params_)};
+    return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} * Sophus::SE2d{second_rotation, translation};
+  }
+
+  void update_motion(const Sophus::SE2d& 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{0.0};
+      const auto second_rotation = current_orientation * previous_orientation.inverse() * first_rotation.inverse();
+      const auto combined_rotation = first_rotation * second_rotation;
+
+      {
+        const auto lock = std::lock_guard<std::shared_mutex>{params_mutex_};
+        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(combined_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;
+  }
+
+ private:
+  DifferentialDriveModelParam params_;
+  std::optional<Sophus::SE2d> 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};
+  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

beluga/test/beluga/CMakeLists.txt

@@ -5,6 +5,7 @@ ament_add_gmock(test_beluga
   algorithm/test_exponential_filter.cpp
   algorithm/test_particle_filter.cpp
   algorithm/test_sampling.cpp
+  motion/test_differential_drive_model.cpp
   sensor/test_likelihood_field_model.cpp
   test_tuple_vector.cpp
   test_spatial_hash.cpp)

beluga/test/beluga/motion/test_differential_drive_model.cpp

@@ -0,0 +1,157 @@
+// Copyright 2022 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/differential_drive_model.h>
+#include <ciabatta/ciabatta.h>
+#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 DifferentialDriveModelTest : public ::testing::Test {
+ protected:
+  MockMixin<beluga::DifferentialDriveModel> mixin_{
+      beluga::DifferentialDriveModelParam{0.0, 0.0, 0.0, 0.0}};  // No variance
+};
+
+TEST_F(DifferentialDriveModelTest, 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(DifferentialDriveModelTest, 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(DifferentialDriveModelTest, 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(DifferentialDriveModelTest, 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(DifferentialDriveModelTest, 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(DifferentialDriveModelTest, RotateTranslateRotate) {
+  mixin_.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin_.update_motion(SE2d{SO2d{-Constants::pi() / 2}, Vector2d{1.0, 2.0}});
+  const auto result = mixin_.apply_motion(SE2d{SO2d{Constants::pi()}, Vector2d{3.0, 4.0}});
+  ASSERT_THAT(result, testing::SE2Eq(SO2d{Constants::pi() / 2}, Vector2d{2.0, 2.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(DifferentialDriveModelSamples, Translate) {
+  const double alpha = 0.2;
+  const double origin = 5.0;
+  const double distance = 3.0;
+  auto mixin = MockMixin<beluga::DifferentialDriveModel>{
+      beluga::DifferentialDriveModelParam{0.0, 0.0, alpha, 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(DifferentialDriveModelSamples, RotateFirstQuadrant) {
+  const double alpha = 0.2;
+  const double initial_angle = Constants::pi() / 6;
+  const double motion_angle = Constants::pi() / 4;
+  auto mixin = MockMixin<beluga::DifferentialDriveModel>{
+      beluga::DifferentialDriveModelParam{alpha, 0.0, 0.0, 0.0}};  // Rotation variance
+  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(DifferentialDriveModelSamples, 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::DifferentialDriveModel>{
+      beluga::DifferentialDriveModelParam{alpha, 0.0, 0.0, 0.0}};  // Rotation variance
+  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

beluga/test/utils/sophus_matchers.hpp

@@ -0,0 +1,48 @@
+// Copyright 2022 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.
+
+#pragma once
+
+#include <gmock/gmock.h>
+
+#include <sophus/se2.hpp>
+#include <sophus/so2.hpp>
+
+namespace testing {
+
+template <class Scalar>
+inline auto Vector2Eq(const Eigen::Vector2<Scalar>& expected) {
+  return AllOf(
+      Property("x", &Eigen::Vector2<Scalar>::x, DoubleNear(expected.x(), 0.001)),
+      Property("y", &Eigen::Vector2<Scalar>::y, DoubleNear(expected.y(), 0.001)));
+}
+
+template <class Scalar>
+inline auto SO2Eq(const Sophus::SO2<Scalar>& expected) {
+  return Property("unit_complex", &Sophus::SO2<Scalar>::unit_complex, Vector2Eq<Scalar>(expected.unit_complex()));
+}
+
+template <class Scalar>
+inline auto SE2Eq(const Sophus::SE2<Scalar>& expected) {
+  return AllOf(
+      Property("translation", &Sophus::SE2<Scalar>::translation, Vector2Eq<Scalar>(expected.translation())),
+      Property("so2", &Sophus::SE2<Scalar>::so2, SO2Eq<Scalar>(expected.so2())));
+}
+
+template <class Scalar>
+inline auto SE2Eq(const Sophus::SO2<Scalar>& rotation, const Eigen::Vector2<Scalar>& translation) {
+  return SE2Eq<Scalar>(Sophus::SE2<Scalar>{rotation, translation});
+}
+
+}  // namespace testing