1. 理解圖像特征點的意義, 並掌握在單幅圖像中提取出特征點，及多幅圖像中匹配
特征點的方法。
2. 理解對極幾何的原理，利用對極幾何的約束，恢復出圖像之間的攝像機的三維運
動。
3. 理解 PNP 問題，及利用已知三維結構與圖像的對應關系，求解攝像機的三維運
動。
4. 理解 ICP 問題，及利用點雲的匹配關系，求解攝像機的三維運動。
5. 理解如何通過三角化，獲得二維圖像上對應點的三維結構。
7.1 特征點法
前端也稱為視覺里程計（VO） ，根據相鄰圖像的信息，估計出粗略的相機運動，給后端提供較好的初始值。
VO分為兩種：特征點法的前端以及不提特征的直接法前端 。
基於特征點法的前端運行穩定，對光照、動態物體不敏感，目前比較成熟 。
7.1.1 特征點
長年的研究中，設計了許多更加穩定的局部圖像特征，如著名的 SIFT（計算量大）, SURF,ORB 等等。相比於朴素的角點，這些人工設計的特征點能夠擁有如下的性質：
1. 可重復性（Repeatability）：相同的“區域”可以在不同的圖像中被找到。
2. 可區別性（Distinctiveness）：不同的“區域”有不同的表達。
3. 高效率（Efciency）：同一圖像中，特征點的數量應遠小於像素的數量。
4. 本地性（Locality）：特征僅與一小片圖像區域相關。
特征點由關鍵點（Key-point）和描述子（Descriptor）兩部分組成
7.1.2 ORB特征
ORB 特征亦由關鍵點和描述子兩部分組成。它的關鍵點稱為“Oriented FAST”，是一種改進的 FAST 角點，什么是 FAST 角點我們將在下文介紹。
’它的描述子稱為 BRIEF（Binary Robust Independent Elementary Features）。因此，提取 ORB 特征分為兩個步驟：
1. FAST 角點提取：找出圖像中的” 角點”。相較於原版的 FAST, ORB 中計算了特征點的主方向，為后續的 BRIEF 描述子增加了旋轉不變特性。
2. BRIEF 描述子：對前一步提取出特征點的周圍圖像區域進行描述
FAST 是一種角點，主要檢測局部像素灰度變化明顯的地方，以速度快著稱。它的思想是：如果一個像素與它鄰域的像素差別較大（過亮或過暗） , 那它更可能是角點。相比於其他角點檢測算法， FAST 只需比較像素亮度的大小，十分快捷。
1. 在圖像中選取像素 p，假設它的亮度為 Ip。
2. 設置一個閾值 T (比如 Ip 的 20%)。
3. 以像素 p 為中心, 選取半徑為 3 的圓上的 16 個像素點。
4. 假如選取的圓上，有連續的 N 個點的亮度大於 Ip + T 或小於 Ip - T，那么像素 p可以被認為是特征點 (N 通常取 12，即為 FAST-12。其它常用的 N 取值為 9 和 11， 他們分別被稱為 FAST-9， FAST-11)。
5. 循環以上四步，對每一個像素執行相同的操作。
預測試操作 ：每個像素，直接檢測鄰域圓上的第 1， 5， 9， 13 個像素的亮度 。
原始的 FAST 角點經常出現“扎堆”的現象。所以在第一遍檢測之后，還需要用非極大值抑制（Non-maximal suppression），在一定區域內僅保留響應極大值的角點，避免角點集中的問題。
FAST 特征點數量很大且不確定，而我們往往希望對圖像提取固定數量的特征。
因此，在 ORB 中，對原始的 FAST 算法進行了改進。我們可以指定最終要提取的角點數量N，對原始 FAST 角點分別計算 Harris 響應值，然后選取前 N 個具有最大響應值的角點，作為最終的角點集合。
FAST 角點不具有方向信息。而且，由於它固定取半徑為 3 的圓，存在尺度問題：遠處看着像是角點的地方，接近后看可能就不是角點了。針對 FAST 角點不具有方向性和尺度的弱點， ORB 添加了尺度和旋轉的描述。
尺度不變性由構建圖像金字塔‹，並在金字塔的每一層上檢測角點來實現。而特征的旋轉是由灰度質心法（Intensity Centroid）實現的。
BRIEF 描述子
BRIEF 是一種二進制描述子，它的描述向量由許多個 0 和 1 組成，這里的 0 和 1 編碼了關鍵點附近兩個像素（比如說 p 和 q）的大小關系：如果 p 比 q 大，則取 1，反之就取 0。
ORB 在 FAST 特征點提取階段計算了關鍵點的方向，所以可以利用方向信息，計算了旋轉之后的“Steer BRIEF”特征，使 ORB 的描述子具有較好的旋轉不變性。
7.1.3特征匹配
快速近似最近鄰（FLANN） 集成在opencv
7.2 實踐 特征提取和匹配
7.3 2D-2D 對極幾何
求取兩幀圖像 I1; I2 之間的運動，設第一幀到第二幀的運動為R; t。兩個相機中心分別為 O1; O2。現在，考慮 I1 中有一個特征點 p1，
它在 I2 中對應着 特征點 p2 。這里我們需要一些術語來描述它們之間的幾何關系。首先，連線 O1p1 和連線 O2p2 在三維空間中會相交於點 P。這時候點 O1; O2; P 三個點可以
確定一個平面，稱為極平面（Epipolar plane） 。 O1O2 連線與像平面 I1; I2 的交點分別為 e1; e2。 e1; e2，稱為極點（Epipoles） ， O1O2 被稱為基線（Baseline） 。稱極平面與
兩個像平面 I1; I2 之間的相交線 l1; l2 為極線（Epipolar line） 。
通過特征匹配的p1,p2就可以確定p的值。
以上兩個式子就是對極約束。對極約束中同時包含了平移和旋轉
基礎矩陣（Fundamental Matrix） F 和本質矩陣（Essential Matrix） E，
相機位姿估計問題變為以下兩步
1. 根據配對點的像素位置，求出 E 或者 F；
2. 根據 E 或者 F，求出 R; t。
7.3.2本質矩陣
E 在不同尺度下是等價的
本質矩陣 E 的奇異值必定是 [σ; σ; 0]T
只考慮它的尺度等價性，使用八對點來估計 E——這就是經典的八點法 
7.3.3單應矩陣
單應矩陣通常描述處於共同平面上的一些點，在兩張圖像之間的變換關系。
無人機俯視相機、掃地機頂視相機
考慮在圖像 I1 和 I2 有一對匹配好的特征點 p1 和 p2。這些特征點落在某平面上。設這個平面滿足方程： 
這種做法把 H 矩陣看成了向量，通過解該向量的線性方程來恢復 H，又稱直接線性變換法（Direct Linear Transform）。
單應性在 SLAM 中具重要意義。當特征點共面，或者相機發生純旋轉的時候，基礎矩陣的自由度下降，這就出現了所謂的退化（degenerate）。
能夠避免退化現象造成的影響，通常我們會同時估計基礎矩陣 F 和單應矩陣H，選擇重投影誤差比較小的那個作為最終的運動估計矩陣
7.5 三角測量
三角測量是指，通過在兩處觀察同一個點的夾角，確定該點的距離。
然而由於噪聲的影響，O1P1 O2P2直線往往無法相交。 通過最二小乘去求解。
已經知道了 R; t，想要求解的是兩個特征點的深度 s1; s2。
理想條件下求零解，常見做法最小二乘
7.7 3D-2D ： PnP
PnP（Perspective-n-Point）是求解 3D 到 2D 點對運動的方法。它描述了當我們知道n 個 3D 空間點以及它們的投影位置時，如何估計相機所在的位姿。
如果兩張圖像中，其中一張特征點的 3D 位置已知，那么最少只需三個點對（需要至少一個額外點驗證結果）就可以估計相機運動。
特征點的 3D 位置可以由三角化，或者由 RGB-D 相機的深度圖確定。因此，在雙目或 RGB-D 的視覺里程計中，我們可以直接使用 PnP 估計相機運動。
在單目視覺里程計中，必須先進行初始化，然后才能使用 PnP。 3D-2D 方法不需要使用對極約束，又可以在很少的匹配點中獲得較好的運
動估計，是最重要的一種姿態估計方法。
PnP 問題有很多種求解方法，例如用三對點估計位姿的 P3P[45]，直接線性變換（DLT），EPnP（Efcient PnP） [46]， UPnP[47] 等等）。此外，還能用非線性優化的方式，構建最
小二乘問題並迭代求解，也就是萬金油式的 Bundle Adjustment。我們先來看 DLT，然后再講 Bundle Adjustment。 
P3P 需要利用給定的三個點的幾何關系。它的輸入數據為三對 3D-2D 匹配點
該方程組是關於 x; y 的一個二元二
次方程（多項式方程）。 
該方程組是關於 x; y 的一個二元二次方程（多項式方程）。 該方程最多可能得到四個解，但我們可以用驗證點來計算最可能的解，得到 A; B; C 在相機坐標系下 的 3D 坐標。然后，根據 3D-3D 的點對，計算相機的運動 R; t。
為了求解 PnP，我們利用了三角形相似性質，求解投影點 a; b; c 在相機坐標系下的 3D 坐標，最后把問題轉換成一個 3D 到 3D 的位姿估計問題。
1. P3P 只利用三個點的信息。當給定的配對點多於 3 組時，難以利用更多的信息。
2. 如果 3D 點或 2D 點受噪聲影響，或者存在誤匹配，則算法失效。
所以后續人們還提出了許多別的方法，如 EPnP、 UPnP 等。它們利用更多的信息，而且用迭代的方式對相機位姿進行優化，以盡可能地消除噪聲的影響。
7.7.3 Bundle Adjustment
除了使用線性方法之外，我們可以把 PnP 問題構建成一個定義於李代數上的非線性最小二乘問題。這將用到本書第四章和第五章的知識。
構建最小二乘問題，然后尋找最好的相機位姿，使它最小化
該問題的誤差項，是將像素坐標（觀測到的投影位置）與 3D 點按照當前估計的位姿進行投影得到的位置相比較得到的誤差，所以稱之為重投影誤差。
最小二乘優化問題已經在第六講介紹過了。使用李代數，可以構建無約束的優化問題 在使用 G-N 和 L-M 之前，我們需要知道每個誤差項關於優化變量的導數，也就是線性化
這里的 J 的形式是值得討論的，甚至可以說是關鍵所在。 后面看不懂了....先把公式貼上來。
記變換到相機坐標系下的空間點坐標為 P ′，並且把它前三維取出來： 
相機投影模型相對於 P ′ 則為： 
⊕ 指李代數上的左乘擾動。 。。。。。
結果2 × 6 的雅可比矩陣： 
一方面，除了優化位姿，我們還希望優化特征點的空間位置。因此，需要討論 e 關於空間點 P 的導數。所幸這個導數矩陣相對來說容易一些。仍利用鏈式法則，有：
7.9 ICP
3D-3D 的位姿估計問題。
有一組配對好的 3D 點
想要找一個歐氏變換 R; t，使得：
可以用迭代最近點（Iterative Closest Point, ICP）求解。 ICP 的求解也分為兩種方式：利用線性代數的求解（主要是 SVD），以及利用非線性優化方式的求解（類似於 Bundle Adjustment）。
各環節