Quaternion Conversion

How do we convert from quaternions to a Rotation Matrix or Rotation Vectors? The key to this is writing quaternion multiplication as a matrix multiplication.

Let $\mathbf{q}=[s,\mathbf{v}{]}^T$, and we can define ${}^{+}$ and ^\bigoplus as:

$${\mathbf{q}}^{+}=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right],{\mathbf{q}}^{⨁}=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}-{\mathbf{v}}^{\wedge }\end{array}\right]$$

These two symbols map the quaternion to a $4\times 4$ matrix. Then, quaternion multiplication can be written in the form of a matrix:

$${\mathbf{q}}_1^{+}{\mathbf{q}}_2=\left[\begin{array}{cc}{s}_1& -{\mathbf{v}}_1^T\\ {\mathbf{v}}_1& s_1\mathbf{I}+{\mathbf{v}}_1^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_2\\ {\mathbf{v}}_2\end{array}\right]=\left[\begin{array}{c}-{\mathbf{v}}_1^T{\mathbf{v}}_2+s_1{s}_2\\ s_1{\mathbf{v}}_2+s_2{\mathbf{v}}_1+{\mathbf{v}}_1^{\wedge }{\mathbf{v}}_2\end{array}\right]={\mathbf{q}}_1{\mathbf{q}}_2$$

The left side is matrix multiplication and the right side is quaternion multiplication. Similarly, for ${}^{⨁}$, we also get:

$${\mathbf{q}}_1{\mathbf{q}}_2={\mathbf{q}}_1^{+}{\mathbf{q}}_2={\mathbf{q}}_2^{⨁}{\mathbf{q}}_1$$

Then, consider the problem of using a quaternion to rotate a spatial point. According to the previous section, we have:

\mathbf{p}' = \mathbf{q}\mathbf{p}\mathbf{q}^{-1} = \mathbf{q}^{+}\mathbf{p}^{+}\mathbf{q}^{-1} = \mathbf{q^{+}\mathbf{q}^{-1^\bigoplus}\mathbf{p}}

Substituting the matrix corresponding to two symbols, we get:

$${\mathbf{q}}^{+}({\mathbf{q}}^{-1}{)}^{⨁}=\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^T\\ -\mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0^T& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2\end{array}\right]$$

Since ${\mathbf{p}}^{\prime }$ and $\mathbf{p}$ are both imaginary quaternions, so in fact that the bottom right corner of the matrix gives the transformation formula from quaternion to rotation matrix:

$$\boxed{\mathbf{R}=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2}$$

In order to obtain the conversion formula of the quaternion to the rotation vector, we take the trace on both sides of the above formula:

$$\begin{array}{rl}\text{tr}(\mathbf{R})& =\text{tr}(\mathbf{v}{\mathbf{v}}^T)+3s^2+2s\cdot 0+\text{tr}(({\mathbf{v}}^{\wedge }{)}^2)\\ & =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ & =(1-s^2)+3s^2-2(1-s^2)\\ & =4s^2-1\end{array}$$

Also obtained by the form:

$$\begin{array}{rl}\theta & =\arccos \left(\frac{\text{tr}(\mathbf{R})-1}{2}\right)\\ & =\arccos (2s^2-1)\end{array}$$

so:

$$\begin{array}{rl}\cos \theta & =2s^2-1=2{\cos }^2\frac{\theta }{2}-1\\ \theta & =2\arccos s\end{array}$$

For the rotation matrix, if we replace $\mathbf{p}$ with the imaginary part of $\mathbf{q}$ in the formula, it’ easy to know the imaginary part of $\mathbf{q}$ is not moving when it is rotated; that is, it constitutes exactly the rotation axis. So we get the rotation matrix just by the normalizing $\mathbf{q}$‘s imaginary part. In summary, the conversion formula from quaternion to rotation vector can be written as:

$$\{\begin{array}{l}\theta =2\arccos q_0\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}$$

To convert from other representations, we just need to reverse the above steps.