diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 5808f36f8..a169c833c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -418,6 +418,29 @@ TEST(Pose3, transform_to_rotate) { EXPECT(assert_equal(expected, actual, 0.001)); } +/* ************************************************************************* */ +// Check transformPoseFrom and its pushforward +Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) { + return wTa.transformPoseFrom(aTb); +} + +TEST(Pose3, transformPoseFrom) +{ + Matrix actual = (T2*T2).matrix(); + Matrix expected = T2.matrix()*T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix H1, H2; + T2.transformPoseFrom(T2, H1, H2); + + Matrix numericalH1 = numericalDerivative21(transformPoseFrom_, T2, T2); + EXPECT(assert_equal(numericalH1, H1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3)); + + Matrix numericalH2 = numericalDerivative22(transformPoseFrom_, T2, T2); + EXPECT(assert_equal(numericalH2, H2, 1e-4)); +} + /* ************************************************************************* */ TEST(Pose3, transformTo) { Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));