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

Added test that exposes problem in localCoordinates

release/4.3a0
dellaert 2014-02-22 14:31:18 -05:00
parent 2b323d5cb7
commit 39b3b2160e
1 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -30,6 +30,36 @@ TEST (EssentialMatrix, equality) {
EXPECT(assert_equal(expected, actual));
}
//*******************************************************************************
TEST(EssentialMatrix, localCoordinates0) {
EssentialMatrix E;
Vector expected = zero(5);
Vector actual = E.localCoordinates(E);
EXPECT(assert_equal(expected, actual, 1e-8));
}
//*************************************************************************
TEST (EssentialMatrix, localCoordinates) {
// Pose between two cameras
Pose3 pose(c1Rc2, c1Tc2);
EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8));
Vector d = zero(6);
d(4) += 1e-5;
Vector actual2 = hx.localCoordinates(
EssentialMatrix::FromPose3(pose.retract(d)));
EXPECT(assert_equal(zero(5), actual2, 1e-8));
}
//*************************************************************************
TEST (EssentialMatrix, retract0) {
EssentialMatrix actual = trueE.retract(zero(5));
EXPECT(assert_equal(trueE, actual));
}
//*************************************************************************
TEST (EssentialMatrix, retract1) {
EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2);
@ -89,7 +119,10 @@ TEST (EssentialMatrix, rotate) {
// Derivatives
Matrix actH1, actH2;
try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
try {
bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) {
} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8));