Modernized and tested RQ and added Rot3::ypr. Note this yaw-pitch-roll is different from ML version, which is focused on cameras. Let the fun begin...

cpp/Pose3.cpp

@@ -85,7 +85,7 @@ Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
 /* ************************************************************************* */
 Vector hPose (const Vector& x) {
   Pose3 pose(x);                          // transform from vector to Pose3
-  Vector w = RQ(pose.rotation().matrix()); // get angle differences
+  Vector w = pose.rotation().ypr();       // get angle differences
   Vector d = pose.translation().vector(); // get translation differences
   return concatVectors(2,&w,&d);
 }

cpp/Rot3.cpp

@@ -67,6 +67,22 @@ namespace gtsam {
   			r3_.x(), r3_.y(), r3_.z());
   	}
 
+	/* ************************************************************************* */
+	Point3 Rot3::unrotate(const Point3& p) const {
+		return Point3(
+				r1_.x() * p.x() + r1_.y() * p.y() + r1_.z() * p.z(),
+				r2_.x() * p.x() + r2_.y() * p.y() + r2_.z() * p.z(),
+				r3_.x() * p.x() + r3_.y() * p.y() + r3_.z() * p.z()
+				);
+	}
+
+	/* ************************************************************************* */
+  Vector Rot3::ypr() const {
+  	Matrix I;Vector q;
+  	boost::tie(I,q)=RQ(matrix());
+  	return q;
+  }
+
 	/* ************************************************************************* */
 	Rot3 rodriguez(const Vector& n, double t) {
 		double n0 = n(0), n1=n(1), n2=n(2);
@@ -136,64 +152,40 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	/** This function receives a rotation 3 by 3 matrix and returns 3 rotation angles.
+	pair<Matrix,Vector> RQ(const Matrix& A) {
-	 *  The implementation is based on the algorithm in multiple view geometry
+		double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22);
-	 *  the function returns a vector that its arguments are: thetax, thetay, thetaz in radians.
+		double Cx =  A22 / a; //cosX
-	 */
+		double Sx = -A21 / a; //sinX
-	/* ************************************************************************* */
+		Matrix Qx = Matrix_(3, 3,
-	Vector RQ(Matrix R) {
+				1.0, 0.0, 0.0,
-		double Cx = R(2, 2) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow(
+				0.0,  Cx, -Sx,
-				(double) (R(2, 1)), 2.0)))); //cosX
+				0.0,  Sx, Cx);
-		double Sx = -R(2, 1) / (double) ((sqrt(pow((double) (R(2, 2)), 2.0) + pow(
+		Matrix B = A * Qx;
-				(double) (R(2, 1)), 2.0)))); //sinX
-		Matrix Qx(3, 3);
-		for (int i = 0; i < 3; i++)
-			for (int j = 0; j < 3; j++)
-				Qx(i, j) = 0;
 
-		Qx(0, 0) = 1;
+		double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22);
-		Qx(1, 1) = Cx;
+		double Cy = B22 / b; //cosY
-		Qx(1, 2) = -Sx;
+		double Sy = B20 / b; //sinY
-		Qx(2, 1) = Sx;
+		Matrix Qy = Matrix_(3,3,
-		Qx(2, 2) = Cx;
+				 Cy, 0.0,  Sy,
-		R = R * Qx;
+				0.0, 1.0, 0.0,
-		double Cy = R(2, 2) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R(
+				-Sy, 0.0,  Cy);
-				2, 0)), 2.0))); //cosY
+		Matrix C = B * Qy;
-		double Sy = R(2, 0) / (sqrt(pow((double) (R(2, 2)), 2.0) + pow((double) (R(
-				2, 0)), 2.0))); //sinY
-		Matrix Qy(3, 3);
-		for (int i = 0; i < 3; i++)
-			for (int j = 0; j < 3; j++)
-				Qy(i, j) = 0;
 
-		Qy(0, 0) = Cy;
+		double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11);
-		Qy(0, 2) = Sy;
+		double Cz =  C11 / c; //cosZ
-		Qy(1, 1) = 1;
+		double Sz = -C10 / c; //sinZ
-		Qy(2, 0) = -Sy;
+		Matrix Qz = Matrix_(3, 3,
-		Qy(2, 2) = Cy;
+				 Cz, -Sz, 0.0,
-		R = R * Qy;
+				 Sz,  Cz, 0.0,
-		double Cz = R(1, 1) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow((double) (R(
+				0.0, 0.0, 1.0);
-				1, 0)), 2.0))); //cosZ
+		Matrix R = C * Qz;
-		double Sz = -R(1, 0) / (sqrt(pow((double) (R(1, 1)), 2.0) + pow(
-				(double) (R(1, 0)), 2.0)));//sinZ
-		Matrix Qz(3, 3);
-		for (int i = 0; i < 3; i++)
-			for (int j = 0; j < 3; j++)
-				Qz(i, j) = 0;
-		Qz(0, 0) = Cz;
-		Qz(0, 1) = -Sz;
-		Qz(1, 0) = Sz;
-		Qz(1, 1) = Cz;
-		Qz(2, 2) = 1;
-		R = R * Qz;
-		double pi = atan2(sqrt(2.0) / 2.0, sqrt(2.0) / 2.0) * 4.0;
 
