這篇部落格是在學習高翔《視覺SLAM十四講》過程中對計算機視覺的多視圖幾何相關知識點做的總結，個人覺得這部分内容比較難，有了解不對的地方請指正！
〇、ORB特征點
對于特征點法的SLAM來說，ORB(Oriented FAST and Rotated BRIEF)特征應該是目前最合适的特征點了。ORB與SIFT、SURF之間的性能對比如下：
- 計算速度：ORB >> SURF >> SIFT（各差一個量級）
- 旋轉魯棒性：SURF > ORB ~ SIFT
- 模糊魯棒性：SURF > ORB ~ SIFT
- 尺度變換魯棒性：SURF > SIFT > ORB（ORB算法在尺度方面效果較差）
特征點由關鍵點和描述子兩部分組成，對于ORB特征點來說，它使用的是FAST關鍵點+BRIEF描述子。
FAST是一種角點，主要檢測局部像素灰階變化明顯的地方，以速度快著稱。但FAST角點本身并不具有尺度和方向資訊，針對這一問題，ORB算法通過建構圖像金字塔，并在金字塔的每一層上檢測角點來實作FAST的尺度不變性；用灰階質心法來實作FAST的旋轉不變性。在ORB中，将這種可以描述角點尺度與旋轉特性的FAST稱為Oriented FAST。
BRIEF是一種二進制描述子，為關鍵點添加描述子的作用是為了實作圖像之間的特征比對。BRIEF使用了随機選點的比較，速度非常快，而且由于使用了二進制表達，存儲起來也十分友善。但原始的BRIEF描述子不具有旋轉不變性，圖像發生旋轉時容易丢失，針對這一問題，ORB算法利用在FAST角點提取階段計算出的關鍵點方向資訊，使BRIEF具有了較好的旋轉不變性。
有了ORB特征點，我們需要用它來做圖像間的特征比對。所謂特征比對，就是比較特征點的相似度，這個相似度通過它們的描述子來計算。對于BRIEF這種二進制的描述子，通常使用漢明距離（Hamming distance）來度量兩個特征點之間的相似程度。漢明距離是指兩個長度相等的字元串之間不同資料位的個數，例如：兩個8位的二進制串00110100與10110100，它們之間隻有第一個資料位不同，是以它們的漢明距離為1。在ORB的特征比對中，漢明距離越小，特征點的相似度越高，在某種程度上，我們可以認為兩個特征點在兩幀圖像中為同一個點，即完成了特征比對。
特征比對在視覺SLAM中真的是至關重要的一步，設想一下，假如每一幀圖像都能實作100%的正确比對，那我們就不再需要各種令人頭疼的濾波、優化、回環檢測等等，這将是一個多麼美好的畫面啊啊啊。。。但，現實告訴你，絕大部分的圖像之間是不可能做到100%正确比對的，是以說，SLAM的主要工作在後端，就是如何利用不準确的資料估計得到準确的結果。
當然，下面總結的在視覺SLAM中經常用到的，非常重要的，Multiple View Geometry in Computer Vision中的，比較難以了解又容易混淆的幾個算法，都是在理想的特征比對結果之上推導出來的。
一、對極幾何
對極幾何（Epipolar geometry）又叫對極限制，是根據圖像二維平面資訊來估計單目相機幀間運動或雙目相機相對位姿關系的一種算法。直覺來講，當相機在兩個不同視角對同一物體進行拍攝時，物體在兩幅圖像中的成像肯定會有不同，那麼，根據這兩幅不同的圖像，我們如何判斷出相機的位姿發生了怎樣的變化，這正是對極幾何要解決的問題。
需要明确的是，在對極幾何中，我們的已知條件僅僅是每幅圖像中特征點的像素坐标，當然，計算對極限制的前提是我們必須知道兩幅圖像中特征點之間準确的比對關系。
 |
