Fix differential drive motion model (#267)

### Proposed changes

Going over the math a few times, it doesn't seem to me that the rotation
variance computation distributes over rotation composition. Not with
those nonlinear max, min, abs operations in between.

#### Type of change

- [x] 🐛 Bugfix (change which fixes an issue)
- [ ] 🚀 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
- [ ] 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/)

### Additional comments

Not sure how to write a test for this. Would a formal proof (that
`rotation_variance(a) * rotation_variance(b) != rotation_variance(a *
b)`) do?

---------

Signed-off-by: Michel Hidalgo <[email protected]>
Signed-off-by: Nahuel Espinosa <[email protected]>
Co-authored-by: Nahuel Espinosa <[email protected]>

beluga/include/beluga/motion/differential_drive_model.hpp

@@ -121,9 +121,8 @@ class DifferentialDriveModel : public Mixin {
       const auto first_rotation =
           distance > params_.distance_threshold
               ? Sophus::SO2d{std::atan2(translation.y(), translation.x())} * previous_orientation.inverse()
-              : Sophus::SO2d{0.0};
+              : Sophus::SO2d{};
       const auto second_rotation = current_orientation * previous_orientation.inverse() * first_rotation.inverse();
-      const auto combined_rotation = first_rotation * second_rotation;
 
       {
         first_rotation_params_ = DistributionParam{
@@ -133,7 +132,8 @@ class DifferentialDriveModel : public Mixin {
         translation_params_ = DistributionParam{
             distance, std::sqrt(
                           params_.translation_noise_from_translation * distance_variance +
-                          params_.translation_noise_from_rotation * rotation_variance(combined_rotation))};
+                          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) +
@@ -155,7 +155,8 @@ class DifferentialDriveModel : public Mixin {
 
   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()};
+    static const auto kFlippingRotation = Sophus::SO2d{Sophus::Constants<double>::pi()};
+    const auto flipped_rotation = rotation * kFlippingRotation;
     const auto delta = std::min(std::abs(rotation.log()), std::abs(flipped_rotation.log()));
     return delta * delta;
   }

beluga/test/beluga/motion/test_differential_drive_model.cpp

@@ -151,4 +151,48 @@ TEST(DifferentialDriveModelSamples, RotateThirdQuadrant) {
   ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01);
 }
 
+TEST(DifferentialDriveModelSamples, RotateTranslateRotateFirstQuadrant) {
+  const double alpha = 0.2;
+  auto mixin = UUT{beluga::DifferentialDriveModelParam{0.0, 0.0, 0.0, alpha}};  // Translation variance from rotation
+  auto generator = std::mt19937{std::random_device()()};
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{1.0, 1.0}});
+  auto view = ranges::views::generate([&]() {
+                return mixin.apply_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}}, generator).translation().norm();
+              }) |
+              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+  const auto [mean, stddev] = get_statistics(view);
+  ASSERT_NEAR(mean, 1.41, 0.01);
+
+  // Net rotation is zero comparing final and initial poses, but
+  // the model requires a 45 degree counter-clockwise rotation,
+  // a forward translation, and a 45 degree clockwise rotation.
+  const double first_rotation = Constants::pi() / 4;
+  const double second_rotation = first_rotation;
+  const double rotation_variance = (first_rotation * first_rotation) + (second_rotation * second_rotation);
+  ASSERT_NEAR(stddev, std::sqrt(alpha * rotation_variance), 0.01);
+}
+
+TEST(DifferentialDriveModelSamples, RotateTranslateRotateThirdQuadrant) {
+  const double alpha = 0.2;
+  auto mixin = UUT{beluga::DifferentialDriveModelParam{0.0, 0.0, 0.0, alpha}};  // Translation variance from rotation
+  auto generator = std::mt19937{std::random_device()()};
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}});
+  mixin.update_motion(SE2d{SO2d{0.0}, Vector2d{-1.0, -1.0}});
+  auto view = ranges::views::generate([&]() {
+                return mixin.apply_motion(SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}}, generator).translation().norm();
+              }) |
+              ranges::views::take_exactly(1'000'000) | ranges::views::common;
+  const auto [mean, stddev] = get_statistics(view);
+  ASSERT_NEAR(mean, 1.41, 0.01);
+
+  // Net rotation is zero comparing final and initial poses, but
+  // the model requires a 45 degree counter-clockwise rotation,
+  // a backward translation and a 45 degree clockwise rotation.
+  const double first_rotation = Constants::pi() / 4;
+  const double second_rotation = first_rotation;
+  const double rotation_variance = (first_rotation * first_rotation) + (second_rotation * second_rotation);
+  ASSERT_NEAR(stddev, std::sqrt(alpha * rotation_variance), 0.01);
+}
+
 }  // namespace