-		Vector result(3);
+		Vector angles(3);
-		result(0) = -atan2(Sx, Cx);
+		angles(0) = -atan2(Sx, Cx);
-		result(1) = -atan2(Sy, Cy);
+		angles(1) = -atan2(Sy, Cy);
-		result(2) = -atan2(Sz, Cz);
+		angles(2) = -atan2(Sz, Cz);
 
-		return result;
+		return make_pair(R,angles);
 	}
 
 /* ************************************************************************* */

cpp/Rot3.h

@@ -91,13 +91,10 @@ namespace gtsam {
     }
 
     /** rotate from world to rotated = R'*p */
-    Point3 unrotate(const Point3& p) const {
+    Point3 unrotate(const Point3& p) const;
-			return Point3(
+
-					r1_.x() * p.x() + r1_.y() * p.y() + r1_.z() * p.z(),
+    /** use RQ to calculate yaw-pitch-roll angle representation */
-					r2_.x() * p.x() + r2_.y() * p.y() + r2_.z() * p.z(),
+    Vector ypr() const;
-					r3_.x() * p.x() + r3_.y() * p.y() + r3_.z() * p.z()
-					);
-		}
 
     /** friends */
     friend Matrix Dunrotate1(const Rot3& R, const Point3& p);
@@ -162,7 +159,16 @@ namespace gtsam {
   Matrix Dunrotate1(const Rot3& R, const Point3& p);
   Matrix Dunrotate2(const Rot3& R); // does not depend on p !
 
-  /** receives a rotation 3 by 3 matrix and returns 3 rotation angles.*/
+	/**
-  Vector RQ(Matrix R);
+	 * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
+	 * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
+	 * such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
+	 * be the identity and Q is a yaw-pitch-roll decomposition of A.
+	 * The implementation uses Givens rotations and is based on Hartley-Zisserman.
+	 * @param a 3 by 3 matrix A=RQ
+	 * @return an upper triangular matrix R
+	 * @return a vector [thetax, thetay, thetaz] in radians.
+	 */
+  std::pair<Matrix,Vector> RQ(const Matrix& A);
 
 }

cpp/testRot3.cpp

@@ -95,6 +95,19 @@ TEST( Rot3, rodriguez3) {
   CHECK(assert_equal(R1,R2));
 }
 
+/* ************************************************************************* */
+//TEST(Rot3, manifold) {
+//	Rot3 t1 = rodriguez(0.1,0.4,0.2);
+//	Rot3 t2 = rodriguez(0.3,0.1,0.7);
+//	Rot3 origin;
+//	Vector d12 = t1.log(t2);
+//	CHECK(assert_equal(t2, t1.exmap(d12)));
+//	CHECK(assert_equal(t2, origin.exmap(d12)*t1));
+//	Vector d21 = t2.log(t1);
+//	CHECK(assert_equal(t1, t2.exmap(d21)));
+//	CHECK(assert_equal(t1, origin.exmap(d21)*t2));
+//}
+
 /* ************************************************************************* */
 TEST( Rot3, exmap)
 {
@@ -121,8 +134,6 @@ TEST( Rot3, Drotate2_DNrotate2)
 }
 
 /* ************************************************************************* */
-// unrotate 
-
 TEST( Rot3, unrotate)
 {
   Point3 w = R*P;
@@ -146,6 +157,32 @@ TEST( Rot3, Dunrotate2_DNunrotate2)
   CHECK(assert_equal(numerical,computed,error));
 }
 
+/* ************************************************************************* */
+TEST( Rot3, RQ)
+{
+	// Try RQ on a pure rotation
+	Matrix actualK; 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(expected,actual,1e-6));
+
+  // Try using ypr call
+  actual = R.ypr();
+  CHECK(assert_equal(expected,actual,1e-6));
+
+  // Try RQ to recover calibration from 3*3 sub-block of projection matrix
+	Matrix K = Matrix_(3,3,
+			500.0,   0.0, 320.0,
+			  0.0, 500.0, 240.0,
+			  0.0,   0.0,   1.0
+			);
+	Matrix A = K*R.matrix();
+  boost::tie(actualK,actual) = RQ(A);
+  CHECK(assert_equal(K,actualK));
+  CHECK(assert_equal(expected,actual,1e-6));
+}
+
 /* ************************************************************************* */
 int main(){ TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */