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

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

and if $a \not = 0$, then

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

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

and

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

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

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

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

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