Roll, pitch, yaw calculation

Question

How can I calculate the roll, pitch and yaw angles associated with a homogeneous transformation matrix?

I am using the following formulas at the moment, but I am not sure whether they are correct or not.

pitch = atan2( -r20, sqrt(r21*r21+r22*r22) );
yaw   = atan2(  r10, r00 );
roll  = atan2(  r21, r22 );

r10 means second row and first column.

Answer 1

Your equations are correct only if the order of rotations is: roll, then pitch, then yaw. For the record, the correspondence with Euler angles (with respect to the frame of reference implicitly given with the transformation matrix) is as follows:

• Roll is the rotation about the x axis (between -180 and 180 deg);
• Pitch is the rotations about the y axis (between -90 and 90 deg);
• Yaw is the rotation about the z axis (between -180 and 180).

Given these, the order roll, pitch, yaw mentioned in the first sentence corresponds to the rotation matrix obtain by the matrix product Rz Ry Rx (in this order). Note that your formula give the values of these angles in radians (multiply by 180 and divide by pi to obtain values in degrees). All rotations are counter-clockwise with respect to the axis.

Figure taken from Wikipedia

---

Following your comment about this link, I think this paper might help to understand the program you are referring to. The input to the Matlab function is supposed to be your transformation matrix, followed by 'deg' if you want the angles to be returned in degrees, and an obsolete option 'zyx' if the order of the rotations is around z, then around y, then around x.

Answer 2

[This might be better suited as a comment but it is to long for that ;) ]

When I compare your formula with the one on the german Wikipedia page about roll, pitch an yaw (see here) there is a difference in the calculation of the pitch. According to Wikipedia your formula should look like this:

pitch = atan2(-r20,(sqrt(pow(r21,2)+pow(r00,2))); // replaced r22 by r00

Note that on the wikipedia page they use a different indexing for the matrix elements (thex start with 1 and not with 0 for the first row/column). Furthermore they call pitch beta, yaw alpha and roll gamma. Also, they divide the coefficents for atan2 in the yaw and roll calculation by the cos(pitch), but that should cancel out.

Otherwise your formula looks fine to me.