主要內容

本章開始進入視覺里程計(VO)部分，VO按是否需要提取特徵，分為特徵點法的前端和不提特徵的前端。這一章講的是基於特徵點法的前端，分為以下內容。

1. 特徵點法：找到兩張2D影象上的匹配點。
2. 對極幾何：根據2D-2D特徵點對求解R,t。
3. 三角測量：根據2D-2D特徵點求深度。
4. PnP：根據3D點雲和匹配的2D影象求R,t。
5. ICP：求兩個點雲之間的R,t。

關係是：

1. 特徵點法找到2D影象的匹配點對，用於對極幾何和pnp
2. 對極幾何求出2D-2D的位姿。
3. 根據對極幾何求出的位姿，三角測量求出2D-2D的深度。
4. 根據三角測量求出的深度，可以初始化單目SLAM，得到三維點資訊；或用RGBD相機獲得三維資訊。知道3D資訊，對於下一張2D影象，可根據特徵點法找到的匹配點對，使用PNP，求出位姿資訊和深度資訊。
5. 根據pnp求出的深度資訊；或直接用RGBD得到的兩個點雲，可以用ICP，求出兩個點雲之間的位姿變換。

一、特徵點法

這部分內容，講述了提取2D影象的特徵點，並對不同影象之間的特徵點進行匹配，得到匹配的特徵點對。這些匹配點是後面用來估計相機位姿的基礎。

特徵點：在SLAM中被成為路標。分為關鍵點和描述子。
對於SLAM來說，SIFT太慢，FAST沒有方向資訊，使用改進FAST的ORB特徵。

ORB特徵

關鍵點：Oriented FAST

是改進的，具有方向性的FAST描述子。
FAST用於檢測區域性畫素灰度明顯變化的位置，思想是看鄰域內有沒有連續N個和它差T灰度的點。加上NMS和其他遍歷的加速trick。
ORB的改進：

• 根據FAST關鍵點的Harris響應值，選取前N大的，以固定特徵點數目。
• 構建影象金字塔，解決尺度問題。
• 使用灰度質心法，解決旋轉問題。(加一個幾何中心到質心的向量)

描述子：BRIEF

是一種二進位制描述子，描述了關鍵點附近兩個畫素的大小關係。

特徵匹配

區域性特徵會導致誤匹配，難以解決。
BRIEF描述子使用漢明距離進行相似性度量，快速近似最近鄰(FLANN)適合數目多的特徵點匹配。

二、對極幾何

• 已知：匹配點對$p_1$，$p_2$的畫素座標。
• 給定：兩張二維影象，二維影象上特徵點的匹配關係。
• 未知：P的三維空間座標，$I_1$到$I_2$的變換矩陣($T_{}$
  1,2T_{1,2}，即$R,t$)。

一般是用於__單目SLAM的初始化__，用對極幾何可求出位姿，在用三角測量估計三維空間點的位置後，就能用其他更準確的方法繼續求解了。

1. 對極約束

設P在圖1的相機座標系下，座標為：
$P=[X,Y,Z]^T$
$p_1$，$p_2$的畫素座標（單位畫素）：
$s_1p_1=KP, s_2p_2=K(RP+t)$

其歸一化平面座標（單位米）：
$x_1=K^{-1}p_1,x_2=K^{-1}p_2$

得到：
$x_2=Rx_1+t$
這裡的$x$是齊次座標，等式表達了一個齊次關係。

兩邊同時左乘${t‘}$，這個東西相當於與它做外積，得到的結果和它、$x_2$都垂直。因此再左乘一個$x_2^T$，就得到了0。
最終得到：
$x_2^Tt’Rx_1=0$

中間部分寫成$E$矩陣，稱為本質矩陣：
$x_2^TEx_1=0$
帶入$p_1$，$p_2$：
$p_2K^{-T}t ’RK^{-1}p_1=0,E=t^{‘}R$
中間寫成$F$矩陣，稱為基礎矩陣：
$p_2Fp_1=0,F=K^{-T}tRK^{-1}$
t’是t的反對稱矩陣。
這兩個式子稱為對極約束。根據對極約束，估計$R,t$的方法為：

