三維空間 旋轉矩陣 四元數 尤拉角 相互轉換 組合旋轉

API和原始碼 

旋轉矩陣 四元數 尤拉角 相互轉換的原始碼

這個是開源的原始碼。在ROS 的TF2包中就有。

TF2是TF的第二個版本。

API說明：

http://docs.ros.org/hydro/api/tf2/html/classtf2_1_1Matrix3x3.html

http://docs.ros.org/hydro/api/tf2/html/classtf2_1_1Quaternion.html

原始碼頁：

http://docs.ros.org/hydro/api/tf2/html/Quaternion_8h_source.html

http://docs.ros.org/hydro/api/tf2/html/Matrix3x3_8h_source.html

http://docs.ros.org/hydro/api/tf2/html/Vector3_8h.html

理論說明：

參見

http://blog.csdn.net/wangjiannuaa/article/details/8952196

當用一個四元數乘以一個向量時，實際上就是讓該向量圍繞著這個四元數所描述的旋轉軸，轉動這個四元數所描述的角度而得到的向量

四元組的優點
有多種方式可表示旋轉，如 axis/angle、尤拉角(Euler angles)、矩陣(matrix)、四元組等。 相對於其它方法，四元組有其本身的優點：
四元數不會有尤拉角存在的 gimbal lock 問題
四元數由4個陣列成，旋轉矩陣需要9個數
兩個四元數之間更容易插值
四元數、矩陣在多次運算後會積攢誤差，需要分別對其做規範化(normalize)和正交化(orthogonalize)，對四元數規範化更容易
與旋轉矩陣類似，兩個四元組相乘可表示兩次旋轉

兩個四元數相乘也表示一個旋轉
Q1 * Q2 表示先以Q2旋轉，再以Q1旋轉