第三章 三維空間剛體運動

主要目標

理解三維空間的剛體運動描述方式：旋轉矩陣、變換矩陣、四元數和歐拉角。

掌握 Eigen 庫的矩陣、幾何模塊使用方法。
一、 旋轉矩陣
剛體，它不光有位置，還有自身的姿態。相機也可以看成三維空間的剛體，于是位置是指相機在空間中的哪個地方，而姿態
則是指相機的朝向。我們要用數學語言來描述它。
1）點和向量
點就是空間當中的基本元素，沒有長度，沒有體積。把兩個點連接起來，就構成了向量。向量可以看成從某點指向另一點的一個箭頭。
2）基
維空間中的某個點的坐標也可以用 R3 來描述。假設在這個線性空間內，我們找到了該空間的一組基(e1, e2, e3)，那么，任意向量 a 在這組基下就有一個坐標：

這里 (a1, a2, a3)T 稱為 a 在此基下的坐標。坐標的具體取值，一是和向量本身有關，二是和坐標系（基）的選取有關。
3）內積和外積
對于 a, b ∈ R3，通常意義下的內積可以寫成：

其中 ?a, b? 指向量 a, b 的夾角。內積也可以描述向量間的投影關系。而外積則是這個樣子：

外積的結果是一個向量，它的方向垂直于這兩個向量，大小為 |a| |b|sin ?a, b?，是兩個向量張成的四邊形的有向面積。
對于外積運算，我們引入 ∧ 符號，把 a 寫成一個矩陣。事實上是一個反對稱矩陣（Skew-symmetric matrix），可以將 ∧ 記成一個反對稱符號。這樣就把外積 a × b 寫成了矩陣與向量的乘法a∧b，把它變成了線性運算。

4）坐標系間的歐氏變換
如下圖所示的坐標變換。對于同一個向量 p，它在世界坐標系下的坐標 pw 和在相機坐標系下的坐標 pc是不同的。這個變換關系由變換矩陣 T 來描述。變換矩陣由旋轉矩陣和平移向量構成。

歐氏變換由旋轉和平移組成。我們首先考慮旋轉。設某個單位正交基 (e1, e2, e3) 經過一次旋轉變成了 (e′1, e′2, e′3)。那么，對于同一個向量 a（該向量并沒有隨著坐標系的旋轉而發生運動），它在兩個坐標系下的坐標為 [a1, a2, a3]T 和 [a′1, a′2, a′3]T。因為向量本身沒變，根據坐標的定義，有：

為了描述兩個坐標之間的關系，我們對上述等式的左右兩邊同時左乘 [eT1 eT2 eT3 ]T,那么左邊的系數就變成了單位矩陣，所以：

5)變換矩陣與齊次坐標

這是一個數學技巧：我們在一個三維向量的末尾添加 1，將其變成了四維向量，稱為齊次坐標。對于這個四維向量，我們可以把旋轉和平移寫在一個矩陣里面，使得整個關系變成線性關系。該式中，矩陣 T 稱為變換矩陣（Transform Matrix）。
二、實踐：Eigen
輸入以下命令進行安裝：
sudo apt-get install libeigen3-dev
三、旋轉向量和歐拉角
1）旋轉向量
任意旋轉都可以用一個旋轉軸和一個旋轉角來刻畫。于是，我們可以使用一個向量，其方向與旋轉軸一致，而長度等于旋轉角。這種向量稱為旋轉向量（或軸角/角軸，Axis-Angle），只需一個三維向量即可描述旋轉。同樣，對于變換矩陣，我們使用一個旋轉向量和一個平移向量即可表達一次變換。這時的變量維數正好是六維。
假設旋轉軸為一個單位長度的向量 n，角度為 θ，那么向量 θn 也可以描述這個旋轉。從旋轉向量到旋轉矩陣的轉換過程由羅德里格斯公式（Rodrigues’s Formula ）表明：

一個旋轉矩陣到旋轉向量的轉換：

2）歐拉角
而歐拉角則提供了一種非常直觀的方式來描述旋轉——它使用了 3 個分離的轉角，把一個旋轉分解成 3 次繞不同軸的旋轉。
具體參考：https://blog.csdn.net/sinolover/article/details/90671784
三、四元數
1）四元數的定義
旋轉矩陣用 9 個量描述 3 自由度的旋轉，具有冗余性；歐拉角和旋轉向量是緊湊的，但具有奇異性。而四元數是 Hamilton 找到的一種擴展的復數。它既是緊湊的，也沒有奇異性。如果說缺點，四元數不夠直觀，其運算稍復雜些。
一個四元數 q 擁有一個實部和三個虛部。

其中 i, j, k 為四元數的三個虛部。這三個虛部滿足以下關系式：

2）四元數的運算
四元數 qa, qb 的加減運算為：

乘法：

模長：

共軛：

逆：

數乘：

3）四元數表示旋轉
首先，把三維空間點用一個虛四元數來描述：

相當于把四元數的 3 個虛部與空間中的 3 個軸相對應。那么，旋轉后的點 p′ 即可表示為這樣的乘積：

這里的乘法均為四元數乘法，結果也是四元數。最后把 p′ 的虛部取出，即得旋轉之后點的坐標。
4）四元數到其他旋轉表示的轉換
四元數到旋轉矩陣：

四元數到旋轉向量的轉換公式：

四、相似、仿射、射影變換
1、相似變換
相似變換比歐氏變換多了一個自由度，它允許物體進行均勻縮放，其矩陣表示為：

2、仿射變換
仿射變換的矩陣形式如下：

3、射影變換

4、常見變換比較

五、實踐：Eigen 幾何模塊
? 旋轉矩陣（3 × 3）：Eigen::Matrix3d。
? 旋轉向量（3 × 1）：Eigen::AngleAxisd。
? 歐拉角（3 × 1）：Eigen::Vector3d。
? 四元數（4 × 1）：Eigen::Quaterniond。
? 歐氏變換矩陣（4 × 4）：Eigen::Isometry3d。
? 仿射變換（4 × 4）：Eigen::Affine3d。
? 射影變換（4 × 4）：Eigen::Projective3d。

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲第三讲的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。