Conversion of rotation matrix to quaternion

The axis and angle are directly coded in this matrix.

Compute the unit eigenvector for the eigenvalue $1$ for this matrix (it must exist!) and call it $u=(u_1,u_2,u_3)$. You will be writing it as $u=u_1i+u_2j+u_2k$ from now on. This is precisely the axis of rotation, which, geometrically, all nonidentity rotations have.

You can recover the angle from the trace of the matrix: $tr(M)=2\cos(\theta)+1$. This is a consequence of the fact that you can change basis to an orthnormal basis including the axis you found above, and the rotation matrix will be the identity on that dimension, and it will be a planar rotation on the other two dimensions. That is, it will have to be of the form

$$\begin{bmatrix}\cos(\theta)&-\sin(\theta)&0\\\sin(\theta)&\cos(\theta)&0\\0&0&1\end{bmatrix}$$

Since the trace is invariant between changes of basis, you can see how I got my equation.

Once you've solved for $\theta$, you'll use it to construct your rotation quaternion $q=\cos(\theta/2)+u\sin(\theta/2)$.

Reference: Shuster, M. 1993, "A Survey of Attitude Representations", Journal of the Astronautical Sciences, 41(4):349-517

See equations and discussion in the paper above, p463-464.

One of the quaternion elements is guaranteed to have a magnitude of greater than 0.5 and hence a squared value of 0.25. We can use this to determine the "best" set of parameters to use to calculate the quaternion from a rotation matrix

double b1_squared = 0.25 * (1.0 + R11 + R22 + R33);
if (b1_squared >= 0.25)
{
    // Equation (164)
    double b1 = sqrt(b1_squared);

    double over_b1_4 = 0.25 / b1;
    double b2 = (R32 - R23) * over_b1_4;
    double b3 = (R13 - R31) * over_b1_4;
    double b4 = (R21 - R12) * over_b1_4;

    // Return the quaternion
    Eigen::Vector4d q(b1, b2, b3, b4);
    return q;
}

I will leave it as an exercise to the OP to fill out the other three.

See also:

• Farrell, 2008, "Aided Navigation: GPS with High Rate Sensors", McGraw Hill, Appendix D.

there is a solution in https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf

in summary, it's final code is as below:

if (m22 < 0) {
    if (m00 >m11) {
        t = 1 + m00 -m11 -m22;
        q = Quaternion( t, m01+m10, m20+m02, m12-m21 );
    }
    else {
        t = 1 -m00 + m11 -m22;
        q = Quaternion( m01+m10, t, m12+m21, m20-m02 );
    }
}
else {
    if (m00 < -m11) {
        t = 1 -m00 -m11 + m22;
        q = Quaternion( m20+m02, m12+m21, t, m01-m10 );
    }
    else {
        t = 1 + m00 + m11 + m22;
        q = Quaternion( m12-m21, m20-m02, m01-m10, t );
    }
}
q *= 0.5 / Sqrt(t);