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

Fixed tests for Quaternion mode but Pose3 not in EXPMAP mode

release/4.3a0
dellaert 2014-10-23 09:52:35 +02:00
parent 056254bf93
commit 845697555a
2 changed files with 33 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -95,13 +95,20 @@ TEST( PoseBetweenFactor, Error ) {
// The expected error // The expected error
Vector expectedError(6); Vector expectedError(6);
// The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
expectedError << -0.0298135267953815, expectedError << -0.0298135267953815,
0.0131341515747393, 0.0131341515747393,
0.0968868439682154, 0.0968868439682154,
#if defined(GTSAM_POSE3_EXPMAP)
-0.145701634472172, -0.145701634472172,
-0.134898525569125, -0.134898525569125,
-0.0421026389164264; -0.0421026389164264;
#else
-0.13918755,
-0.142346243,
-0.0390885321;
#endif
#else #else
expectedError << -0.029839512616488, expectedError << -0.029839512616488,
0.013145599455949, 0.013145599455949,
@ -132,14 +139,20 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) {
// The expected error // The expected error
Vector expectedError(6); Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! // The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
expectedError << 0.0173358202010741, expectedError << 0.0173358202010741,
0.0222210698409755, 0.0222210698409755,
-0.0125032003886145, -0.0125032003886145,
#if defined(GTSAM_POSE3_EXPMAP)
0.0263800787416566, 0.0263800787416566,
0.00540285006310398, 0.00540285006310398,
0.000175859555693563; 0.000175859555693563;
#else
0.0264132886,
0.0052376953,
-7.16127036e-05;
#endif
#else #else
expectedError << 0.017337193670445, expectedError << 0.017337193670445,
0.022222830355243, 0.022222830355243,

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

@ -90,14 +90,20 @@ TEST( PosePriorFactor, Error ) {
// The expected error // The expected error
Vector expectedError(6); Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! // The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
expectedError << -0.182948257976108, expectedError << -0.182948257976108,
0.13851858011118, 0.13851858011118,
-0.157375974517456, -0.157375974517456,
#if defined(GTSAM_POSE3_EXPMAP)
0.766913166076379, 0.766913166076379,
-1.22976117053126, -1.22976117053126,
0.949345561430261; 0.949345561430261;
#else
0.740211734,
-1.19821028,
1.00815609;
#endif
#else #else
expectedError << -0.184137861505414, expectedError << -0.184137861505414,
0.139419283914526, 0.139419283914526,
@ -115,7 +121,7 @@ TEST( PosePriorFactor, Error ) {
Vector actualError(factor.evaluateError(pose)); Vector actualError(factor.evaluateError(pose));
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -127,14 +133,20 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
// The expected error // The expected error
Vector expectedError(6); Vector expectedError(6);
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! // The solution depends on choice of Pose3 and Rot3 Expmap mode!
#if defined(GTSAM_ROT3_EXPMAP) #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
expectedError << -0.0224998729281528, expectedError << -0.0224998729281528,
0.191947887288328, 0.191947887288328,
0.273826035236257, 0.273826035236257,
#if defined(GTSAM_POSE3_EXPMAP)
1.36483391560855, 1.36483391560855,
-0.754590051075035, -0.754590051075035,
0.585710674473659; 0.585710674473659;
#else
1.49751986,
-0.549375791,
0.452761203;
#endif
#else #else
expectedError << -0.022712885347328, expectedError << -0.022712885347328,
0.193765110165872, 0.193765110165872,
@ -151,7 +163,7 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
Vector actualError(factor.evaluateError(pose)); Vector actualError(factor.evaluateError(pose));
// Verify we get the expected error // Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9)); CHECK(assert_equal(expectedError, actualError, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */