第三章 三維空間剛體運動

主要目標

1. 理解三維空間的剛體運動描述方式：旋轉矩陣、變換矩陣、四元數和尤拉角。
2. 掌握 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。