Finding quaternion parameters from a rotation matrix

Recall from Section 3.2.3 that given a rotation matrix (3.43), the yaw, pitch, and roll parameters could be directly determined using the $ \atan2$ function. It turns out that the quaternion representation can also be determined directly from the matrix. This is the inverse of the function in (4.20).4.9

For a given rotation matrix (3.43), the quaternion parameters $ h = a + b i + c j + d
k$ can be computed as follows [210]. The first component is

$\displaystyle a = \begin{matrix}\frac{1}{2} \end{matrix} \sqrt{r_{11} + r_{22} + r_{33} + 1} ,$ (4.24)

and if $ a \not = 0 $, then

$\displaystyle b = {r_{32} - r_{23} \over 4 a},$ (4.25)

$\displaystyle c = {r_{13} - r_{31} \over 4 a},$ (4.26)

and

$\displaystyle d = {r_{21} - r_{12} \over 4 a}.$ (4.27)

If $ a = 0$, then the previously mentioned equator problem occurs. In this case,

$\displaystyle b = {r_{13} r_{12} \over \sqrt{r_{12}^2 r_{13}^2 + r_{12}^2 r_{23}^2 + r_{13}^2 r_{23}^2}},$ (4.28)

$\displaystyle c = {r_{12} r_{23} \over \sqrt{r_{12}^2 r_{13}^2 + r_{12}^2 r_{23}^2 + r_{13}^2 r_{23}^2}},$ (4.29)

and

$\displaystyle d = {r_{13} r_{23} \over \sqrt{r_{12}^2 r_{13}^2 + r_{12}^2 r_{23}^2 + r_{13}^2 r_{23}^2}}.$ (4.30)

This method fails if $ r_{12} = r_{23} = 0$ or $ r_{13} = r_{23} =
0$ or $ r_{12} = r_{23} = 0$. These correspond precisely to the cases in which the rotation matrix is a yaw, (3.39), pitch, (3.40), or roll, (3.41), which can be detected in advance.

Steven M LaValle 2012-04-20