Pitch, Yaw, Roll calculation

Question

I am struggling with calculation of yaw, pitch, roll rotation of a plane on 3D. Let's say that I have a plane with 3 points with known coordinates. After rotating this plane in space in 3 ways: roll, pitch, yaw, I will have a new plane. In case, I know the coordinates of these 3 points before and after rotation. How can I calculate roll, yaw, pitch. Thank you

Answer 1

It is a property of rotation in $3D$ that the axis of rotation lies in the plane that is perpendicular to the straight line segment connecting the initial point and the final point, and that passes through the midpoint of this line segment.

So if you have three points $A, B, C$ and their images $A', B', C'$, then you can find the axis of rotation, by finding the equation of the perpendicular bisecting planes to ${AA}', {BB'}$ and ${CC}'$, the intersection of any two of these three planes defines the axis of rotation $a$.

The angle of rotation can be found by projecting $A$ and $A'$ onto a plane that is perpendicular to the axis, and then find the angle between the two projections.

Once the axis of rotation and the angle of rotation are found, then the rotation matrix that describes the rotation that occured, can be found from the Rodrigues' Rotation Matrix Formula, which in one of its forms gives

$ R = {a a}^T + (I - {aa}^T ) \cos \theta + S_a \sin \theta $

where $a$ is the normalized vector pointing along the axis as found above, and the $\theta$ is the angle of rotation. And,

$S_a = \begin{bmatrix} 0 && - a_z && a_y \\ a_z && 0 && - a_x \\ - a_y && a_x && 0 \end{bmatrix} $

Once the rotation matrix is found, it can decomposed into the product of yaw-pitch-roll rotation matrices.

Normally, yaw rotation is about the $z$ axis, pitch is about the local $x$ axis, and roll is about the $y$ axis. Combining the three rotations, gives

$ R = R_z(\phi) R_x(\theta) R_y(\psi) $

Define $c_1 = \cos \phi, s_1 = \sin \phi $ and $c_2 = \cos \theta , s_2 \sin \theta $ and $c_3 = \cos \psi , s3 = \sin \phi $, then the above equation is

$ R = \begin{bmatrix} c_1 && -s_1 && 0 \\ s_1 && c_1 && 0 \\ 0 && 0 && 1 \end{bmatrix} \begin{bmatrix} 1 && 0 && 0 \\ 0 && c_2 && -s_2 \\ 0 && s_2 && c_2 \end{bmatrix} \begin{bmatrix} c_3 && 0 && s_3 \\ 0 && 1 && 0 \\ -s_3 && 0 && c_3 \end{bmatrix} $

This reduces to

$R = \begin{bmatrix} c_1 && -s_1 && 0 \\ s_1 && c_1 && 0 \\ 0 && 0 && 1 \end{bmatrix} \begin{bmatrix} c_3 && 0 && s_3 \\s_2 s_3 && c_2 && -s_2 c_3 \\ - c_2 s_3 && s_2 && c_2 c_3 \end{bmatrix} $

And further to

$ R = \begin{bmatrix} c_1 c_3 - s_1 s_2 s_3 && - s_1 c_2 && c_1 s_3 + s_1 s_2 c_3 \\ s_1 c_3 + c_1 s_2 s_3 && c_1 c_2 && s_1 s_3 - c_1 s_2 c_3 \\ -c_2 s_3 && s_2 && c_2 c_3 \end{bmatrix} $

By comparing entries on both sides of the equality, we deduce that

$ s_2 = \sin \theta = R_{31} $

which has two solutions $ \theta_1 = \sin^{-1} (R_{32}) $ and $\theta_2 = \pi - \theta_1 $

Looking at the second column,

$\phi = \text{atan2}( R_{22} / \cos(\theta) , - R_{12} / \cos(\theta) ) $

Note that the $\text{atan2}(x,y)$ function that I am using (and which is implemented this way in many programming languages) returns the angle $a$ such that $\cos(a) = \dfrac{x}{\sqrt{x^2 + y^2}} , \sin(a) = \dfrac{y}{\sqrt{x^2 + y^2}} $. There are other implementations that flip the order the parameters.

Finally looking at the third row, we get

$ \psi = \text{atan2}( R_{33} / \cos(\theta) , - R_{31} / \cos(\theta) ) $

This shows that its possible to achieve the factorization of $R$ in only two ways.