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

numerically checks getter jacobians

release/4.3a0
BrettRD 2023-01-09 16:58:59 +11:00
parent 65a21edc89
commit 80aa47539c
1 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -166,6 +166,32 @@ TEST(OrientedPlane3, jacobian_retract) {
}
}
//*******************************************************************************
TEST(OrientedPlane3, jacobian_normal) {
Matrix23 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<Unit3(const OrientedPlane3&)> f = std::bind(
&OrientedPlane3::normal, std::placeholders::_1, boost::none);
H_expected = numericalDerivative11(f, plane);
plane.normal(H_actual);
EXPECT(assert_equal(H_actual, H_expected, 1e-5));
}
//*******************************************************************************
TEST(OrientedPlane3, jacobian_distance) {
Matrix13 H_actual, H_expected;
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
std::function<double(const OrientedPlane3&)> f = std::bind(
&OrientedPlane3::distance, std::placeholders::_1, boost::none);
H_expected = numericalDerivative11(f, plane);
plane.distance(H_actual);
EXPECT(assert_equal(H_actual, H_expected, 1e-5));
}
//*******************************************************************************
TEST(OrientedPlane3, getMethodJacobians) {
OrientedPlane3 plane(-1, 0.1, 0.2, 5);
@ -173,8 +199,7 @@ TEST(OrientedPlane3, getMethodJacobians) {
Matrix23 H_normal;
Matrix13 H_distance;
// The getter's jacobians lie exactly on the tangent space
// so they should exactly equal the retract jacobian for the zero vector.
// confirm the getters are exactly on the tangent space
Vector3 v(0, 0, 0);
plane.retract(v, H_retract);
plane.normal(H_normal);