diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 88eb0c251..9eb5e2f29 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -166,6 +166,32 @@ TEST(OrientedPlane3, jacobian_retract) { } } +//******************************************************************************* +TEST(OrientedPlane3, jacobian_normal) { + Matrix23 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::normal, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.normal(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + +//******************************************************************************* +TEST(OrientedPlane3, jacobian_distance) { + Matrix13 H_actual, H_expected; + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + + std::function f = std::bind( + &OrientedPlane3::distance, std::placeholders::_1, boost::none); + + H_expected = numericalDerivative11(f, plane); + plane.distance(H_actual); + EXPECT(assert_equal(H_actual, H_expected, 1e-5)); +} + //******************************************************************************* TEST(OrientedPlane3, getMethodJacobians) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); @@ -173,8 +199,7 @@ TEST(OrientedPlane3, getMethodJacobians) { Matrix23 H_normal; Matrix13 H_distance; - // The getter's jacobians lie exactly on the tangent space - // so they should exactly equal the retract jacobian for the zero vector. + // confirm the getters are exactly on the tangent space Vector3 v(0, 0, 0); plane.retract(v, H_retract); plane.normal(H_normal);