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

Fix test for full Pose3 expmap

release/4.3a0
Fan Jiang 2020-07-21 20:55:21 -04:00
parent 24b2f50fe3
commit 456f1baf8f
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
Vector9 P = Camera().localCoordinates(camera);
Vector3 X = point;
#ifdef GTSAM_POSE3_EXPMAP
Vector2 expectedMeasurement(1.3124675, 1.2057287);
#else
Vector2 expectedMeasurement(1.2431567, 1.2525694);
#endif
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
}
@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
// Check that Local worked as expected
TEST(AdaptAutoDiff, Local) {
using namespace example;
#ifdef GTSAM_POSE3_EXPMAP
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
#else
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
#endif
EXPECT(equal_with_abs_tol(expectedP, P));
Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
EXPECT(equal_with_abs_tol(expectedX, X));