RQ3 matrix decomposition

gistfile1.cpp

void rq3(const Matrix3d &A, Matrix3d &R, Matrix3d& Q)
{
    // Find rotation Qx to set A(2,1) to 0
    double c = -A(2,2)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
    double s = A(2,1)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
    Matrix3d Qx,Qy,Qz;
    Qx << 1 ,0,0, 0,c,-s, 0,s,c;
    R = A*Qx;
    // Find rotation Qy to set A(2,0) to 0
    c = R(2,2)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
    s = R(2,0)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
    Qy << c, 0, s, 0, 1, 0,-s, 0, c;
    R*=Qy;

    // Find rotation Qz to set A(1,0) to 0
    c = -R(1,1)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
    s =  R(1,0)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
    Qz << c ,-s, 0, s ,c ,0, 0, 0 ,1;
    R*=Qz;

    Q = Qz.transpose()*Qy.transpose()*Qx.transpose();
    // Adjust R and Q so that the diagonal elements of R are +ve
    // Make sure that R determinant is 1
    //        if (R.determinant() < 0)
    //            R.col(2) =-R.col(2);
    for (int n=0; n<3; n++)
    {
        if (R(n,n)<0)
        {
            R.col(n) = - R.col(n);
            Q.row(n) = - Q.row(n);
        }
    }
}

Matrix3d K; //intrinsic matrix
Matrix3d R; //extrinsic orientation
Matrix3d M; //upper-left 3x3 part of projection matrix

rq3(M, K, R);