refactored and thoroughly checked rodrgues, added two more unit test

cpp/Rot3.cpp

@@ -137,23 +137,26 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 rodriguez(const Vector& n, double t) {
+  Rot3 rodriguez(const Vector& w, double theta) {
-    double n0 = n(0), n1=n(1), n2=n(2);
+  	// get components of axis \omega
-    double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
+    double wx = w(0), wy=w(1), wz=w(2);
+    double w2_xx = wx*wx, w2_yy = wy*wy, w2_zz = wz*wz;
 #ifndef NDEBUG
-    double l_n = n00+n11+n22;
+    double l_n = w2_xx + w2_yy + w2_zz;
     if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
 #endif
 
-    double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
+    double c = cos(theta), s = sin(theta), c_1 = 1 - c;
 
-    double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
+    double swx = wx * s, swy = wy * s, swz = wz * s;
-    double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
+    double C00 = c_1*w2_xx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
-    double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
+    double                  C11 = c_1*w2_yy, C12 = c_1*wy*wz;
+    double                                   C22 = c_1*w2_zz;
 
-    Point3 r1 = Point3( ct + C00,  s2 + C01, -s1 + C02);
+    // Important: these are columns, so this reads transposed !!!!
-    Point3 r2 = Point3(-s2 + C01,  ct + C11,  s0 + C12);
+    Point3 r1 = Point3(   c + C00,  swz + C01, -swy + C02);
-    Point3 r3 = Point3( s1 + C02, -s0 + C12,  ct + C22);
+    Point3 r2 = Point3(-swz + C01,    c + C11,  swx + C12);
+    Point3 r3 = Point3( swy + C02, -swx + C12,    c + C22);
 
     return Rot3(r1, r2, r3);
   }

cpp/testRot3.cpp

@@ -68,12 +68,13 @@ TEST( Rot3, equals)
 }
 
 /* ************************************************************************* */
+// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
 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, 3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
+	Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
-	return R; // matrix constructor will be tripped
+	return R;
 }
 
 /* ************************************************************************* */
@@ -82,19 +83,19 @@ TEST( Rot3, rodriguez)
 	Rot3 R1 = rodriguez(epsilon, 0, 0);
 	Vector w = Vector_(3, epsilon, 0., 0.);
 	Rot3 R2 = slow_but_correct_rodriguez(w);
-	CHECK(assert_equal(R1,R2));
+	CHECK(assert_equal(R2,R1));
 }
 
 /* ************************************************************************* */
 TEST( Rot3, rodriguez2)
 {
-	Vector v(3);
+	Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
-	v(0) = 0;
+	double angle = 3.14 / 4.0;
-	v(1) = 1;
+	Rot3 actual = rodriguez(axis, angle);
-	v(2) = 0;
+	Rot3 expected(0.707388, 0, 0.706825,
-	Rot3 R1 = rodriguez(v, 3.14 / 4.0);
+			                 0, 1,        0,
-	Rot3 R2(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388);
+			         -0.706825, 0, 0.707388);
-	CHECK(assert_equal(R1,R2,1e-5));
+	CHECK(assert_equal(expected,actual,1e-5));
 }
 
 /* ************************************************************************* */
@@ -103,7 +104,21 @@ TEST( Rot3, rodriguez3)
 	Vector w = Vector_(3, 0.1, 0.2, 0.3);
 	Rot3 R1 = rodriguez(w / norm_2(w), norm_2(w));
 	Rot3 R2 = slow_but_correct_rodriguez(w);
-	CHECK(assert_equal(R1,R2));
+	CHECK(assert_equal(R2,R1));
+}
+
+/* ************************************************************************* */
+TEST( Rot3, rodriguez4)
+{
+	Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
+	double angle = M_PI_2;
+	Rot3 actual = rodriguez(axis, angle);
+	double c=cos(angle),s=sin(angle);
+	Rot3 expected(c,-s, 0,
+			          s, c, 0,
+			          0, 0, 1);
+	CHECK(assert_equal(expected,actual,1e-5));
+	CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
 }
 
 /* ************************************************************************* */