SLAM學習之路#4 三維空間剛體運動

這篇講的是三維空間剛體運動 3D Space Rigid Body Motion 的表示及運算，主要是理解3d coordinate system、Rotation matrix、Transformation matrix、Rotation vector…等。

3d coordinate system
# 由三個正交的軸組合
# 左手座標系、右手座標系
# 其三維空間中的點所組合的向量a，可由三維座標系表示：

左、右手座標系

向量a表達式

# 運算，包含加法、減法、內積、外積

三維向量內積與外積

其中向量的加減很直觀，內積描述了向量的投影關係，而外積則有以下幾點說明：
1.外積方向垂直於a、b兩向量，大小為|a||b|sin(a, b)
2.外積為兩向量張成的四邊形面積
3.用a^b表達，a寫成矩陣形式，事實上是一個反對稱矩陣（是一個方形矩陣，其轉置矩陣和自身的加法逆元相等），因此可以把^看成一個反對稱符號，目的是把外積a x b寫成矩陣與向量的乘法a ^ b的線性運算
4.外積只對三維向量存在定義，我們能用外積表示向量的旋轉

Rotation matrix

某(e1, e2, e3)標準正交基（Orthonormal basis）經過一次旋轉變成了(e’1, e’2, e’3)，對於同一個向量a，它在兩個座標系下的座標為：

R矩陣應該描述了旋轉的過程，所以又稱為旋轉矩陣，而旋轉矩陣是一個行列式為1的正交矩陣（反之也是），因此我們把旋轉矩陣的集合定義為特殊正交群SO(n)。

特殊正交群SO(n)

另外，由於旋轉矩陣為正交，它的逆（與轉置）描述了一個相反的旋轉：

反轉矩陣

Transformation matrix

在歐式變換中，除了旋轉外還有平移

旋轉加平移結合在一起

引入齊次座標跟轉換矩陣

轉換矩陣T，左上角為旋轉矩陣，右側為平移向量，左下角為0，右下角為1。這種矩陣又稱為特殊歐式群：

特殊歐式群

轉換矩陣的逆表示一個反向的轉換

Homogeneous coordinate

投影幾何的概念，透過加一個維度，用四個實數描述一個三維向量。在齊次座標中，某個點x的每個分量同乘一個非零常數k後，仍然代表同一個點，因此一個點的具體座標值不是唯一的，例如[1, 1, 1, 1]與[5, 5, 5, 5]是同一個點，這兩個點的座標跟歐式空間中是一樣的。

Rotation vector

用矩陣表示轉換有幾個缺點：
1. SO(3) 的旋轉矩陣有九個量，但一次旋轉只有三個自由度；SE(3) 的轉換矩陣用十六個量表達了六自由度的轉換。
2. 旋轉矩陣跟變換矩陣自身帶有約束，它必須是正交矩陣，且行列式為1，當我們要估計這些矩陣時，會讓求解變得複雜。

任意的旋轉都可以用一個旋轉軸和一個旋轉角來表示，於是，我們可以使用一個向量，其方向與旋轉軸一致，長度等於旋轉角，這種向量我們稱為旋轉向量，而旋轉向量就是我們之後要提到的李代數，所以詳細的內容我們留到後面講，而前面的SO(3)及SE(3)則是李群。

最後，旋轉向量跟旋轉矩陣要如何轉換？假設有一個旋轉軸為n，角度為θ，它所對應的旋轉向量為θn，並可以用下式表達，符號^是向量到反對稱的轉換符號：

羅德里格斯公式(Rodrigues’s Formula )

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

而旋轉軸上的向量在旋轉後不會發生變化，只要將上式R = ….的兩邊都乘上n，θ代0，即可得到Rn = n。

Euler angle

將旋轉分解成三次，不同軸上的轉動，以便理解
1. 繞物體的Z軸旋轉，得到yaw
2. 繞物體的Y軸旋轉，得到pitch
3. 繞物體的X軸旋轉，得到roll

但Euler angle會有方向鎖的問題，若pitch為±90◦ 時，第一次旋轉与第三次旋轉將使用同一個轴，使系统丢失一個自由度（由三次旋转變成了两次旋轉）。

Euler angle旋轉式意圖

四元數

先略

各種Transform matrix

相似變換，旋轉部分多了s的縮放因子

仿射變換，A是一個可逆矩陣，而不必是正交矩陣，經過仿射變換後，立方體不再是方的，但各面還是平行四邊形

射影變換，從真實世界中的方形磁磚到相機底片的變換可以看成是一個射影變換，它不在是方形的，且由於遠到近的關係，它甚至不是平行四邊形的，而是一個不規則四邊形

Reference：

1. 視覺SLAM 14講