如圖1所示，以其中一對比對點為例。 P P P為空間中的一點（坐标未知），其在左邊圖像中的投影為 p l = [ u l , v l , 1 ] T \color{#F00}p_l=[u_l,v_l,1]^\text T pl=[ul,vl,1]T，在右邊圖像中的投影為 p r = [ u r , v r , 1 ] T \color{#F00}p_r=[u_r,v_r,1]^\text T pr=[ur,vr,1]T，當我們以相機坐标系 O L O_L OL為參考坐标系時，有：
(1) { s l p l = K P s r p r = K ( R P + t )    ⟹    { s l K − 1 p l = P s r K − 1 p r = R P + t \begin{cases} s_lp_l = KP \\ s_rp_r = K(RP+t) \end{cases}\implies \begin{cases} s_lK^{-1}p_l = P \\ s_rK^{-1}p_r = RP+t \end{cases}\tag{1} {slpl=KPsrpr=K(RP+t)⟹{slK−1pl=PsrK−1pr=RP+t(1)
其中 s l s_l sl、 s r s_r sr分别為點 P P P在相機坐标系 O L O_L OL、 O R O_R OR中的 z z z坐标值（即深度）， K K K為 3 × 3 3\times 3 3×3的相機内參矩陣， R R R、 t t t即為 O L O_L OL與 O R O_R OR之間的相對位姿關系。取 x l = K − 1 p l ,   x r = K − 1 p r \color{#F0F}x_l=K^{-1}p_l,\,x_r=K^{-1}p_r xl=K−1pl,xr=K−1pr，則有：
(2) { s l x l = P s r x r = R P + t    ⟹    s r x r = R ( s l x l ) + t    ⟹    s r x r = s l R x l + t \begin{cases} s_lx_l = P \\ s_rx_r = RP+t \end{cases}\implies s_rx_r = R(s_lx_l)+t\implies s_rx_r = s_lRx_l+t\tag{2} {slxl=Psrxr=RP+t⟹srxr=R(slxl)+t⟹srxr=slRxl+t(2)
上式兩邊同時左乘 t t t的反對稱矩陣 t ∧ t^\wedge t∧，相當于兩側同時與 t t t做外積（ t t t與自身的外積 t ∧ t = 0 t^\wedge t=\bf 0 t∧t=0）：
(3) s r t ∧ x r = s l t ∧ R x l s_rt^\wedge x_r=s_lt^\wedge Rx_l\tag{3} srt∧xr=slt∧Rxl(3)
然後，兩側同時左乘 x r T x_r^{\text T} xrT：
(4) s r x r T t ∧ x r = s l x r T t ∧ R x l s_rx_r^{\text T}t^\wedge x_r=s_lx_r^{\text T}t^\wedge Rx_l\tag{4} srxrTt∧xr=slxrTt∧Rxl(4)
上式左側中， t t t與 x r x_r xr的外積 t ∧ x r t^\wedge x_r t∧xr是一個與 t t t和 x r x_r xr都垂直的向量，是以，再将 x r T x_r^{\text T} xrT與 t ∧ x r t^\wedge x_r t∧xr做内積，其結果必為 0 0 0，進而有：
(5) s l x r T t ∧ R x l = 0    ⟹    s l ( K − 1 p r ) T t ∧ R ( K − 1 p l ) = 0    ⟹    p r T K -T t ∧ R K − 1 p l = 0 \color{#F00}s_lx_r^{\text T}t^\wedge Rx_l=0\implies s_l(K^{-1}p_r)^{\text T}t^\wedge R(K^{-1}p_l)=0\implies p_r^{\text T}K^{\text {-T}}t^\wedge RK^{-1}p_l=0\tag{5} slxrTt∧Rxl=0⟹sl(K−1pr)Tt∧R(K−1pl)=0⟹prTK-Tt∧RK−1pl=0(5)
(5)式以非常簡潔的形式描述了兩幅圖像中比對點對 p l p_l pl和 p r p_r pr之間存在的數學關系，這種關系就叫對極限制。另外，我們稱 E = t ∧ R \color{#F00}E=t^\wedge R E=t∧R為本質矩陣（Essential Matrix），稱 F = K -T t ∧ R K − 1 = K -T E K − 1 \color{#F00}F=K^{\text {-T}}t^\wedge RK^{-1}=K^{\text {-T}}EK^{-1} F=K-Tt∧RK−1=K-TEK−1為基礎矩陣（Fundamental Matrix），于是(5)式可以進一步簡化為：
(6) x r T E x l = p r T F p l = 0 x_r^{\text T}Ex_l=p_r^{\text T}Fp_l=0\tag{6} xrTExl=prTFpl=0(6)
OK，現在我們需要重新明确一下對極幾何的工作：對極幾何是要利用已知的 n n n對如上述 p l p_l pl、 p r p_r pr這樣的比對點（二維像素坐标），來計算相機的運動 R R R、 t t t，即求解本質矩陣 E E E或基礎矩陣 F F F。具體求解過程可參看《視覺SLAM十四講》P143-148。
對極幾何存在的問題：根據對極限制求解得到的相機旋轉運動 R R R是準确的，平移運動 t t t是不具備真實尺度的。
二、三角測量
三角測量（Triangulation）又叫三角化，是根據前後兩幀圖像中比對到的特征點像素坐标以及兩幀之間的相機運動 R R R、 t t t，計算特征點三維空間坐标的一種算法。直覺來講，當有兩個相對位置已知的相機同時拍攝到同一物體時，如何根據兩幅圖像中的資訊估計出物體的實際位姿，即通過三角化獲得二維圖像上對應點的三維結構，這正是三角測量要解決的問題。
 |
如圖2所示為三角測量基本原理的示意圖，該圖與圖1非常相似，但應該注意其中的差別：此時我們已經知道了 O L O_L OL與 O R O_R OR的關系， p l p_l pl、 p r p_r pr也是已知的，要求的是點 P P P的三維空間坐标。由(1)式我們知道，要求點 P P P的三維坐标，需要先求深度 s s s；由(2)式我們有：
(7) s r x r = s l R x l + t s_rx_r = s_lRx_l+t\tag{7} srxr=slRxl+t(7)
對上式兩側左乘一個 x r x_r xr的反對稱矩陣 x r ∧ x_r^\wedge xr∧，得：
(8) s r x r ∧ x r = s l x r ∧ R x l + x r ∧ t    ⟹    s l x r ∧ R x l + x r ∧ t = 0 s_rx_r^\wedge x_r = s_lx_r^\wedge Rx_l+x_r^\wedge t\implies s_lx_r^\wedge Rx_l+x_r^\wedge t=\bf 0\tag{8} srxr∧xr=slxr∧Rxl+xr∧t⟹slxr∧Rxl+xr∧t=0(8)
(8)式是一個超定方程組（Overdetermined system），通常隻能求得它的最小二乘解。當我們求出 s l s_l sl後，根據(7)式就很容易求出 s r s_r sr，最後由(1)式，我們有：
(9) { s l K − 1 p l = P O L ， P O L 表示點 P 在相機坐标系 O L 下的三維坐标 s r K − 1 p r = P O R ， P O R 表示點 P 在相機坐标系 O R 下的三維坐标 \begin{cases} s_lK^{-1}p_l = P_{O_L}， & \text {$P_{O_L}$表示點$P$在相機坐标系$O_L$下的三維坐标 }\\ s_rK^{-1}p_r = P_{O_R}， & \text {$P_{O_R}$表示點$P$在相機坐标系$O_R$下的三維坐标 } \end{cases}\tag{9} {slK−1pl=POL，srK−1pr=POR，POL表示點P在相機坐标系OL下的三維坐标 POR表示點P在相機坐标系OR下的三維坐标 (9)
上式中 P O L P_{O_L} POL與 P O R P_{O_R} POR之間的關系為：
(10) P O R = R P O L + t P_{O_R}=RP_{O_L}+t\tag{10} POR=RPOL+t(10)
即：以 O L O_L OL坐标系為參考， O R O_R OR經過運動 R R R、 t t t變換到 O L O_L OL，那麼原來在 O L O_L OL坐标系下的點 P P P在 O R O_R OR坐标系下表示為 P O R = R P O L + t P_{O_R}=RP_{O_L}+t POR=RPOL+t。如果我們還知道相機與世界坐标系的變換關系 T T T，就可以将 P O L P_{O_L} POL或 P O R P_{O_R} POR轉換到世界坐标系下表示，得到點 P P P的世界坐标。
三、PnP問題
PnP（Perspective-n-Point）是根據圖像中特征點的二維像素坐标及其對應的三維空間坐标，來估計相機在參考坐标系中位姿的一類算法。直覺來講，當相機觀察到空間中的某一物體時，我們已經知道了該物體在某一參考坐标系下的位置和姿态，那麼如何通過圖檔中物體的成像判斷出相機此時在參考坐标系下的位姿？這正是PnP要解決的問題，即利用已知三維結構與圖像的對應關系求解相機與參考坐标系的相對關系（相機的外參）。
PnP是一類問題，針對不同的情況有不同的解法，常見的算法有：P3P、DLS、EPnP、UPnP等。
| |
如圖3所示，考慮空間中某個點 P P P，它在世界坐标系 O w O_w Ow中的齊次坐标為 P = [ x , y , z , 1 ] T P=[x,y,z,1]^{\text T} P=[x,y,z,1]T，其投影到圖像中的像素坐标為 p = [ u , v , 1 ] T p=[u,v,1]^{\text T} p=[u,v,1]T。假設此時相機坐标系 O c O_c Oc與世界坐标系 O w O_w Ow之間的相對位姿關系由 R R R、 t t t來描述，則有：
(11) s [ u v 1 ] = K [ R t ] [ x y z 1 ] = [ f x 0 u 0 0 f y v 0 0 0 1 ] [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ x y z 1 ] \color{#F00} s\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = K\left[\begin{array}{c|c}R&t\end{array}\right]\begin{bmatrix} x\\y\\z\\1 \end{bmatrix} = \begin{bmatrix} f_x&0&u_0 \\0 &f_y&v_0\\0&0&1\end{bmatrix} \left[ \begin{array}{ccc|c} r_{11}&r_{12}&r_{13}&t_1 \\ r_{21}&r_{22}&r_{23}&t_2 \\ r_{31}&r_{32}&r_{33}&t_3 \end{array} \right] \begin{bmatrix}x\\y\\z\\1\end{bmatrix}\tag{11} s⎣⎡uv1⎦⎤=K[Rt]⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎡fx000fy0u0v01⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤(11)
其中的 R = [ r 1 r 2 r 3 ] = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\begin{bmatrix}\bf r_1&\bf r_2&\bf r_3\end{bmatrix}=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix} R=[r1r2r3]=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤、 t = [ t 1 t 2 t 3 ] t=\begin{bmatrix}t_1\\t_2\\t_3\end{bmatrix} t=⎣⎡t1t2t3⎦⎤即為我們要求的旋轉矩陣與平移向量，其實它倆就是所謂的相機外參。通過相機的外參可以将世界坐标系下的點轉換到相機坐标系下表示，通過相機的内參可以将相機坐标系下的點轉換到像素坐标系下表示。
為了簡化表示，我們暫時不考慮相機内參（因為内參 K K K是已知且不變的），并令 T 34 = [ R t ] = [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] = [ t 11 t 12 t 13 t 14 t 21 t 22 t 23 t 24 t 31 t 32 t 33 t 34 ] = [ τ 1 τ 2 τ 3 ] T_{34}=\left[\begin{array}{c|c}R&t\end{array}\right]=\left[\begin{array}{ccc|c}r_{11}&r_{12}&r_{13}&t_1\\r_{21}&r_{22}&r_{23}&t_2\\r_{31}&r_{32}&r_{33}&t_3\end{array}\right]=\begin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\\t_{21}&t_{22}&t_{23}&t_{24}\\t_{31}&t_{32}&t_{33}&t_{34}\end{bmatrix} = \begin{bmatrix}\bf{\tau_1}\\ \bf{\tau_2}\\ \bf{\tau_3}\end{bmatrix} T34=[Rt]=⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤=⎣⎡t11t21t31t12t22t32t13t23t33t14t24t34⎦⎤=⎣⎡τ1τ2τ3⎦⎤，則上式可表示為：
s [ u v 1 ] = [ t 11 t 12 t 13 t 14 t 21 t 22 t 23 t 24 t 31 t 32 t 33 t 34 ] [ x y z 1 ] = [ τ 1 τ 2 τ 3 ] P s\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} =\begin{bmatrix}t_{11}&t_{12}&t_{13}&t_{14}\\t_{21}&t_{22}&t_{23}&t_{24}\\t_{31}&t_{32}&t_{33}&t_{34}\end{bmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix} = \begin{bmatrix}\bf{\tau_1}\\ \bf{\tau_2}\\ \bf{\tau_3}\end{bmatrix}P s⎣⎡uv1⎦⎤=⎣⎡t11t21t31t12t22t32t13t23t33t14t24t34⎦⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎡τ1τ2τ3⎦⎤P
用最後一行把 s s s 消去（ s = τ 3 P s={\bf\tau_3}P s=τ3P），得到兩個限制：
u = τ 1 P τ 3 P ， v = τ 2 P τ 3 P u=\frac{{\bf\tau_1}P}{{\bf\tau_3}P} \quad，v=\frac{{\bf\tau_2}P}{{\bf\tau_3}P} \quad u=τ3Pτ1P，v=τ3Pτ2P
于是有：
(12) { τ 1 P − τ 3 P   u = 0 , τ 2 P − τ 3 P   v = 0. \begin{cases} {\bf\tau_1}P-{\bf\tau_3}P\,u=0, \\ {\bf\tau_2}P-{\bf\tau_3}P\,v=0. \end{cases}\tag{12} {τ1P−τ3Pu=0,τ2P−τ3Pv=0.(12)
可以看到，每個特征點提供了兩個關于 T 34 T_{34} T34 的線性限制，而 T 34 T_{34} T34 中有12個未知數，是以理論上至少需要6對比對點就可以求出 T 34 T_{34} T34 。
四、ICP問題
ICP（Iterative Closest Point）是根據前後兩幀圖像中比對好的特征點在相機坐标系下的三維坐标，求解相機幀間運動的一種算法。直覺來講，當相機在某處觀察某一物體時，我們知道了相機此時與物體之間的相對位姿關系；當相機運動到另一處，我們亦知道此時相機與物體的相對位姿關系，那麼，如何通過這兩次相機與物體的相對位姿關系來确定相機發生了怎樣的運動？這正是ICP要解決的問題。
在ICP問題中，圖像資訊僅僅用來做特征點的比對，而并不參與視圖幾何的運算。也就是說，ICP問題的求解用不到相機的内參與特征點的像素坐标。
| |
如圖4所示，設空間中一點 P P P在相機坐标系 O L O_L OL下的坐标為 P O L = [ x l , y l , z l ] T P_{O_L}=[x_l,y_l,z_l]^{\text T} POL=[xl,yl,zl]T，在相機坐标系 O R O_R OR下的坐标為 P O R = [ x r , y r , z r ] T P_{O_R}=[x_r,y_r,z_r]^{\text T} POR=[xr,yr,zr]T， O L O_L OL經過運動 R R R、 t t t到達 O R O_R OR，則 P O L P_{O_L} POL與 P O R P_{O_R} POR之間的關系為：
(13) P O L = R P O R + t P_{O_L}=RP_{O_R}+t\tag{13} POL=RPOR+t(13)
寫成齊次變換的形式：
(14) [ x l y l z l 1 ] = [ R t ] [ x r y r z r 1 ] = [ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 ] [ x r y r z r 1 ] \begin{bmatrix} x_l \\ y_l \\ z_l \\ 1 \\ \end{bmatrix}=\left[\begin{array}{c|c}R&t\end{array}\right]\begin{bmatrix} x_r \\ y_r \\ z_r \\ 1 \\ \end{bmatrix}=\left[\begin{array}{ccc|c}r_{11}&r_{12}&r_{13}&t_1\\r_{21}&r_{22}&r_{23}&t_2\\r_{31}&r_{32}&r_{33}&t_3\end{array}\right]\begin{bmatrix} x_r \\ y_r \\ z_r \\ 1 \\ \end{bmatrix}\tag{14} ⎣⎢⎢⎡xlylzl1⎦⎥⎥⎤=[Rt]⎣⎢⎢⎡xryrzr1⎦⎥⎥⎤=⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤⎣⎢⎢⎡xryrzr1⎦⎥⎥⎤(14)
由此可見，每一個點 P P P可以得到3個限制，那麼理論上至少需要4個點就可以求得相機的運動 R R R、 t t t。