18

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?

Nirvana
  • 1,577
  • 3
  • 19
  • 34

4 Answers4

13

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

rschwieb
  • 141,564
  • 15
  • 151
  • 364
  • 3
    A lot of good stuff in here (+1), but knowing $\cos\theta$ leaves IMHO the sign of $\sin(\theta/2)$ undetermined. The quickest fix I can think of would be to take a vector $v$ orthogonal to $u$ (any such $v$ will do), and calculate the cross-product of $v$ and $Mv$ (in that order). If that has a positive dot product with $u$, then $\theta>0$. If the dot product is negative, so is $\theta$. (A nice mental workout with the right-handed rule) – Jyrki Lahtonen Aug 12 '14 at 13:01
  • 2
    @ Jyrki Lahtonen Would you explain the reasoning for doing such steps. If it is correct then it will be a great idea coz I had seen methods with numerical issues. This is out of all such issues – Nirvana Sep 07 '14 at 13:38
4

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);
SRH
  • 41
  • 1
  • 1
    Only issue I found with this is the signs can be flipped – Azmisov May 02 '20 at 00:10
  • Upon further examination, it is just `q[3]` which is flipped. Negating last parameter in the `Quaternion(...)` constructor will give the right signs. – Azmisov May 02 '20 at 00:15
  • 1
    @Azmisov You probably just have the incorrect transposition (row-major vs. column-major). I transposed the matrix first and it fixed my sign issues. – Dan Aug 29 '20 at 10:29
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.
Damien
  • 1,396
  • 16
  • 26
1

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] $$

achille hui
  • 116,756
  • 6
  • 165
  • 318
  • Try to avoid calculating $M - M^T$. At small angles, this is close to the identity matrix and thus [catastrophic cancellation](http://en.wikipedia.org/wiki/Loss_of_significance) can become a problem. – Damien Aug 12 '14 at 22:35
  • @Damien, it is true this will be numerical unstable for small $\theta$ but then this involves the minimal number of steps to extract the rotational axis. The symmetric part of $M$ is higher order in $\theta$ and even more problematic to use. – achille hui Aug 12 '14 at 22:46