1. 根據匹配點的位置，求出$E$或者$F$。
2. 根據$E$或者$F$，求出$R,t$。

2. 求解本質矩陣E

本質矩陣長什麼樣子：

• 大小為3x3
• 由平移和旋轉定義，共有3+3=6個自由度
• 因為對極約束，E具有尺度等價性，自由度-1，共五個。

八點法

根據對極約束，最少五對點就能求出來E，為了方便，一般使用八對點。
寫出由八對點構成的線性方程組，可以解出來E。

估計相機運動

對E做SVD，就能求出來$R,t$。可以得到四組解(深度正/負，相機位置的左/右)：

因為深度要是正的才合法，可以把解帶入一個點看看深度是否都為正(用三角測量？)，從中選出最終解。

3. 單應矩陣

描述了兩個平面之間的對映關係。
單應矩陣：H。自由度為8，可通過四對匹配特徵點算出。
單應性：當特徵點共面，或發生純旋轉時，自由度下降。

4. 單目SLAM的一些問題

尺度不確定性

t的尺度不確定性。對t乘任意倍數，對極約束依然成立。
單目SLAM的初始化：對t進行歸一化，固定尺度，或吃實話時，所有特徵點的平均深度為1。

純旋轉問題

若是純旋轉，t為0，則E為0，無法求解R。
單目初始化不能只有旋轉，必須要有平移。

多於八對點

計算最小二乘解。
或使用RANSAC，來避免誤匹配的影響。

5. 三角測量

單目SLAM(2d-2d)中，使用對極約束估計$R,t$，使用三角測量估計深度。

三、3D-2D PnP

• 給定：一張已知特徵點3D位置的影象，一張二維影象。
• 已知：特徵點在世界座標系(指的是相機最初位置的那個座標系)下的三維座標，特徵點對映到影象上的二維座標，三維- - 座標系中的特徵點與二維影象上點的匹配關係。
• 未知：相機座標系在世界座標系下的位姿，特徵點在相機座標系下的深度。
• 最初使用對極幾何，初始化出特徵點的深度。對於後續的二維影象（當前幀），當前幀與之前幀進行特徵點匹配，匹配到的點若之前已經估計出深度，就知道了世界座標系下的三維座標和當前幀二維點的匹配關係，可以用PNP求解當前幀的相機，相對於世界座標系的位姿變換。

1. 直接線性變換

用來求$R,t$。
待求變數$R,t$寫成增廣矩陣，共有3*4=12個未知量。
變換可得，一對匹配點能夠寫成兩個等式，最少通過6對匹配點，可以求出$R,t$。匹配點大於六對時，可以用SVD等方法求最小二乘解。
要注意使得求得的旋轉矩陣滿足約束。

2 . P3P

**用來求三維點在相機座標系下的深度。**得到深度後，再用ICP求位姿變換。
僅用三對匹配點，和一個驗證點。
已知$A,B,C$在世界座標系下的三維點座標，$a,b,c$是其在像平面的投影，已知其歸一化座標。
未知$A,B,C$在相機座標系下的座標。
根據三角形的相似關係和餘弦定理，可以推出：
$OA^2+OB^2-2OAOBcos<a,b>=AB^2$
$OB^2+OC^2-2OBOCcos<b,c>=BC^2$
$OA^2+OC^2-2OAOCcos<a,c>=AC^2$

記$x=OA/OC$，$y=OB/OC$，得：
$x^2+y^2-2xycos<a,b>=AB^2/OC^2$
$Y^2+1-2ycos<b,c>=BC^2/OC^2$
$x^2+1-2xcos<b,c>=AC^2/OC^2$

記$v=AB^2/OC^2,UV=BC^2/OC^2,wv=AC^2/OC^2$，得：