diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index a1531e07c..1ad2b0405 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -161,6 +161,42 @@ TEST (OrientedPlane3, error2) { EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } +//******************************************************************************* +// Wrapper to make retract return a Vector3 so we can test numerical derivatives. +Vector4 RetractTest(const OrientedPlane3& plane, const Vector3& v, + OptionalJacobian<4, 3> H) { + OrientedPlane3 plane_retract = plane.retract(v, H); + return Vector4(plane_retract.normal().point3().x(), + plane_retract.normal().point3().y(), + plane_retract.normal().point3().z(), + plane_retract.distance()); +} + +//******************************************************************************* +TEST (OrientedPlane3, jacobian_retract) { + OrientedPlane3 plane(-1, 0.1, 0.2, 5); + Matrix43 H; + { + Vector3 v (-0.1, 0.2, 0.3); + plane.retract(v, H); + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } + { + Matrix43 H; + Vector3 v (0, 0, 0); + plane.retract(v, H); + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 542aca038..f33037b26 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -371,6 +371,41 @@ TEST(Unit3, retract) { } } +//******************************************************************************* +// Wrapper to make retract return a Vector3 so we can test numerical derivatives. +Vector3 RetractTest(const Unit3&p, const Vector2& v, OptionalJacobian<3, 2> H) { + Unit3 p_retract = p.retract(v, H); + return p_retract.point3(); +} + +//******************************************************************************* +TEST (OrientedPlane3, jacobian_retract) { + Unit3 p; + { + Vector2 v (-0.2, 0.1); + Matrix32 H; + p.retract(v, H); + + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + } + { + Vector2 v (0, 0); + Matrix32 H; + p.retract(v, H); + + // Test that jacobian is numerically as expected. + boost::function f = + boost::bind(RetractTest, _1, _2, boost::none); + Matrix32 H_expected_numerical = numericalDerivative22(f, p, v); + EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); + + } +} + //******************************************************************************* TEST(Unit3, retract_expmap) { Unit3 p;