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
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<double>::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;
}
/* ************************************************************************* */

2
gtsam/geometry/Unit3.h
View File

@ -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;

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.
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<Unit3(const Vector2&)> 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<Vector3(const Unit3&, const Vector2&)> 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<Vector3(const Unit3&, const Vector2&)> 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));
}
}