From f217a5bd8affaae464b3184a31f372434d638360 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 12 Mar 2010 19:16:53 +0000
Subject: [PATCH] I reverted back to the old expmap in Pose3 :-( It does not
 work well for Urban, and now it turns out Yong-Dian has problems, too. It
 seems there is still something that we do not quite understand about the
 whole Lie group optimization business. If you want to run with the new
 expmap, define CORRECT_POSE3_EXPMAP

---
 cpp/Pose3.cpp           | 12 ++++++------
 cpp/testPose3.cpp       |  6 +++---
 cpp/testPose3Config.cpp | 12 ++++++------
 3 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp
index 7954ff17e..0976a7c1e 100644
--- a/cpp/Pose3.cpp
+++ b/cpp/Pose3.cpp
@@ -44,7 +44,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
 
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 
   /** Modified from Murray94book version (which assumes w and v normalized?) */
   template<> Pose3 expmap(const Vector& xi) {
@@ -144,7 +144,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 		const Matrix R = pose.rotation().matrix();
     Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
     return collect(2,&DR,&R);
@@ -170,7 +170,7 @@ namespace gtsam {
   Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
     Point3 q = transform_to(pose,p);
     Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 		return collect(2, &DR, &_I3);
 #else
     Matrix DT = - pose.rotation().transpose(); // negative because of sub
@@ -186,7 +186,7 @@ namespace gtsam {
   /* ************************************************************************* */
   // compose = Pose3(compose(R1,R2),transform_from(p1,t2)
   Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 		return AdjointMap(inverse(p2));
 #else
 		const Rot3& R2 = p2.rotation();
@@ -199,7 +199,7 @@ namespace gtsam {
 	}
 
 	Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 
 		return I6;
 #else
@@ -214,7 +214,7 @@ namespace gtsam {
   // inverse = Pose3(inverse(R),-unrotate(R,t));
 	// TODO: combined function will save !
 	Matrix Dinverse(const Pose3& p) {
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 		return - AdjointMap(p);
 #else
 		const Rot3& R = p.rotation();
diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp
index e2974df77..c25a9571d 100644
--- a/cpp/testPose3.cpp
+++ b/cpp/testPose3.cpp
@@ -34,7 +34,7 @@ TEST( Pose3, expmap_a)
   fill(v.begin(), v.end(), 0);
   v(0) = 0.3;
   CHECK(assert_equal(expmap(id,v), Pose3(R, Point3())));
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 
   v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
 #else
@@ -52,7 +52,7 @@ TEST(Pose3, expmap_b)
   CHECK(assert_equal(expected, p2));
 }
 
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 
 /* ************************************************************************* */
 // test case for screw motion in the plane
@@ -384,7 +384,7 @@ TEST(Pose3, manifold)
 	// Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-)
 	CHECK(assert_equal(d12,-d21));
 
-#ifndef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
 
 
 	// todo: Frank - Below only works for correct "Agrawal06iros style expmap
diff --git a/cpp/testPose3Config.cpp b/cpp/testPose3Config.cpp
index b97c45300..07b5e7eee 100644
--- a/cpp/testPose3Config.cpp
+++ b/cpp/testPose3Config.cpp
@@ -36,17 +36,17 @@ TEST( Pose3Config, pose3Circle )
 TEST( Pose3Config, expmap )
 {
 	Pose3Config expected;
-#ifdef FASTER_BUT_INCORRECT_EXMAP
+#ifdef CORRECT_POSE3_EXMAP
+	expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
+	expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
+	expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
+	expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
+#else
 	// expected is circle shifted to East
 	expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
 	expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
 	expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
 	expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
-#else
-	expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0)));
-	expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0)));
-	expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0)));
-	expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0)));
 #endif
 
 	// Note expmap coordinates are in global coordinates with non-compose expmap