diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 82207d4ce..5cfa80779 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -83,11 +83,12 @@ Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobia } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector3& v, OptionalJacobian<4,3> H) const { - Matrix32 H_n; +OrientedPlane3 OrientedPlane3::retract(const Vector3& v, + OptionalJacobian<3,3> H) const { + Matrix22 H_n; Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); if (H) { - *H << H_n, Vector3::Zero(), 0, 0, 1; + *H << H_n, Vector2::Zero(), 0, 0, 1; } return OrientedPlane3(n_retract, d_ + v(2)); } diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 7306942f2..596118385 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -134,7 +134,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector3& v, OptionalJacobian<4,3> H = boost::none) const; + OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 1ad2b0405..54cf84ea8 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -161,39 +161,23 @@ 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; + Matrix33 H_actual; + boost::function f = + boost::bind(&OrientedPlane3::retract, plane, _1, boost::none); { 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)); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 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)); + plane.retract(v, H_actual); + Matrix H_expected_numerical = numericalDerivative11(f, v); + EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9)); } }