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