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

Fix jacobian of retract for OrientedPlane3 and tests

release/4.3a0
Toni 2019-01-15 17:52:45 -05:00
parent 21163e9bdc
commit dccff83e38
3 changed files with 14 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/OrientedPlane3.cpp
View File

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

2
gtsam/geometry/OrientedPlane3.h
View File

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

34
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -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<OrientedPlane3(const Vector3&)> 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<Vector4(const OrientedPlane3&, const Vector3&)> 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<Vector4(const OrientedPlane3&, const Vector3&)> 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));
}
}