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

Unit test fixes for quaternion mode and also rotation matrix + full expmap mode.

release/4.3a0
cbeall3 2014-05-06 20:03:45 -04:00
parent 2649b0fd7a
commit 842554a230
7 changed files with 82 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -125,7 +125,7 @@ TEST (EssentialMatrix, rotate) {
} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH1, actH1, 1e-7));
// Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
}
@ -149,7 +149,7 @@ TEST (EssentialMatrix, FromPose3_b) {
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
//*************************************************************************

4
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -35,13 +35,13 @@ TEST(BetweenFactor, Rot3) {
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1));
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2));
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}
/* ************************************************************************* */

4
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -63,8 +63,8 @@ TEST( EssentialMatrixConstraint, test ) {
factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
CHECK(assert_equal(expectedH2, actualH2, 1e-5));
}
/* ************************************************************************* */

9
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -128,7 +128,11 @@ TEST (EssentialMatrixFactor, minimization) {
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
@ -339,7 +343,12 @@ TEST (EssentialMatrixFactor, extraMinimization) {
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;

20
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -59,7 +59,11 @@ TEST (RotateFactor, test) {
EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529);
#else
Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
#endif
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
@ -97,7 +101,12 @@ TEST (RotateFactor, minimization) {
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(3545.40, graph.error(initial), 1);
#else
EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
#endif
// Optimize
LevenbergMarquardtParams parameters;
@ -120,7 +129,13 @@ TEST (RotateDirectionsFactor, test) {
EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
Vector expectedE = (Vector(2) << -0.0890529, -0.202981);
#else
Vector expectedE = (Vector(2) << -0.08867, -0.20197);
#endif
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
@ -160,7 +175,12 @@ TEST (RotateDirectionsFactor, minimization) {
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(3335.9, graph.error(initial), 1);
#else
EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);
#endif
// Optimize
LevenbergMarquardtParams parameters;

28
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -95,12 +95,21 @@ TEST( PoseBetweenFactor, Error ) {
// The expected error
Vector expectedError(6);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
expectedError << -0.0298135267953815,
0.0131341515747393,
0.0968868439682154,
-0.145701634472172,
-0.134898525569125,
-0.0421026389164264;
#else
expectedError << -0.029839512616488,
0.013145599455949,
0.096971291682578,
-0.139187549519821,
-0.142346243063553,
-0.039088532100977;
#endif
// Create a factor and calculate the error
Key poseKey1(1);
@ -123,12 +132,23 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) {
// The expected error
Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP)
expectedError << 0.0173358202010741,
0.0222210698409755,
-0.0125032003886145,
0.0263800787416566,
0.00540285006310398,
0.000175859555693563;
#else
expectedError << 0.017337193670445,
0.022222830355243,
-0.012504190982804,
0.026413288603739,
0.005237695303536,
-0.000071612703633;
#endif
// Create a factor and calculate the error
Key poseKey1(1);
@ -165,8 +185,8 @@ TEST( PoseBetweenFactor, Jacobian ) {
factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
CHECK(assert_equal(expectedH2, actualH2, 1e-6));
}
/* ************************************************************************* */
@ -194,8 +214,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
Vector error = factor.evaluateError(pose1, pose2, actualH1, actualH2);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH2, actualH2, 1e-9));
CHECK(assert_equal(expectedH1, actualH1, 1e-6));
CHECK(assert_equal(expectedH2, actualH2, 1e-5));
}
/* ************************************************************************* */

25
gtsam_unstable/slam/tests/testPosePriorFactor.cpp
View File

@ -90,12 +90,23 @@ TEST( PosePriorFactor, Error ) {
// The expected error
Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP)
expectedError << -0.182948257976108,
0.13851858011118,
-0.157375974517456,
0.766913166076379,
-1.22976117053126,
0.949345561430261;
#else
expectedError << -0.184137861505414,
0.139419283914526,
-0.158399296722242,
0.740211733817804,
-1.198210282560946,
1.008156093015192;
#endif
// Create a factor and calculate the error
Key poseKey(1);
@ -116,12 +127,22 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
// The expected error
Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP)
expectedError << -0.0224998729281528,
0.191947887288328,
0.273826035236257,
1.36483391560855,
-0.754590051075035,
0.585710674473659;
#else
expectedError << -0.022712885347328,
0.193765110165872,
0.276418420819283,
1.497519863757366,
-0.549375791422721,
0.452761203187666;
#endif
// Create a factor and calculate the error
Key poseKey(1);
@ -152,7 +173,7 @@ TEST( PosePriorFactor, Jacobian ) {
factor.evaluateError(pose, actualH1);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}
/* ************************************************************************* */
@ -176,7 +197,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
factor.evaluateError(pose, actualH1);
// Verify we get the expected error
CHECK(assert_equal(expectedH1, actualH1, 1e-9));
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
}
/* ************************************************************************* */