math - 从四元数到欧拉角的错误转换

Question

我正在将角度轴表示转换为欧拉角。我决定检查并确保我从转换中得到的欧拉角会导致回到原始轴角。我打印出这些值，但它们不匹配!我已阅读 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)。然后我对 (roll, pitch, yaw) 进行一些边界检查以保持 -pi 和 pi 之间的数字，然后我将它们打印出来。应该是cAngle=angle 和(cRx,cRy,cRz)=(rx,ry,rz)，但这些都是错误的。

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

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

Answer 1

我没有(也不会)检查您的代码。 即使您的代码是正确的，您的测试也会由于至少两个原因而失败。

There are 2 ways用欧拉角表示相同的旋转。另见 Euler angle to Quaternion then Quaternion to euler angle ，该问题基本上与您遇到的问题相同。

四元数有所谓的double covering属性:每个旋转对应两个单位四元数。

也就是说，即使您的转换是正确的，您也可以获得另一种表示，而不是您开始时使用的表示。无论您是从欧拉角还是从四元数开始，都没有关系。

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

此测试的优点是明确如果你搞砸了一些东西(例如公式中的符号)，这个测试可以帮助你找到你的错误。

我不会将欧拉角用作 they screw up the stability of your application .他们是 not very handy任何一个。