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

gtsam/geometry/Rot3.h

@@ -21,6 +21,10 @@
 
 #pragma once
 
+#ifndef ROT3_DEFAULT_COORDINATES_MODE
+#define ROT3_DEFAULT_COORDINATES_MODE Rot3::FIRST_ORDER
+#endif
+
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
@@ -220,6 +224,15 @@ namespace gtsam {
     /// @name Manifold
     /// @{
 
+    /** Enum to indicate which method should be used in Rot3::retract() and
+     * Rot3::localCoordinates()
+     */
+    enum CoordinatesMode {
+      FIRST_ORDER, ///< A fast first-order approximation to the exponential map.
+      SLOW_CALEY, ///< Retract and localCoordinates using the Caley transform.
+      CORRECT_EXPMAP ///< The correct exponential map, computationally expensive.
+    };
+
     /// dimension of the variable - used to autodetect sizes
     static size_t Dim() { return dimension; }
 
@@ -227,10 +240,10 @@ namespace gtsam {
     size_t dim() const { return dimension; }
 
     /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
-    Rot3 retract(const Vector& omega) const;
+    Rot3 retract(const Vector& omega, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
     /// Returns inverse retraction
-    Vector localCoordinates(const Rot3& t2) const;
+    Vector localCoordinates(const Rot3& t2, CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
 
     /// @}
     /// @name Lie Group

gtsam/geometry/Rot3M.cpp

@@ -246,14 +246,8 @@ Vector Rot3::Logmap(const Rot3& R) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::retract(const Vector& omega) const {
+Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const {
-#ifdef CORRECT_ROT3_EXMAP
+  if(mode == Rot3::FIRST_ORDER) {
-	return (*this)*Expmap(omega);
-#else
-#ifdef SLOW_CAYLEY
-	Matrix Omega = skewSymmetric(omega);
-	return (*this)*Cayley<3>(-Omega/2);
-#else
     const double x = omega(0), y = omega(1), z = omega(2);
     const double x2 = x*x, y2 = y*y, z2 = z*z;
     const double xy = x*y, xz = x*z, yz = y*z;
@@ -263,22 +257,22 @@ Rot3 Rot3::retract(const Vector& omega) const {
         (xy + 2*z)*_2f, (4-x2+y2-z2)*f, (yz - 2*x)*_2f,
         (xz - 2*y)*_2f, (yz + 2*x)*_2f, (4-x2-y2+z2)*f
     );
-#endif
+  } else if(mode == Rot3::SLOW_CALEY) {
-#endif
+    Matrix Omega = skewSymmetric(omega);
+    return (*this)*Cayley<3>(-Omega/2);
+  } else if(mode == Rot3::CORRECT_EXPMAP) {
+    return (*this)*Expmap(omega);
+  } else {
+    assert(false);
+    exit(1);
+  }
 }
 
 /* ************************************************************************* */
-Vector Rot3::localCoordinates(const Rot3& T) const {
+Vector Rot3::localCoordinates(const Rot3& T, CoordinatesMode mode) const {
-#ifdef CORRECT_ROT3_EXMAP
+  if(mode == Rot3::FIRST_ORDER) {
-	return Logmap(between(T));
-#else
     // Create a fixed-size matrix
     Eigen::Matrix3d A(between(T).matrix());
-#ifdef SLOW_CAYLEY
-	// using templated version of Cayley
-	Matrix Omega = Cayley<3>(A);
-	return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
-#else
     // Mathematica closed form optimization (procrastination?) gone wild:
     const double a=A(0,0),b=A(0,1),c=A(0,2);
     const double d=A(1,0),e=A(1,1),f=A(1,2);
@@ -290,8 +284,18 @@ Vector Rot3::localCoordinates(const Rot3& T) const {
     const double y = (b * f - ce - c) * K;
     const double z = (fg - di - d) * K;
     return -2 * Vector_(3, x, y, z);
-#endif
+  } else if(mode == Rot3::SLOW_CALEY) {
-#endif
+    // Create a fixed-size matrix
+    Eigen::Matrix3d A(between(T).matrix());
+    // using templated version of Cayley
+    Matrix Omega = Cayley<3>(A);
+    return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
+  } else if(mode == Rot3::CORRECT_EXPMAP) {
+    return Logmap(between(T));
+  } else {
+    assert(false);
+    exit(1);
+  }
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3Q.cpp

@@ -150,12 +150,12 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-	Rot3 Rot3::retract(const Vector& omega) const {
+	Rot3 Rot3::retract(const Vector& omega, CoordinatesMode mode) const {
 		return compose(Expmap(omega));
 	}
 
 	/* ************************************************************************* */
-	Vector Rot3::localCoordinates(const Rot3& t2) const {
+	Vector Rot3::localCoordinates(const Rot3& t2, CoordinatesMode mode) const {
 		return Logmap(between(t2));
 	}
 

gtsam/geometry/tests/testRot3M.cpp

@@ -201,23 +201,77 @@ TEST(Rot3, log)
 }
 
 /* ************************************************************************* */
-TEST(Rot3, manifold)
+TEST(Rot3, manifold_first_order)
 {
 	Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
 	Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
 	Rot3 origin;
 
 	// log behaves correctly
-	Vector d12 = gR1.localCoordinates(gR2);
+	Vector d12 = gR1.localCoordinates(gR2, Rot3::FIRST_ORDER);
-	CHECK(assert_equal(gR2, gR1.retract(d12)));
+	CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::FIRST_ORDER)));
-	Vector d21 = gR2.localCoordinates(gR1);
+	Vector d21 = gR2.localCoordinates(gR1, Rot3::FIRST_ORDER);
-	CHECK(assert_equal(gR1, gR2.retract(d21)));
+	CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::FIRST_ORDER)));
+
+	// Check that log(t1,t2)=-log(t2,t1)
+	CHECK(assert_equal(d12,-d21));
+
+	// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
+	Vector d = Vector_(3, 0.1, 0.2, 0.3);
+	// exp(-d)=inverse(exp(d))
+	CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
+	// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
+	Rot3 R2 = Rot3::Expmap (2 * d);
+	Rot3 R3 = Rot3::Expmap (3 * d);
+	Rot3 R5 = Rot3::Expmap (5 * d);
+	CHECK(assert_equal(R5,R2*R3));
+	CHECK(assert_equal(R5,R3*R2));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, manifold_caley)
+{
+  Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
+  Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
+  Rot3 origin;
+
+  // log behaves correctly
+  Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CALEY);
+  CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CALEY)));
+  Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CALEY);
+  CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CALEY)));
+
+  // Check that log(t1,t2)=-log(t2,t1)
+  CHECK(assert_equal(d12,-d21));
+
+  // lines in canonical coordinates correspond to Abelian subgroups in SO(3)
+  Vector d = Vector_(3, 0.1, 0.2, 0.3);
+  // exp(-d)=inverse(exp(d))
+  CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
+  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
+  Rot3 R2 = Rot3::Expmap (2 * d);
+  Rot3 R3 = Rot3::Expmap (3 * d);
+  Rot3 R5 = Rot3::Expmap (5 * d);
+  CHECK(assert_equal(R5,R2*R3));
+  CHECK(assert_equal(R5,R3*R2));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, manifold_expmap)
+{
+  Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
+  Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
+  Rot3 origin;
+
+  // log behaves correctly
+  Vector d12 = gR1.localCoordinates(gR2, Rot3::CORRECT_EXPMAP);
+  CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CORRECT_EXPMAP)));
+  Vector d21 = gR2.localCoordinates(gR1, Rot3::CORRECT_EXPMAP);
+  CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CORRECT_EXPMAP)));
 
-#ifdef CORRECT_ROT3_EXMAP
   // Check that it is expmap
   CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
   CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
-#endif
 
   // Check that log(t1,t2)=-log(t2,t1)
   CHECK(assert_equal(d12,-d21));
@@ -498,8 +552,6 @@ TEST(Rot3, quaternion) {
   EXPECT(assert_equal(expected2, actual2));
 }
 
-#endif
-
 /* ************************************************************************* */
 TEST( Rot3, Cayley ) {
 	Matrix A = skewSymmetric(1,2,-3);
@@ -508,6 +560,8 @@ TEST( Rot3, Cayley ) {
   EXPECT(assert_equal(A, Cayley(Q)));
 }
 
+#endif
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;