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

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

-
Using the tail-head and wing-wing vectors is the right thing to do- you just have to change the order (and recognize there will always be certain orientations that lead to "gimbal lock". I would look down from the top, rotate the head vector about Z first (point into XZ plane), then tilt it to point along the Z axis, and finally rotate the (now pointing in a different direction) wing vector along the X axis. This gives you a unique rotation matrix from which you can get the Euler angles. –  Floris Jun 19 at 7:53

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.

-
As for the "least squares fit" - I agree the two vectors will not be (quite) orthogonal, and that least squares (or subtracting the projection of b onto a from b and renornalizing, if a is considered the reference direction) is a good approach. –  Floris Jun 19 at 11:47
@WetSavanna & Floris I'm a little confused with what you mean by "where x,y,z are the components of the axis of rotation". Are x, y, and z the rotation angles required to transform the rotational axis into the fixed one. If so what is the order of transformation? Also, I was looking for an intrinsic rotation with Bryan Tait angles. More like first rotate about x, then y, and then z. So I could call them as pitch, roll & yaw. Does your solution provide those angles? –  Black Dagger Jun 19 at 16:18
@BlackDagger The co-ordinates I've shown you are the rotation axis of rotation and a single angle. I ken these co-ordinates as "geodesic" or "exponential" co-ordinates for the Lie group $SO(3)$. I am a little busy at the moment to write a full answer to the conversion: see what you can find and if you don't have any success I'll get back to you. If I forget, you can find my contact details on my website, so email me. –  WetSavannaAnimal aka Rod Vance Jun 19 at 22:18
@BlackDagger I should think this article gives you what you need, once you have $\hat{Y}$ and $\hat{Z}$ Not that there are Tait Bryan angles discussed there. These latter are also called canonical co-ordinates of the second kind for $SO(3)$ (the geodesic co-ordinates I found above are also called "canonical co-ordinates of the first kind"). –  WetSavannaAnimal aka Rod Vance Jun 20 at 3:53
@WetSavannaAnimalakaRodVance Need your assistance ! --- physics.stackexchange.com/questions/123522/… –  Black Dagger Jul 7 at 16:50