diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 1ff536b9e..193aab9b5 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -250,33 +250,36 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<3,2> H) const { +Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const { // Compute the 3D xi_hat vector const Vector3 xi_hat = basis() * v; const double theta = xi_hat.norm(); // Treat case of very small v differently + Matrix23 H_from_point; if (theta < std::numeric_limits::epsilon()) { - if (H) { - *H = -p_ * xi_hat.transpose() * basis() + - basis(); - } - - return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); + const Unit3 exp_p_xi_hat = + Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat, + H? &H_from_point : nullptr); + if (H) { + *H = H_from_point * (-p_ * xi_hat.transpose() * basis() + + basis()); + } + return exp_p_xi_hat; } - const Vector3 exp_p_xi_hat = - std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); - + const Unit3 exp_p_xi_hat = + Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat * (sin(theta) / theta), + H? &H_from_point : nullptr); // Jacobian if (H) { - *H = p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + + *H = H_from_point * + (p_ * (-std::sin(theta)) * xi_hat.transpose() / theta * basis() + std::sin(theta) / theta * basis() + xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2)) - * xi_hat.transpose() / theta * basis(); + * xi_hat.transpose() / theta * basis()); } - - return Unit3(exp_p_xi_hat); + return exp_p_xi_hat; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f50f5ef44..f182df285 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -188,7 +188,7 @@ public: }; /// The retract function - Unit3 retract(const Vector2& v, OptionalJacobian<3,2> H = boost::none) const; + Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; /// The local coordinates function Vector2 localCoordinates(const Unit3& s) const; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index f33037b26..815231d38 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -372,37 +372,26 @@ 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) { +TEST (Unit3, jacobian_retract) { Unit3 p; + boost::function f = + boost::bind(&Unit3::retract, p, _1, boost::none); + Matrix22 H; { 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); + Matrix H_expected_numerical = numericalDerivative11(f, 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); + Matrix H_expected_numerical = numericalDerivative11(f, v); EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); - } }