Incorrect conversion from quaternions to euler angles and back

Question

I am converting angles-axis representation to Euler angles. I decided to check and make sure that the Euler angles I got from the conversion would lead back to the original axis-angle. I print out the values, but they do not match! I have read http://forum.onlineconversion.com/showthread.php?t=5408 and http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles as well as similar conversion questions on this website.

In the code below I start with angle 'angle' and axis (rx,ry,rz), then I convert it to quaternions (q0,q1,q2,q3). I convert the quaternions to euler angles (roll, pitch, yaw). Then to check it, I convert (roll,pitch,yaw) back to axis-angle as cAngle and (cRx,cRy,cRz). I then do some bounds checking on (roll, pitch,yaw) to keep the numbers between -pi and pi, and then I print them out. It should be that cAngle=angle and (cRx,cRy,cRz)=(rx,ry,rz), but these are both wrong.

The rotations are in order Z*Y*X as is common, I believe. Is there someting wrong with my math? I plan on eventually adding special cases for when pitch is 0 or PI as in http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ but right now I think the problem is separate.

        //input is angle 'angle' and axis '(rx,ry,rz)'

        //convert rx,ry,rz, angle, into roll, pitch, yaw
        double q0 = Math.Cos(angle / 2);
        double q1 = Math.Sin(angle / 2) *Math.Cos(rx);
        double q2 = Math.Sin(angle / 2) * Math.Cos(ry);
        double q3 = Math.Sin(angle / 2) * Math.Cos(rz);
        double roll = Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
        double pitch = Math.Asin(2 * (q0 * q2 - q3 * q1));
        double yaw = Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));

        //convert back to angle axis
        double cAngle = 2 * Math.Cos(Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2));
        double cRx = Math.Acos((Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) - Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
        double cRy = Math.Acos((Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
        double cRz = Math.Acos((Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2) - Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2)) / Math.Sin(cAngle / 2));

        //stay within +/- PI of 0 to keep the number small
        if (roll > 3.1416) roll = -Math.PI + (roll - Math.PI);
        if (roll < -3.1416) roll = Math.PI + (roll - (-1) * Math.PI);
        if (pitch > 3.1416) pitch = -Math.PI + (pitch - Math.PI);
        if (pitch < -3.1416) pitch = Math.PI + (pitch - (-1) * 3.1416F);
        if (yaw > 3.1416) yaw = -Math.PI + (yaw - Math.PI);
        if (yaw < -3.1416) yaw = Math.PI + (yaw - (-1) * Math.PI);

        Console.WriteLine("original angle, axis " + angle + ": " + rx + ", " + ry + ", " + rz);
        Console.WriteLine("converted angle, axis " + cAngle + ": " + cRx + ", " + cRy + ", " + cRz);
        Console.WriteLine("quats " + q0 + ", " + q1 + ", " + q2 + ", " + q3);
        Console.WriteLine("roll,pitch,yaw:  " + roll + ", " + pitch + ", " + yaw);

+1. I finally know what a quaternion is. more info in a minute than days of Google. Hope you get your answer. — Kaliber64

Ali Ali · Accepted Answer · 2013-02-24T09:46:32

I didn't (and I won't) check your code. Even if your code is correct, your test fails for at least 2 reasons.

There are 2 ways to represent the same rotation with Euler angles. See also Euler angle to Quaternion then Quaternion to euler angle, that question is basically about the same problem you are having.
Quaternions have the so-called double covering property: Two unit quaternions correspond to every rotation.

That is, even if your conversions are correct, you can get the other representation and not the one you started with. It doesn't matter whether you started with Euler angles or quaternions.

If you want to test your code, I suggest checking orthogonal rotations of the unit basis vectors. For example rotate [1, 0, 0] with 90 degrees appropriately to get [0, 1, 0]. Check whether you really get the expected [0, 1, 0], etc. If you got the rotations of all 3 basis vectors right then your code is most likely correct.

This test has the advantage of being unambiguous and if you mess up something (for example a sign in a formula) this test helps you a lot in finding your mistake.

I wouldn't use Euler angles as they screw up the stability of your application. They are not very handy either.

Incorrect conversion from quaternions to euler angles and back

2 Answers