4

角度軸の表現をオイラー角に変換しています。変換から得たオイラー角が元の軸角に戻ることを確認することにしました。値を出力しましたが、一致しません! http://forum.onlineconversion.com/showthread.php?t=5408とhttp://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles、およびこの Web サイトで同様の変換に関する質問を読みました。

以下のコードでは、角度「角度」と軸 (rx、ry、rz) から始めて、それをクォータニオン (q0、q1、q2、q3) に変換します。クォータニオンをオイラー角 (ロール、ピッチ、ヨー) に変換します。それを確認するために、(ロール、ピッチ、ヨー) を軸角度 (cAngle および (cRx,cRy,cRz)) に戻します。次に、(ロール、ピッチ、ヨー) の境界チェックを行って、-pi と pi の間の数値を維持し、それらを出力します。cAngle=angle かつ (cRx,cRy,cRz)=(rx,ry,rz) のはずですが、どちらも間違っています。

回転は、よくあるように Z*Y*X の順だと思います。私の数学に何か問題がありますか？http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/のように、ピッチが 0 または PI の場合の特別なケースを最終的に追加する予定ですが、今のところ問題は別のものだと思います。

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

2 に答える 2

5

私はあなたのコードをチェックしませんでした（そして私はチェックしません）。コードが正しい場合でも、少なくとも2つの理由でテストが失敗します。

つまり、変換が正しい場合でも、最初の表現ではなく、他の表現を取得できます。オイラー角から始めたのか、クォータニオンから始めたのかは関係ありません。

コードをテストする場合は、単位基底ベクトルの直交回転を確認することをお勧めします。たとえば、[1, 0, 0]を取得するには、適切に90度回転します[0, 1, 0]。期待どおりの結果が得られるかどうかを確認します[0, 1, 0]。3つの基底ベクトルすべての回転が正しい場合は、コードが正しい可能性があります。

このテストには明確であるという利点があり、何かを台無しにした場合（たとえば、数式のサイン）、このテストは間違いを見つけるのに大いに役立ちます。

アプリケーションの安定性を損なうので、オイラー角は使用しません。それらもあまり便利ではありません。

于 2013-02-24T09:46:32.573 に答える