How to calculate roll, yaw and pitch angles from 3D co-ordinates (Euler Angles)

Question

I have digitized a video of a flying fly in a 3-dimensional space.

At all instants I know the x, y, and z co-oridinates of the following points on the fly's body ---

The points are my choice, and the points can be placed anywhere else, any number of times.

Now using these points I wish to know the body angles (yaw, pitch and roll) of the fly with respect to a fixed axis.

I have tried the following (which I think is wrong).

1. Using the head and the tail as a head-vector --- I calculated the angle of this vector with the Z axis. And I called it pitch. But this would work only in certain circumstances..... dont want to go into details.
2. I did the same thing as above for roll except that I used the two wing bases as a vector and then calculated the angle of that vector with the Z axis.

I am using matlab to code the above thing. I know it has to be done with Euler angles like in the figure shown here :

http://mathworld.wolfram.com/EulerAngles.html

But I dont know how to proceed with all the 3D co-ordinates that I have. A little direction would help.....

Answer 1

Find vector running from tail to head and normalise it: call the result $\hat{Z}^\prime$.

Find vector joining left wing point to right wing pointand normalise it: call the result $\hat{Y}^\prime$.

Optional: do sanity check that $\left<\hat{Y}^\prime,\,\hat{Z}^\prime\right>=0$

Now calculate $\hat{X}^\prime = \hat{Y}^\prime\times\hat{Z}^\prime$.

Gather the three vectors as column vectors into the matrix $U = \left(\hat{X}^\prime\, \hat{Y}^\prime\,\hat{Z}^\prime\right)$. This is the rotation matrix that rotates your reference $\hat{X},\,\hat{Y},\,\hat{Z}$ basis into the basis aligned to the fly.

To convert to angles, we need to calculate the axis of rotation and the angle of rotation. This is most readily done by looking at the Rodrigues formula for a general member $\exp\left(H_{3\times 3}\right)$ of $SO(3)$ "backwards"

$$\exp\left(H_{3\times 3}\right)=I_{3\times3}+\frac{\sin\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||}\,H_{3\times 3} +\frac{1-\cos\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||^2}\,H_{3\times 3}^2\tag{1}$$

where:

$$H_{3\times3} = \left(\begin{array}{ccc}0&z&-y\\-z&0&x\\y&-x&0\end{array}\right)\tag{2}$$

and

$$||H_{3\times3}||=\sqrt{x^2+y^2+y^2}\tag{3}$$

where $x,\,y,\,z$ are the components of the axis of rotation and $\sqrt{x^2+y^2+y^2}$ is the angle of rotation in radians. $H_{3\times3}$ is the member of the Lie algebra $\mathfrak{so}(3)$ that exponentiates to the rotation matrix.

So, we take the matrix $U$ you found above and compare it with (1): you can see in (1) that the skew-symmetric part is:

$$\frac{1}{2}(U-U^T) = \frac{\sin\left(||H_{3\times 3}||\right)}{||H_{3\times 3}||}\,H_{3\times 3}\tag{4}$$

and this will let you read off $H_{3\times3}$ and the rotation angle $||H_{3\times 3}||$.

Incidently, if you were doing this to build a tracking system for the fly, you would need to do something like find the least squares best fit to vectors $\hat{Z}^\prime$ and $\hat{Y}^\prime$: the raw data vectors would not quite be orthogonal owing to noisy data. Alternatively, you could choose 3 points on the fly, track these and find $\hat{Z}^\prime$ and $\hat{Y}^\prime$ as the orthogonal basis for the triangle's three sides. You wouldn't have to do least squares best fit then, but your tracking might be less accurate.