diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index ca550cdc7..88976697a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -172,6 +172,35 @@ TEST(Pose2, expmap_c_full) EXPECT(assert_equal(xi, Pose2::Logmap(expected),1e-6)); } +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(Pose2, Adjoint_full) { + Pose2 T(1, 2, 3); + Pose2 expected = T * Pose2::Expmap(screwPose2::xi) * T.inverse(); + Vector xiprime = T.Adjoint(screwPose2::xi); + EXPECT(assert_equal(expected, Pose2::Expmap(xiprime), 1e-6)); + + Vector3 xi2(4, 5, 6); + Pose2 expected2 = T * Pose2::Expmap(xi2) * T.inverse(); + Vector xiprime2 = T.Adjoint(xi2); + EXPECT(assert_equal(expected2, Pose2::Expmap(xiprime2), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(Pose2, Adjoint_hat) { + Pose2 T(1, 2, 3); + auto hat = [](const Vector& xi) { return ::wedge(xi); }; + Matrix3 expected = T.matrix() * hat(screwPose2::xi) * T.matrix().inverse(); + Matrix3 xiprime = hat(T.Adjoint(screwPose2::xi)); + EXPECT(assert_equal(expected, xiprime, 1e-6)); + + Vector3 xi2(4, 5, 6); + Matrix3 expected2 = T.matrix() * hat(xi2) * T.matrix().inverse(); + Matrix3 xiprime2 = hat(T.Adjoint(xi2)); + EXPECT(assert_equal(expected2, xiprime2, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 98d3e11ee..d516ddc8b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -143,6 +143,24 @@ TEST(Pose3, Adjoint_full) EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(Pose3, Adjoint_hat) +{ + auto hat = [](const Vector& xi) { return ::wedge(xi); }; + Matrix4 expected = T.matrix() * hat(screwPose3::xi) * T.matrix().inverse(); + Matrix4 xiprime = hat(T.Adjoint(screwPose3::xi)); + EXPECT(assert_equal(expected, xiprime, 1e-6)); + + Matrix4 expected2 = T2.matrix() * hat(screwPose3::xi) * T2.matrix().inverse(); + Matrix4 xiprime2 = hat(T2.Adjoint(screwPose3::xi)); + EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + Matrix4 expected3 = T3.matrix() * hat(screwPose3::xi) * T3.matrix().inverse(); + Matrix4 xiprime3 = hat(T3.Adjoint(screwPose3::xi)); + EXPECT(assert_equal(expected3, xiprime3, 1e-6)); +} + /* ************************************************************************* */ /** Agrawal06iros version of exponential map */ Pose3 Agrawal06iros(const Vector& xi) {