從零開始一起學習SLAM | 三維空間剛體的旋轉
剛體，顧名思義，是指本身不會在運動過程中產生形變的物體，如相機的運動就是剛體運動，運動過程中同一個向量的長度和夾角都不會發生變化。剛體變換也稱為歐式變換。
視覺SLAM中使用的相機就是典型的剛體，相機一般通過人手持、機載（安裝在機器人上）、車載（固定在車輛上）等方式在三維空間內運動，形式包括旋轉、平移、縮放、切變等。其中，剛體在三維空間中最重要的運動形式就是旋轉。那麼剛體的旋轉如何量化表達呢？
三維空間中剛體的旋轉表示
三維空間中剛體的旋轉總共有4種表示方法，高翔的十四講中的第3講比較詳細的講解了。本文提煉中最重要的內容，並加上實際使用過程中的經驗總結進行了歸納。下面按照重要順序分別進行介紹。
一、旋轉矩陣
1、SLAM編程中使用比較頻繁。需要重點掌握。
2、旋轉矩陣不是一般矩陣，它有比較強的約束條件。旋轉矩陣R具有正交性，R和R的轉置的乘積是單位陣，且行列式值為1。
3、旋轉矩陣R的逆矩陣表示了一個和R相反的旋轉。
4、旋轉矩陣R通常和平移向量t一起組成齊次的變換矩陣T，描述了歐氏坐標變換。引入齊次坐標是為了可以方便的描述連續的歐氏變換，這個在上一篇文章《從零開始一起學習SLAM | 為什麼要用齊次坐標？》中有講解。
5、冗餘。用9個元素表示3個自由度的旋轉，比較冗餘。
二、四元數
1、SLAM編程中使用頻繁程度接近旋轉矩陣。稍微有點抽象，不太直觀，但是一定得掌握。
2、四元數由一個實部和三個虛部組成，是一種非常緊湊、沒有奇異的表達方式。
3、編程時候很多坑，必須注意。首先，一定要注意四元素定義中實部虛部和列印係數的順序不同，很容易出錯！
其次，單位四元素才能描述旋轉，所以四元素使用前必須歸一化：q.normalize()。
三、旋轉向量
1、用一個旋轉軸n和旋轉角θ來描述一個旋轉，所以也稱軸角。不過很明顯，因為旋轉角度有一定的週期性（360°一圈），所以這種表達方式具有奇異性。
2、從旋轉向量到旋轉矩陣的轉換過程稱為 羅德里格斯公式。這個推導比較麻煩，否則也不會有一個專屬的名字了。OpenCV和MATLAB中都有專門的羅德里格斯函數。
3、旋轉向量本身沒什麼出彩的，不過旋轉向量和旋轉矩陣的轉換關係，其實對應於李代數和李羣的映射，這對於後面理解李代數很有幫助。
四、歐拉角
1、把一次旋轉分解成3次繞不同坐標軸的旋轉，比如航空領域經常使用的「偏航-俯仰-滾轉」（yaw，pitch，roll）就是一種歐拉角。該表達方式最大的優勢就是直觀。
2、歐拉角在SLAM中用的很少，原因是它的一個致命缺點：萬向鎖。也就是在俯仰角為±90°時，第一次和第3次旋轉使用的是同一個坐標軸，會丟失一個自由度，引起奇異性。事實上，想要表達三維旋轉，至少需要4個變數。
瞭解了四種旋轉的表達方式，那麼編程時如何使用呢？
矩陣線性代數運算庫Eigen
事實上，上述幾種旋轉的表達方式在一個第三方庫Eigen中已經定義好啦。Eigen是一個C++開源線性代數庫，安裝非常方便，Ubuntu下一行代碼即可搞定：
sudo apt-get install libeigen3-dev
Eigen在SLAM編程中是必備基礎，必須熟練編程。關於Eigen，主要有以下幾點需要強調或注意。
1、Eigen庫不同於一般的庫，它只有頭文件，沒有.so和 .a那樣的二進位庫文件，所以在CMakeLists.txt裏只需要添加頭文件路徑，並不需要使用 target_link_libraries 將程序鏈接到庫上。
2、Eigen以矩陣為基本數據單元，在Eigen中，所有的矩陣和向量都是Matrix模板類的對象，Matrix一般使用3個參數：數據類型、行數、列數
Eigen::Matrix<typename Scalar, int rowsNum, int colsNum>
而向量只是一種特殊的矩陣（一行或者一列）。同時，Eigen通過typedef 預先定義好了很多內置類型，如下，我們可以看到底層仍然是Eigen::Matrix
typedef Eigen::Matrix<float, 4, 4> Matrix4f;
typedef Eigen::Matrix<float, 3, 1> Vector3f;
3、為了提高效率，對於已知大小的矩陣，使用時需要指定矩陣的大小和類型。如果不確定矩陣的大小，可以使用動態矩陣Eigen::Dynamic
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
4、Eigen在數據類型方面「很傻很天真」。什麼意思呢？就是使用Eigen時操作數據類型必須完全一致，不能進行自動類型提升。比如C++中，float類型加上double類型變數不會報錯，編譯器會自動將結果提升為double。但是在Eigen中float類型矩陣和double類型矩陣不能直接相加，必須統一為float或者double，否則會報錯。
5、Eigen除了空間幾何變換外，還提供了大量矩陣分解、稀疏線性方程求解等函數，非常方便。學習Eigen最好的方式就是官網：
http://eigen.tuxfamily.org/dox/
有非常多的示例參考。
上述四種旋轉表達方式是可以相互轉化的。在Eigen中它們之間的轉化非常的方便。下圖是我看的別人總結的旋轉矩陣、四元素、旋轉向量之間的相互轉化圖：
作業
題目1：
已知旋轉矩陣定義是沿著Z軸旋轉45°。請按照該定義初始化旋轉向量、旋轉矩陣、四元數、歐拉角。請編程實現：
1、以上四種表達方式的相互轉換關係並輸出，並參考給出的結果驗證是否正確。
2、假設平移向量為（1,2,3）,請輸出旋轉矩陣和該平移矩陣構成的歐式變換矩陣，並根據歐式變換矩陣提取旋轉向量及平移向量。
本程序學習目標：
1、學習eigen中剛體旋轉的四種表達方式，熟悉他們之間的相互轉換關係
2、熟悉旋轉平移和歐式變換矩陣的相互轉換關係
以下是參考的編程框架：
題目2：
我們知道單位四元數q可以表達旋轉。一個三維空間點可以用虛四元數p表示，用四元數 q 旋轉點 p 的結果p為:
證明：此時 p′ 必定為虛四元數（實部為零）。
公眾號菜單欄回復：「旋轉」，即可下載題目1代碼框架和輸出參考結果。
歡迎留言討論，更多學習視頻、文檔資料、作業參考答案等掃描下方二維碼進入知識星球「從零開始學習SLAM」和其他學習SLAM學的夥伴一起學習交流~
相關閱讀
從零開始一起學習SLAM | 為什麼要學SLAM？
從零開始一起學習SLAM | 學習SLAM到底需要學什麼？
從零開始一起學習SLAM | SLAM有什麼用？
從零開始一起學習SLAM | C++新特性要不要學？
從零開始一起學習SLAM | 為什麼要用齊次坐標？
零基礎小白，如何入門計算機視覺？
本文首發於公眾號：計算機視覺life。原文鏈接：
從零開始一起學習SLAM | 三維空間剛體的旋轉
推薦閱讀：
