retract and localCoordinates optional argument in Pose3 to switch between different math versions, and unit testing all versions

gtsam/geometry/Pose3.cpp

@@ -97,11 +97,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
 	// Different versions of retract
-  Pose3 Pose3::retract(const Vector& xi) const {
+  Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
-#ifdef CORRECT_POSE3_EXMAP
+    if(mode == Pose3::FIRST_ORDER) {
-  	// Lie group exponential map, traces out geodesic
-		return compose(Expmap(xi));
-#else
       Vector omega(sub(xi, 0, 3));
       Point3 v(sub(xi, 3, 6));
 
@@ -119,16 +116,19 @@ namespace gtsam {
       // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2);
 
       return Pose3(R, t);
-#endif
+    } else if(mode == Pose3::CORRECT_EXPMAP) {
+      // Lie group exponential map, traces out geodesic
+      return compose(Expmap(xi));
+    } else {
+      assert(false);
+      exit(1);
+    }
 	}
 
   /* ************************************************************************* */
   // different versions of localCoordinates
-	Vector Pose3::localCoordinates(const Pose3& T) const {
+	Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
-#ifdef CORRECT_POSE3_EXMAP
+    if(mode == Pose3::FIRST_ORDER) {
-		// Lie group logarithm map, exact inverse of exponential map
-		return Logmap(between(T));
-#else
       // R is always done exactly in all three retract versions below
       Vector omega = R_.localCoordinates(T.rotation());
 
@@ -142,7 +142,13 @@ namespace gtsam {
       // TODO: correct second order t inverse
 
       return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
-#endif
+    } else if(mode == Pose3::CORRECT_EXPMAP) {
+      // Lie group logarithm map, exact inverse of exponential map
+      return Logmap(between(T));
+    } else {
+      assert(false);
+      exit(1);
+    }
 	}
 
   /* ************************************************************************* */

gtsam/geometry/Pose3.h

@@ -18,6 +18,10 @@
 
 #pragma once
 
+#ifndef POSE3_DEFAULT_COORDINATES_MODE
+#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
+#endif
+
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 
@@ -98,6 +102,14 @@ namespace gtsam {
     /// @name Manifold
     /// @{
 
+    /** Enum to indicate which method should be used in Pose3::retract() and
+     * Pose3::localCoordinates()
+     */
+    enum CoordinatesMode {
+      FIRST_ORDER, ///< A fast first-order approximation to the exponential map.
+      CORRECT_EXPMAP ///< The correct exponential map, computationally expensive.
+    };
+
     /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
     static size_t Dim() { return dimension; }
 
@@ -106,10 +118,10 @@ namespace gtsam {
 
 
     /// Retraction from R^6 to Pose3 manifold neighborhood around current pose
-    Pose3 retract(const Vector& d) const;
+    Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
 
     /// Local 6D coordinates of Pose3 manifold neighborhood around current pose
-    Vector localCoordinates(const Pose3& T2) const;
+    Vector localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
 
     /// @}
     /// @name Lie Group

gtsam/geometry/tests/testPose3.cpp

@@ -44,18 +44,25 @@ TEST( Pose3, equals)
 }
 
 /* ************************************************************************* */
-TEST( Pose3, retract)
+TEST( Pose3, retract_first_order)
 {
   Pose3 id;
   Vector v = zero(6);
   v(0) = 0.3;
-  EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
+  EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2));
-#ifdef CORRECT_POSE3_EXMAP
-  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
-#else
   v(3)=0.2;v(4)=0.7;v(5)=-2;
-#endif
+  EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2));
-  EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
+}
+
+/* ************************************************************************* */
+TEST( Pose3, retract_expmap)
+{
+  Pose3 id;
+  Vector v = zero(6);
+  v(0) = 0.3;
+  EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::CORRECT_EXPMAP),1e-2));
+  v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
+  EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::CORRECT_EXPMAP),1e-2));
 }
 
 /* ************************************************************************* */
@@ -443,31 +450,48 @@ TEST( Pose3, transformPose_to)
 }
 
 /* ************************************************************************* */
-TEST(Pose3, localCoordinates)
+TEST(Pose3, localCoordinates_first_order)
 {
 	Vector d12 = repeat(6,0.1);
-	Pose3 t1 = T, t2 = t1.retract(d12);
+	Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER);
-	EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
+	EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER)));
 }
 
 /* ************************************************************************* */
-TEST(Pose3, manifold)
+TEST(Pose3, localCoordinates_expmap)
+{
+  Vector d12 = repeat(6,0.1);
+  Pose3 t1 = T, t2 = t1.retract(d12, Pose3::CORRECT_EXPMAP);
+  EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP)));
+}
+
+/* ************************************************************************* */
+TEST(Pose3, manifold_first_order)
 {
 	Pose3 t1 = T;
 	Pose3 t2 = T3;
 	Pose3 origin;
-	Vector d12 = t1.localCoordinates(t2);
+	Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER);
-	EXPECT(assert_equal(t2, t1.retract(d12)));
+	EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER)));
-	// todo: richard - commented out because this tests for "compose-style" (new) expmap
+	Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER);
-	// EXPECT(assert_equal(t2, retract(origin,d12)*t1));
+	EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER)));
-	Vector d21 = t2.localCoordinates(t1);
-	EXPECT(assert_equal(t1, t2.retract(d21)));
-	// todo: richard - commented out because this tests for "compose-style" (new) expmap
-	// EXPECT(assert_equal(t1, retract(origin,d21)*t2));
-
-	// Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map
-	//	EXPECT(assert_equal(d12,-d21));
 }
+
+/* ************************************************************************* */
+TEST(Pose3, manifold_expmap)
+{
+  Pose3 t1 = T;
+  Pose3 t2 = T3;
+  Pose3 origin;
+  Vector d12 = t1.localCoordinates(t2, Pose3::CORRECT_EXPMAP);
+  EXPECT(assert_equal(t2, t1.retract(d12, Pose3::CORRECT_EXPMAP)));
+  Vector d21 = t2.localCoordinates(t1, Pose3::CORRECT_EXPMAP);
+  EXPECT(assert_equal(t1, t2.retract(d21, Pose3::CORRECT_EXPMAP)));
+
+  // Check that log(t1,t2)=-log(t2,t1)
+  EXPECT(assert_equal(d12,-d21));
+}
+
 /* ************************************************************************* */
 TEST(Pose3, subgroups)
 {