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 { OrientedPlane3 OrientedPlane3::retract(const Vector3& v,
Matrix32 H_n; OptionalJacobian<3,3> H) const {
Matrix22 H_n;
Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr)); Unit3 n_retract (n_.retract(Vector2(v(0), v(1)), H? &H_n : nullptr));
if (H) { 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)); return OrientedPlane3(n_retract, d_ + v(2));
} }

2
gtsam/geometry/OrientedPlane3.h
View File

@ -134,7 +134,7 @@ public:
} }
/// The retract function /// 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 /// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const; 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)); 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) { TEST (OrientedPlane3, jacobian_retract) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5); 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); Vector3 v (-0.1, 0.2, 0.3);
plane.retract(v, H); plane.retract(v, H_actual);
// Test that jacobian is numerically as expected. Matrix H_expected_numerical = numericalDerivative11(f, v);
boost::function<Vector4(const OrientedPlane3&, const Vector3&)> f = EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
boost::bind(RetractTest, _1, _2, boost::none);
Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
} }
{ {
Matrix43 H;
Vector3 v (0, 0, 0); Vector3 v (0, 0, 0);
plane.retract(v, H); plane.retract(v, H_actual);
// Test that jacobian is numerically as expected. Matrix H_expected_numerical = numericalDerivative11(f, v);
boost::function<Vector4(const OrientedPlane3&, const Vector3&)> f = EXPECT(assert_equal(H_expected_numerical, H_actual, 1e-9));
boost::bind(RetractTest, _1, _2, boost::none);
Matrix43 H_expected_numerical = numericalDerivative22(f, plane, v);
EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
} }
} }