Implemented and tested Rot3.retract based on the Cayley Transform (about three times faster)

gtsam/geometry/Rot3.h

@@ -227,10 +227,10 @@ namespace gtsam {
     size_t dim() const { return dimension; }
 
     /// Retraction from R^3 to Pose2 manifold neighborhood around current pose
-    Rot3 retract(const Vector& v) const { return compose(Expmap(v)); }
+    Rot3 retract(const Vector& omega) const;
 
     /// Returns inverse retraction
-    Vector localCoordinates(const Rot3& t2) const { return Logmap(between(t2)); }
+    Vector localCoordinates(const Rot3& t2) const;
 
     /// @}
     /// @name Lie Group

gtsam/geometry/Rot3M.cpp

@@ -236,6 +236,38 @@ Vector Rot3::Logmap(const Rot3& R) {
   }
 }
 
+/* ************************************************************************* */
+Rot3 Rot3::retract(const Vector& omega) const {
+#ifdef CORRECT_ROT3_EXMAP
+	return (*this)*Expmap(omega);
+#else
+#ifdef SLOW_CAYLEY
+	Matrix Omega = skewSymmetric(omega);
+	return (*this)*Cayley(-Omega/2);
+#else
+	double x = omega(0), y = omega(1), z = omega(2);
+	double x2 = x*x, y2 = y*y, z2 = z*z;
+	double xy = x*y, xz = x*z, yz = y*z;
+	double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0*f;
+	return (*this)* Rot3(
+			(4+x2-y2-z2)*f, (xy - 2*z)*_2f, (xz + 2*y)*_2f,
+			(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
+#endif
+}
+
+/* ************************************************************************* */
+Vector Rot3::localCoordinates(const Rot3& T) const {
+#ifdef CORRECT_ROT3_EXMAP
+	return Logmap(between(T));
+#else
+	Matrix Omega = Cayley(between(T).matrix());
+	return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
+#endif
+}
+
 /* ************************************************************************* */
 Matrix Rot3::matrix() const {
 	Matrix R(3,3);

gtsam/geometry/Rot3Q.cpp

@@ -149,6 +149,16 @@ namespace gtsam {
     return angleAxis.axis() * angleAxis.angle();
   }
 
+  /* ************************************************************************* */
+	Rot3 Rot3::retract(const Vector& omega) const {
+		return compose(Expmap(omega));
+	}
+
+	/* ************************************************************************* */
+	Vector Rot3::localCoordinates(const Rot3& t2) const {
+		return Logmap(between(t2));
+	}
+
   /* ************************************************************************* */
   Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
 

gtsam/geometry/tests/testPose3.cpp

@@ -49,13 +49,13 @@ TEST( Pose3, retract)
   Pose3 id;
   Vector v = zero(6);
   v(0) = 0.3;
-  EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v)));
+  EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),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),1e-5));
+  EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
 }
 
 /* ************************************************************************* */
@@ -86,7 +86,7 @@ TEST(Pose3, expmap_b)
   Pose3 p1(Rot3(), Point3(100, 0, 0));
   Pose3 p2 = p1.retract(Vector_(6,0.0, 0.0, 0.1,  0.0, 0.0, 0.0));
   Pose3 expected(Rot3::rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
-  EXPECT(assert_equal(expected, p2));
+  EXPECT(assert_equal(expected, p2,1e-2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3M.cpp

@@ -30,11 +30,12 @@ using namespace gtsam;
 Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 Point3 P(0.2, 0.7, -2.0);
 double error = 1e-9, epsilon = 0.001;
+static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
 TEST( Rot3, constructor)
 {
-	Rot3 expected(eye(3, 3));
+	Rot3 expected(I3);
 	Vector r1(3), r2(3), r3(3);
 	r1(0) = 1;
 	r1(1) = 0;
@@ -88,7 +89,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
 	double t = norm_2(w);
 	Matrix J = skewSymmetric(w / t);
 	if (t < 1e-5) return Rot3();
-	Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
+	Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
 	return R;
 }
 
@@ -189,10 +190,14 @@ TEST(Rot3, manifold)
 	// log behaves correctly
 	Vector d12 = gR1.localCoordinates(gR2);
 	CHECK(assert_equal(gR2, gR1.retract(d12)));
-	CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
 	Vector d21 = gR2.localCoordinates(gR1);
 	CHECK(assert_equal(gR1, gR2.retract(d21)));
+
+#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));
@@ -381,7 +386,7 @@ TEST( Rot3, RQ)
 	Vector actual;
 	boost::tie(actualK, actual) = RQ(R.matrix());
 	Vector expected = Vector_(3, 0.14715, 0.385821, 0.231671);
-	CHECK(assert_equal(eye(3),actualK));
+	CHECK(assert_equal(I3,actualK));
 	CHECK(assert_equal(expected,actual,1e-6));
 
 	// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
@@ -415,7 +420,7 @@ TEST( Rot3, expmapStability ) {
                           w(2), 0.0, -w(0),
                           -w(1), w(0), 0.0 );
   Matrix W2 = W*W;
-  Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
+  Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
       - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
   Rot3 expectedR( Rmat );
   CHECK(assert_equal(expectedR, actualR, 1e-10));
@@ -475,6 +480,14 @@ TEST(Rot3, quaternion) {
 
 #endif
 
+/* ************************************************************************* */
+TEST( Rot3, Cayley ) {
+	Matrix A = skewSymmetric(1,2,-3);
+	Matrix Q = Cayley(A);
+  EXPECT(assert_equal(I3, trans(Q)*Q));
+  EXPECT(assert_equal(A, Cayley(Q)));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;

gtsam/geometry/tests/timeRot3.cpp

@@ -27,8 +27,9 @@ int main()
 {
   int n = 300000;
   Vector v = Vector_(3,1.,0.,0.);
+  Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 
-  // Rodriguez formula given axis angle
+  cout << "Rodriguez formula given axis angle" << endl;
   long timeLog = clock();
   for(int i = 0; i < n; i++)
   	Rot3::rodriguez(v,0.001);
@@ -37,7 +38,7 @@ int main()
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
 
-  // Rodriguez formula given canonical coordinates
+  cout << "Rodriguez formula given canonical coordinates" << endl;
   timeLog = clock();
   for(int i = 0; i < n; i++)
   	Rot3::rodriguez(v);
@@ -46,7 +47,25 @@ int main()
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
 
-  // Slow rotation matrix
+  cout << "Exmpap" << endl;
+  timeLog = clock();
+  for(int i = 0; i < n; i++)
+  	R*Rot3::Expmap(v);
+  timeLog2 = clock();
+  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << seconds << " seconds" << endl;
+  cout << ((double)n/seconds) << " calls/second" << endl;
+
+  cout << "Retract" << endl;
+  timeLog = clock();
+  for(int i = 0; i < n; i++)
+  	R.retract(v);
+  timeLog2 = clock();
+  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << seconds << " seconds" << endl;
+  cout << ((double)n/seconds) << " calls/second" << endl;
+
+  cout << "Slow rotation matrix" << endl;
   timeLog = clock();
   for(int i = 0; i < n; i++)
   	Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
@@ -55,7 +74,7 @@ int main()
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
 
-  // Fast Rotation matrix
+  cout << "Fast Rotation matrix" << endl;
   timeLog = clock();
   for(int i = 0; i < n; i++)
   	Rot3::RzRyRx(0.1,0.2,0.3);