From 845697555a99016eb64ea0028023e86d55392dc0 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 23 Oct 2014 09:52:35 +0200
Subject: [PATCH] Fixed tests for Quaternion mode but Pose3 not in EXPMAP mode

---
 .../slam/tests/testPoseBetweenFactor.cpp      | 17 +++++++++++--
 .../slam/tests/testPosePriorFactor.cpp        | 24 ++++++++++++++-----
 2 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
index bf9dc0e8e..900356213 100644
--- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
@@ -95,13 +95,20 @@ TEST( PoseBetweenFactor, Error ) {
 
   // The expected error
   Vector expectedError(6);
+  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
   expectedError << -0.0298135267953815,
                     0.0131341515747393,
                     0.0968868439682154,
+#if defined(GTSAM_POSE3_EXPMAP)
                    -0.145701634472172,
                    -0.134898525569125,
                    -0.0421026389164264;
+#else
+                   -0.13918755,
+                   -0.142346243,
+                   -0.0390885321;
+#endif
 #else
   expectedError << -0.029839512616488,
                     0.013145599455949,
@@ -132,14 +139,20 @@ 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)
+  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
+#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
   expectedError <<    0.0173358202010741,
                       0.0222210698409755,
                      -0.0125032003886145,
+#if defined(GTSAM_POSE3_EXPMAP)
                       0.0263800787416566,
                       0.00540285006310398,
                       0.000175859555693563;
+#else
+                      0.0264132886,
+                      0.0052376953,
+                      -7.16127036e-05;
+#endif
 #else
   expectedError <<    0.017337193670445,
                       0.022222830355243,
diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
index cbfa45431..16b2bccc1 100644
--- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
+++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp
@@ -90,14 +90,20 @@ 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)
+  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
+#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
   expectedError << -0.182948257976108,
                     0.13851858011118,
                    -0.157375974517456,
+#if defined(GTSAM_POSE3_EXPMAP)
                     0.766913166076379,
                    -1.22976117053126,
                     0.949345561430261;
+#else
+                    0.740211734,
+                   -1.19821028,
+                    1.00815609;
+#endif
 #else
   expectedError << -0.184137861505414,
                     0.139419283914526,
@@ -115,7 +121,7 @@ TEST( PosePriorFactor, Error ) {
   Vector actualError(factor.evaluateError(pose));
 
   // 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
   Vector expectedError(6);
-  // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
-#if defined(GTSAM_ROT3_EXPMAP)
+  // The solution depends on choice of Pose3 and Rot3 Expmap mode!
+#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
   expectedError << -0.0224998729281528,
                     0.191947887288328,
                     0.273826035236257,
+#if defined(GTSAM_POSE3_EXPMAP)
                     1.36483391560855,
                    -0.754590051075035,
                     0.585710674473659;
+#else
+                    1.49751986,
+                   -0.549375791,
+                    0.452761203;
+#endif
 #else
   expectedError << -0.022712885347328,
                     0.193765110165872,
@@ -151,7 +163,7 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
   Vector actualError(factor.evaluateError(pose));
 
   // Verify we get the expected error
-  CHECK(assert_equal(expectedError, actualError, 1e-9));
+  CHECK(assert_equal(expectedError, actualError, 1e-8));
 }
 
 /* ************************************************************************* */