Conversion of rotation matrix to quaternion

We use unit length Quaternion to represent rotations. Following is a general rotation matrix obtained ${\begin{bmatrix}m_{00} & m_{01}&m_{02} \\ m_{10} & m_{11}&m_{12}\\ m_{20} & m_{21}&m_{22}\end{bmatrix}}_{3\times 3}\tag 1 $.

How do I accurately calculate quaternion $q = q_1i+q_2j+q_3k+q_4$for this matrix?Means how can we write $q_i$s interms of given $m_{ij}$ accurately?

Solution 1:

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)$.

Solution 2:

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);

Solution 3:

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.

Solution 4:

Let $\mathcal{A} \subset \mathcal{M}_{3\times 3}(\mathbb{R})$ be the space of $3 \times 3$ real skew symmetric matrices. It is span by three matrices

$$L_x = \begin{bmatrix}0 & 0 & 0\\0 & 0 & -1\\0 & 1 & 0\end{bmatrix},\quad L_y = \begin{bmatrix}0 & 0 & 1\\0 & 0 & 0\\-1& 0 & 0\end{bmatrix},\quad L_z = \begin{bmatrix}0 & -1 & 0\\1 & 0 & 0\\0 & 0 & 0\end{bmatrix} $$ For each $A \in \mathcal{A}$, we can expand $A$ as $x L_x + y L_y + z L_z$ and associated a vector $\vec{A} \in \mathbb{R}^3$ and a quaternion $\tilde{A} \in \mathbb{H}$ to it.

$$A = x L_x + y L_y + z L_z \quad\leftrightarrow\quad \vec{A} = (x,y,z) \quad\leftrightarrow\quad \tilde{A} = x{\bf i} + y{\bf j} + z{\bf k}$$

Let $\mathcal{A}_u = \{\; A \in \mathcal{A} : |\vec{A}| = 1\; \}$. Given any rotation matrix $M \in SO(3)$, we can find a $\theta \in [0,\pi]$ and $L \in \mathcal{A}_u$ such that

$$M = e^{\theta L} = I_3 + \sin\theta L + (1-\cos\theta) L^2$$

The $\theta$ is the angle of rotation associated with $M$ and $\vec{L}$ will be a unit vector in the direction of the rotational axis.

When $\theta \ne 0$ nor $\pi$, we can extract $L$ out by taking the antisymmetric part of $M$ and then "normalize" the corresponding vector because

$$ \sin\theta L = \frac12 \left(M - M^T\right)$$

To fix the value of $\theta$, we can use the relation $\text{Tr}(M) = 1 + 2\cos\theta$.

Once $\theta$ and $L$ is known, the quaternion corresponding to the rotation matrix $M$ is then given by

$$e^{\frac{\theta}{2} \tilde{L}} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2} \tilde{L} = \frac{\sqrt{1+\text{Tr}(M)}}{2}\left[ 1 + \frac{\widetilde{M - M^{T}}}{1 + \text{Tr}(M)}\right] $$