Much faster compund rotation using Justin's (indeed correct) formula

cpp/Rot3.cpp

@@ -43,6 +43,29 @@ namespace gtsam {
   			 0,  0, 1);
   }
 
+  // Considerably faster than composing matrices above !
+  Rot3 Rot3::RzRyRx(double x, double y, double z) {
+  	double cx=cos(x),sx=sin(x);
+  	double cy=cos(y),sy=sin(y);
+  	double cz=cos(z),sz=sin(z);
+  	double ss_ = sx * sy;
+  	double cs_ = cx * sy;
+  	double sc_ = sx * cy;
+  	double cc_ = cx * cy;
+  	double c_s = cx * sz;
+  	double s_s = sx * sz;
+		double _cs = cy * sz;
+  	double _cc = cy * cz;
+		double s_c = sx * cz;
+		double c_c = cx * cz;
+		double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
+  	return Rot3(
+  			_cc,- c_s + ssc,  s_s + csc,
+  			_cs,  c_c + sss, -s_c + css,
+				-sy,        sc_,        cc_
+  			);
+  }
+
   /* ************************************************************************* */
   bool Rot3::equals(const Rot3 & R, double tol) const {
     return equal_with_abs_tol(matrix(), R.matrix(), tol);

cpp/Rot3.h

@@ -63,19 +63,17 @@ namespace gtsam {
     static Rot3 Rx(double t);
     static Rot3 Ry(double t);
     static Rot3 Rz(double t);
-    static Rot3 RzRyRx(double x, double y, double z) { return Rz(z)*Ry(y)*Rx(x);}
+    static Rot3 RzRyRx(double x, double y, double z);
 
     /**
      * Tait-Bryan system from Spatial Reference Model (SRM) (x,y,z) = (roll,pitch,yaw)
      * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf
-     * Differs from one defined by Justin, who uses, (x,y,z) = (roll,-pitch,yaw)
-     * and http://en.wikipedia.org/wiki/Yaw,_pitch,_and_roll, where (x,y,z) = (-roll,pitch,yaw)
      * Assumes vehicle coordinate frame X forward, Y right, Z down
      */
     static Rot3 yaw  (double t) { return Rz(t);} // positive yaw is to right (as in aircraft heading)
     static Rot3 pitch(double t) { return Ry(t);} // positive pitch is up (increasing aircraft altitude)
     static Rot3 roll (double t) { return Rx(t);} // positive roll is to right (increasing yaw in aircraft)
-    static Rot3 ypr  (double y, double p, double r) { return yaw(y)*pitch(p)*roll(r);}
+    static Rot3 ypr  (double y, double p, double r) { return RzRyRx(r,p,y);}
 
     /** print */
     void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
@@ -178,22 +176,6 @@ namespace gtsam {
         R.r3().x(), R.r3().y(), R.r3().z());
   }
 
-  /**
-
-   * Update Rotation with incremental rotation
-   * @param v a vector of incremental roll,pitch,yaw
-   * @param R a rotated frame
-   * @return incremental rotation matrix
-   */
-  //Rot3 exp(const Rot3& R, const Vector& v);
-
-  /**
-   * @param a rotation R
-   * @param a rotation S
-   * @return log(S*R'), i.e. canonical coordinates of between(R,S)
-   */
-  //Vector log(const Rot3& R, const Rot3& S);
-
   /**
    * rotate point from rotated coordinate frame to 
    * world = R*p

cpp/testRot3.cpp

@@ -129,12 +129,28 @@ TEST(Rot3, log)
 	Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
 	Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
 	Rot3 origin;
+
+	// log behaves correctly
 	Vector d12 = logmap(t1, t2);
 	CHECK(assert_equal(t2, expmap(t1,d12)));
 	CHECK(assert_equal(t2, expmap<Rot3>(d12)*t1));
 	Vector d21 = logmap(t2, t1);
 	CHECK(assert_equal(t1, expmap(t2,d21)));
 	CHECK(assert_equal(t1, expmap<Rot3>(d21)*t2));
+
+	// 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(expmap<Rot3>(-d),inverse(expmap<Rot3>(d))));
+	// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
+	Rot3 R2 = expmap<Rot3>(2*d);
+	Rot3 R3 = expmap<Rot3>(3*d);
+	Rot3 R5 = expmap<Rot3>(5*d);
+	CHECK(assert_equal(R5,R2*R3));
+	CHECK(assert_equal(R5,R3*R2));
 }
 
 /* ************************************************************************* */
@@ -233,15 +249,39 @@ TEST( Rot3, xyz )
 {
 	double t = 0.1, st = sin(t), ct = cos(t);
 
-	Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
+	// Make sure all counterclockwise
+	// Diagrams below are all from from unchanging axis
+
+	// z
+	// |   * Y=(ct,st)
+	// x----y
+	Rot3 expected1(
+			1,  0,  0,
+			0, ct,-st,
+			0, st, ct);
 	CHECK(assert_equal(expected1,Rot3::Rx(t)));
 
-	Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
+	// x
+	// |   * Z=(ct,st)
+	// y----z
+	Rot3 expected2(
+			 ct, 0, st,
+			  0, 1,  0,
+			-st, 0, ct);
 	CHECK(assert_equal(expected2,Rot3::Ry(t)));
 
-	// yaw is around z axis
+	// y
-	Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
+	// |   X=* (ct,st)
+	// z----x
+	Rot3 expected3(
+			ct, -st, 0,
+			st,  ct, 0,
+			 0,  0,  1);
 	CHECK(assert_equal(expected3,Rot3::Rz(t)));
+
+	// Check compound rotation
+	Rot3 expected = Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
+	CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
 }
 
 /* ************************************************************************* */
@@ -257,6 +297,10 @@ TEST( Rot3, yaw_pitch_roll )
 
 	// roll is around x axis
 	CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
+
+	// Check compound rotation
+	Rot3 expected = Rot3::yaw(0.1)*Rot3::pitch(0.2)*Rot3::roll(0.3);
+	CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
 }
 
 /* ************************************************************************* */

cpp/timeRot3.cpp

@@ -14,9 +14,10 @@ using namespace gtsam;
 
 int main()
 {
-  int n = 3000000;
+  int n = 300000;
   Vector v = Vector_(3,1.,0.,0.);
 
+  // Rodriguez formula given axis angle
   long timeLog = clock();
   for(int i = 0; i < n; i++)
   	rodriguez(v,0.001);
@@ -25,6 +26,7 @@ int main()
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
 
+  // Rodriguez formula given canonical coordinates
   timeLog = clock();
   for(int i = 0; i < n; i++)
   	rodriguez(v);
@@ -33,5 +35,23 @@ int main()
   cout << seconds << " seconds" << endl;
   cout << ((double)n/seconds) << " calls/second" << endl;
 
+  // Slow rotation matrix
+  timeLog = clock();
+  for(int i = 0; i < n; i++)
+  	Rot3::Rz(0.3)*Rot3::Ry(0.2)*Rot3::Rx(0.1);
+  timeLog2 = clock();
+  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << seconds << " seconds" << endl;
+  cout << ((double)n/seconds) << " calls/second" << endl;
+
+  // Fast Rotation matrix
+  timeLog = clock();
+  for(int i = 0; i < n; i++)
+  	Rot3::RzRyRx(0.1,0.2,0.3);
+  timeLog2 = clock();
+  seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+  cout << seconds << " seconds" << endl;
+  cout << ((double)n/seconds) << " calls/second" << endl;
+
   return 0;
 }