diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f5ebf29afa..b1df450048 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include @@ -1301,6 +1302,39 @@ TEST(Pose3, interpolateRtJacobians) { } } +TEST(Pose3, expressionWrappers) { + Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); + Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); + double t = 0.3; + Values vals; + vals.insert(0,X); + vals.insert(1,Y); + vals.insert(2,t); + + { // interpolate (templated wrapper applies to all classes) + Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT; + std::vector Hlist = {{},{},{}}; + Pose3 expected = interpolate(X, Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT); + Pose3 actual = interpolate(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist); + + EXPECT(assert_equal(expected,actual,1e-6)); + EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6)); + EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6)); + EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6)); + } + { // interpolateRt (Pose3 specialisation) + Matrix expectedJacobianX, expectedJacobianY, expectedJacobianT; + std::vector Hlist = {{},{},{}}; + Pose3 expected = X.interpolateRt(Y, t, expectedJacobianX, expectedJacobianY, expectedJacobianT); + Pose3 actual = interpolateRt(Pose3_(Key(0)), Pose3_(Key(1)), Double_(Key(2))).value(vals, Hlist); + + EXPECT(assert_equal(expected,actual,1e-6)); + EXPECT(assert_equal(expectedJacobianX,Hlist[0],1e-6)); + EXPECT(assert_equal(expectedJacobianY,Hlist[1],1e-6)); + EXPECT(assert_equal(expectedJacobianT,Hlist[2],1e-6)); + } +} + /* ************************************************************************* */ TEST(Pose3, Create) { Matrix63 actualH1, actualH2;