旋轉矩陣是姿態的一種數學表達方式，或者籠統說變換矩陣是一種抽象的數學變量。其抽象在於當你看到數據，根本無法斷定其正確性；往往只有轉換為較為直觀的歐拉角，然后大概目測估算（總不能拿着量角器去測量精度吧）。
我們知道，姿態的數學形式有旋轉矩陣（滿足RTR=E）、歐拉角、旋轉向量（角軸）、四元數（norm(x y z w) = 1）等。今天這里要討論的是旋轉矩陣相關理論基礎。
旋轉矩陣常用於以下三種場景：
- 描述一個frame（坐標系）相對於另一個frame間的位姿；(如：描述相鄰兩幀間的相機運動)
- 描述一個point從一個frame變換到另一個frame；（其實點沒動，只是在兩個frame觀測時，坐標不一樣而已；如：；路標點點P在不同相機坐標系下坐標值不同，但在世界坐標系下是固定的）
- 描述一個point在同一個frame中的運動；（這里的點是運動了的，在VSLAM中應用較少）
繞三個軸的旋轉矩陣請按以下形式進行記憶，可能有些地方是下面的轉置形式。
（本博客和大多數論文資料一樣，都是左乘原則）
一、固定旋轉（Fix Angles）
所謂固定旋轉，就是按照物體外部固定軸旋轉。這種旋轉在工程中很常見，用的相對多一些（相對於下面的歐拉旋轉），例如：旋轉次序為X-Y-Z，那么旋轉矩陣的形式為：RZRYRX。如下圖，其實沒什么好說。
已知旋轉矩陣，怎么求解歐拉角呢？下圖便是一種比較好的求解辦法。我們知道，順時針旋轉180°和逆時針旋轉180°是等效的（讓機械臂旋轉190°其實是走遠路，因為旋轉170°似乎更快），所以求解出來的歐拉角其取值范圍最好是：[0°，180°] or [0°，-180°] 或者[-90°，90°]，這里采用的后者。
下圖中的atan2是matlab中的api，我們在這里簡單驗證下其與tan的區別：可以看到，當一個點在第三象限，如（-1， -1），在歐拉角中，其角度為：-135°，但是atan(-1/-1)算出來是45°，顯然是錯的。
1 >> atan(1) * 180 / pi 2 3 ans = 4 5 45 6 7 >> atan2(1, 1) * 180 / pi 8 9 ans = 10 11 45 12 13 >> atan2(-1, -1) * 180 / pi 14 15 ans = 16 17 -135 18 19 >>
順在在這里用Eigen驗證下：（相關頭文件去這個鏈接中查找： https://www.cnblogs.com/winslam/p/12765822.html ）不去下載，下面C++代碼你都跑不了
1 #include"geometry.h" 2 #include<iostream> 3 using namespace std; 4 5 int main() 6 { 7 Eigen::Matrix3d R1; 8 R1 << 0.866, 0.433, 0.25, 0., 0.5, -0.866, -0.5, 0.75, 0.433; 9 10 Pose pose1(R1); 11 cout << "旋轉矩陣 = " << endl; cout << pose1.rotation() << endl; 12 cout << "歐拉角 = " << endl; cout << pose1.euler_angle().transpose()*(180 / M_PI) << endl; 13 cout << "四元數 = " << endl; cout << pose1.quaternion().coeffs().transpose() << endl; 14 cout << "角軸 = " << endl; 15 cout << pose1.angle_axis().angle()* (180 / M_PI) << " " << pose1.angle_axis().axis().transpose() << endl; 16 cout << "-----------------------------" << endl; 17 return 1; 18 }
打印結果：
1 旋轉矩陣 = 0.866 0.433 0.25 2 0 0.5 -0.866 3 -0.5 0.75 0.433 4 歐拉角 = 63.4349 14.4779 -26.5651 5 四元數 = 0.482959 0.224145 -0.129407 0.836511 6 角軸 = 66.4519 0.881411 0.409071 -0.23617
可以看到Eigen的歐拉角結果和上述PPT中Matlab計算的結果不一樣，但是他們旋轉矩陣具體是一樣的，這個是正常的，下面還會碰到。其實就是利用歐式旋轉矩陣求解
歐拉角的解並不唯一，這個問題在手眼標定中驗證過。
二、歐拉旋轉（Euler Angle）
固定旋轉與歐拉旋轉的關系，例如：固定旋轉次序為:XYZ，寫成旋轉矩陣則為：Rz*Ry*Rx；其等效的歐拉旋轉次序為：ZYX，寫成旋轉矩陣則為：Rz*Ry*Rx，最后的映射矩陣是一樣的，但是過程不一樣，殊途同歸。）例如下面例子：
ZYZ的歐拉旋轉次序，這個我是實際“體驗過”，那是在自研機械臂上示教器是數據。
歐拉旋轉的方式如下圖：
形如固定旋轉，我這里也給出：已經ZYZ類型歐拉旋轉，求解歐拉角的一種方法：
針對上面等式
，這里用Eigen驗證下：
對於等式左邊：
1 #include"geometry.h" 2 #include<iostream> 3 using namespace std; 4 5 int main() 6 { 7 // 低級錯誤：是60.0，不是60 8 Eigen::Matrix3d R1; 9 R1 = Eigen::AngleAxisd(60.0 / 180 * M_PI, Eigen::Vector3d::UnitX()) * 10 Eigen::AngleAxisd(30.0 / 180 * M_PI, Eigen::Vector3d::UnitY()) * 11 Eigen::AngleAxisd( 0.0, Eigen::Vector3d::UnitZ()); 12 13 Pose pose3(R1); 14 cout << "旋轉矩陣 = " << endl; cout << pose3.rotation() << endl; 15 cout << "歐拉角 = " << endl; cout << pose3.euler_angle().transpose()*(180 / M_PI) << endl; 16 cout << "四元數 = " << endl; cout << pose3.quaternion().coeffs().transpose() << endl; 17 cout << "角軸 = " << endl; 18 cout << pose3.angle_axis().angle()* (180 / M_PI) << " " << pose3.angle_axis().axis().transpose() << endl; 19 cout << "-----------------------------" << endl; 22 return 1; 23 }
針對等式右邊，同理有：
#include"geometry.h" #include<iostream> using namespace std; int main() { Eigen::Vector3d RPY; RPY << -56.3 / 180 * M_PI, 64.3 / 180 * M_PI, 73.9 / 180 * M_PI; cout << RPY(2, 0) << endl; Eigen::Matrix3d R2; // note: ZYZ R2 = Eigen::AngleAxisd(RPY(0, 0), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(RPY(1, 0), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(RPY(2, 0), Eigen::Vector3d::UnitZ()); Pose pose4(R2); cout << "旋轉矩陣 = " << endl; cout << pose4.rotation() << endl; cout << "歐拉角 = " << endl; cout << pose4.euler_angle().transpose()*(180 / M_PI) << endl; cout << "四元數 = " << endl; cout << pose4.quaternion().coeffs().transpose() << endl; cout << "角軸 = " << endl; cout << pose4.angle_axis().angle()* (180 / M_PI) << " " << pose4.angle_axis().axis().transpose() << endl; cout << "-----------------------------" << endl; return 1; }
對比兩種旋轉方式，可以看到Eigen計算的結果完全一致；但是比較有意思的是，對於下面，我用的歐拉角明明是( -56.3, 64.3, 73.9 )（ZYZ），結果打印輸出的
歐拉角居然是(59.9, 29.99, 0.031)。這是由於我寫的Pose類底層，我只定義了ZYX的固定旋轉方式。同時，也說明了，同一個旋轉矩陣，可能對應不同的旋轉方式(如：歐拉旋轉、固定旋轉)，不同的旋轉歐拉角、甚至不同的旋轉次序
三、旋轉矩陣小結
歐拉旋轉和固定旋轉的次序有效的排列組合為：12 = 3 * 2 * 2
姿態的數學形式有旋轉矩陣（約束：RTR=E）、歐拉角（萬向鎖問題）、旋轉向量（角軸）、四元數（約束norm(x y z w) = 1），所以VSLAM在對位姿進行優化的時候，
為了避免“帶約束的優化”問題，一般都會采用旋轉向量的形式作為姿態。
參考：
（台大機器人學之運動學——林沛群）
https://www.bilibili.com/video/BV1v4411H7ez?p=3
https://baike.baidu.com/item/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5/3265181?fr=aladdin#5
《jlblanco2010geometry3D_techrep.pdf》
https://blog.csdn.net/uranus1992/article/details/85788205