12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix retract jacobian for Unit3, and tests

release/4.3a0
Toni 2019-01-15 17:42:54 -05:00
parent 50a9695156
commit 8c8bdc4e67
3 changed files with 24 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/geometry/Unit3.cpp
View File

@ -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 // Compute the 3D xi_hat vector
const Vector3 xi_hat = basis() * v; const Vector3 xi_hat = basis() * v;
const double theta = xi_hat.norm(); const double theta = xi_hat.norm();
// Treat case of very small v differently // Treat case of very small v differently
Matrix23 H_from_point;
if (theta < std::numeric_limits<double>::epsilon()) { if (theta < std::numeric_limits<double>::epsilon()) {
if (H) { const Unit3 exp_p_xi_hat =
*H = -p_ * xi_hat.transpose() * basis() + Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat,
basis(); H? &H_from_point : nullptr);
} if (H) {
*H = H_from_point * (-p_ * xi_hat.transpose() * basis() +
return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); basis());
}
return exp_p_xi_hat;
} }
const Vector3 exp_p_xi_hat = const Unit3 exp_p_xi_hat =
std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); Unit3::FromPoint3(std::cos(theta) * p_ + xi_hat * (sin(theta) / theta),
H? &H_from_point : nullptr);
// Jacobian // Jacobian
if (H) { 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() + std::sin(theta) / theta * basis() +
xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2)) xi_hat * ((theta * std::cos(theta) - std::sin(theta)) / std::pow(theta, 2))
* xi_hat.transpose() / theta * basis(); * xi_hat.transpose() / theta * basis());
} }
return exp_p_xi_hat;
return Unit3(exp_p_xi_hat);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/geometry/Unit3.h
View File

@ -188,7 +188,7 @@ public:
}; };
/// The retract function /// 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 /// The local coordinates function
Vector2 localCoordinates(const Unit3& s) const; Vector2 localCoordinates(const Unit3& s) const;

23
gtsam/geometry/tests/testUnit3.cpp
View File

@ -372,37 +372,26 @@ TEST(Unit3, retract) {
} }
//******************************************************************************* //*******************************************************************************
// Wrapper to make retract return a Vector3 so we can test numerical derivatives. TEST (Unit3, jacobian_retract) {
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; Unit3 p;
boost::function<Unit3(const Vector2&)> f =
boost::bind(&Unit3::retract, p, _1, boost::none);
Matrix22 H;
{ {
Vector2 v (-0.2, 0.1); Vector2 v (-0.2, 0.1);
Matrix32 H;
p.retract(v, H); p.retract(v, H);
// Test that jacobian is numerically as expected. // Test that jacobian is numerically as expected.
boost::function<Vector3(const Unit3&, const Vector2&)> f = Matrix H_expected_numerical = numericalDerivative11(f, v);
boost::bind(RetractTest, _1, _2, boost::none);
Matrix32 H_expected_numerical = numericalDerivative22(f, p, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
} }
{ {
Vector2 v (0, 0); Vector2 v (0, 0);
Matrix32 H;
p.retract(v, H); p.retract(v, H);
// Test that jacobian is numerically as expected. // Test that jacobian is numerically as expected.
boost::function<Vector3(const Unit3&, const Vector2&)> f = Matrix H_expected_numerical = numericalDerivative11(f, v);
boost::bind(RetractTest, _1, _2, boost::none);
Matrix32 H_expected_numerical = numericalDerivative22(f, p, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9)); EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
} }
} }