從零開始一起學習SLAM | 三維空間剛體的旋轉
剛體，顧名思義，是指本身不會在運動過程中產生形變的物體，如相機的運動就是剛體運動，運動過程中同一個向量的長度和夾角都不會發生變化。剛體變換也稱為歐式變換。
視覺SLAM中使用的相機就是典型的剛體，相機一般通過人手持、機載（安裝在機器人上）、車載（固定在車輛上）等方式在三維空間內運動，形式包括旋轉、平移、縮放、切變等。其中，剛體在三維空間中最重要的運動形式就是旋轉。那麼剛體的旋轉如何量化表達呢？
三維空間中剛體的旋轉表示
三維空間中剛體的旋轉總共有4種表示方法，高翔的十四講中的第3講比較詳細的講解了。本文提煉中最重要的內容，並加上實際使用過程中的經驗總結進行了歸納。下面按照重要順序分別進行介紹。
1 旋轉矩陣
1、SLAM程式設計中使用比較頻繁。需要重點掌握。
2、旋轉矩陣不是一般矩陣，它有比較強的約束條件。旋轉矩陣R具有正交性，R和R的轉置的乘積是單位陣，且行列式值為1。
3、旋轉矩陣R的逆矩陣表示了一個和R相反的旋轉。
4、旋轉矩陣R通常和平移向量t一起組成齊次的變換矩陣T，描述了歐氏座標變換。引入齊次座標是為了可以方便的描述連續的歐氏變換，這個在上一篇文章《從零開始一起學習SLAM | 為什麼要用齊次座標？》中有講解。
5、冗餘。用9個元素表示3個自由度的旋轉，比較冗餘。
2 四元數
1、SLAM程式設計中使用頻繁程度接近旋轉矩陣。稍微有點抽象，不太直觀，但是一定得掌握。
2、四元數由一個實部和三個虛部組成，是一種非常緊湊、沒有奇異的表達方式。
3、程式設計時候很多坑，必須注意。首先，一定要注意四元素定義中實部虛部和列印係數的順序不同，很容易出錯！
其次，單位四元素才能描述旋轉，所以四元素使用前必須歸一化：q.normalize()。
3 旋轉向量
1、用一個旋轉軸n和旋轉角θ來描述一個旋轉，所以也稱軸角。不過很明顯，因為旋轉角度有一定的週期性（360°一圈），所以這種表達方式具有奇異性。
2、從旋轉向量到旋轉矩陣的轉換過程稱為 羅德里格斯公式。這個推導比較麻煩，否則也不會有一個專屬的名字了。OpenCV和MATLAB中都有專門的羅德里格斯函式。
3、旋轉向量本身沒什麼出彩的，不過旋轉向量和旋轉矩陣的轉換關係，其實對應於李代數和李群的對映，這對於後面理解李代數很有幫助。
4 尤拉角
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′ 必定為虛四元數（實部為零）。
"計算機視覺life"公眾號選單欄回覆：“旋轉”，即可下載題目1程式碼框架和輸出參考結果。
相關閱讀
---------------------
作者：electech6
來源：CSDN
原文：https://blog.csdn.net/electech6/article/details/83316205
版權宣告：本文為博主原創文章，轉載請附上博文連結！