从四元数到欧拉角的错误转换 [英] Incorrect conversion from quaternions to euler angles and back

问题描述

我正在将角度轴表示转换为欧拉角.我决定检查并确保我从转换中得到的欧拉角会回到原来的轴角.我打印出值，但它们不匹配！我已阅读 http://forum.onlineconversion.com/showthread.php?t=5408 和 http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 以及类似的本网站上的转换问题.

在下面的代码中，我从角度角度"和轴 (rx,ry,rz) 开始，然后将其转换为四元数 (q0,q1,q2,q3).我将四元数转换为欧拉角(滚动、俯仰、偏航).然后为了检查它，我将 (roll,pitch,yaw) 转换回轴角为 cAngle 和 (cRx,cRy,cRz).然后我对(滚动、俯仰、偏航)进行一些边界检查，以将数字保持在 -pi 和 pi 之间，然后将它们打印出来.应该是cAngle=angle和(cRx,cRy,cRz)=(rx,ry,rz)，但是这两个都是错的.

我相信，旋转的顺序是 Z*Y*X，这很常见.我的数学有问题吗?我计划最终为音高为 0 或 PI 添加特殊情况，如 http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ 但现在我认为问题是分开的.

//输入是角度'angle'和轴'(rx,ry,rz)'//将rx,ry,rz, angle,转换成roll, pitch, yaw双q0 = Math.Cos(角度/2)；双 q1 = Math.Sin(角度/2) *Math.Cos(rx);双 q2 = Math.Sin(角度/2) * Math.Cos(ry);双 q3 = Math.Sin(角度/2) * Math.Cos(rz);双掷 = Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));双节距 = Math.Asin(2 * (q0 * q2 - q3 * q1));双偏航 = Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));//转换回角度轴双 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(偏航/2));双 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));双 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));双 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));//保持在 0 的 +/- PI 内以保持数字小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("原角度、轴"+角度+":"+rx+","+ry+","+rz);Console.WriteLine("转换后的角度，轴 " + cAngle + ": " + cRx + ", " + cRy + ", " + cRz);Console.WriteLine("quats " + q0 + ", " + q1 + ", " + q2 + ", " + q3);Console.WriteLine("roll,pitch,yaw: " + roll + ", " + pitch + ", " + yaw);

解决方案

我没有(也不会)检查你的代码.即使您的代码是正确的，您的测试也会因为至少 2 个原因而失败.

• 有两种方式用欧拉角表示相同的旋转.另请参阅欧拉角到四元数然后四元数到欧拉角，这个问题基本上与您遇到的问题相同.p>

• 四元数有所谓的双重覆盖属性:每个旋转对应两个单位四元数.

也就是说，即使您的转换是正确的，您也可以获得另一种表示，而不是您开始使用的表示.不管你是从欧拉角还是四元数开始.

<小时>

如果您想测试您的代码，我建议您检查正交单位基向量的旋转.例如，将 [1, 0, 0] 适当旋转 90 度以获得 [0, 1, 0].检查你是否真的得到了预期的 [0, 1, 0] 等.如果你得到了所有 3 个基向量的旋转，那么你的代码很可能是正确的.

此测试具有明确的优点，如果您弄乱了某些东西(例如公式中的符号)，此测试可以帮助您找出错误.

<小时>

我不会使用欧拉角，因为它们会破坏应用程序的稳定性.它们也不是很方便.

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);

解决方案

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.

这篇关于从四元数到欧拉角的错误转换的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！