將四元數轉換爲歐拉角和返回的不正確

Question

我將角軸表示轉換爲歐拉角。我決定檢查並確保從轉換中得到的歐拉角度會回到原來的軸角度。我打印出這些值，但它們不匹配！我已閱讀http://forum.onlineconversion.com/showthread.php?t=5408和http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles以及本網站上的類似轉換問題。

在下面的代碼中，我從角度'angle'和軸（rx，ry，rz）開始，然後將其轉換爲四元數（q0，q1，q2，q3）。我將四元數轉換成歐拉角（滾動，俯仰，偏航）。然後檢查它，我將（滾動，俯仰，偏航）轉換回軸角爲cAngle和（cRx，cRy，cRz）。然後我做一些邊界檢查（滾動，俯仰，偏航）以保持-pi和pi之間的數字，然後將它們打印出來。應該是cAngle =角度和（cRx，cRy，cRz）=（rx，ry，rz），但這些都是錯誤的。

我相信旋轉的順序是Z * Y * X。我的數學有什麼問題嗎？我打算最終在pitch爲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。