diff --git a/beluga/include/beluga/motion/differential_drive_model.hpp b/beluga/include/beluga/motion/differential_drive_model.hpp index 5793d1de3..3731cf719 100644 --- a/beluga/include/beluga/motion/differential_drive_model.hpp +++ b/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::pi()}; + static const auto kFlippingRotation = Sophus::SO2d{Sophus::Constants::pi()}; + const auto flipped_rotation = rotation * kFlippingRotation; const auto delta = std::min(std::abs(rotation.log()), std::abs(flipped_rotation.log())); return delta * delta; } diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/beluga/test/beluga/motion/test_differential_drive_model.cpp index e3e145391..76df6a2e4 100644 --- a/beluga/test/beluga/motion/test_differential_drive_model.cpp +++ b/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