在無人駕駛中，感知、定位、規(guī)劃決策、控制是四個基本的系統(tǒng)模塊。由于當前算法還無法實現(xiàn)絕對的智能，因此依然需要大量的先驗知識來提高模塊性能、魯棒性，以實現(xiàn)安全的自動駕駛。其中，高精地圖是對道路及周邊環(huán)境先驗知識的集成。而建立在地圖之上的準確定位，是判斷行車狀況的重要依據(jù)，為后續(xù)的感知、規(guī)劃決策提供有力支撐。

用于定位的主要數(shù)據(jù)源目前主要有 GPS、激光雷達、視覺、毫米波雷達。對于視覺而言，雖然目前還沒有一套產(chǎn)業(yè)內(nèi)公認的足夠可靠的定位方案，但是在這方面探索從未停止過，主要原因如下:

安全性是無人駕駛系統(tǒng)最重要的指標，因此大部分功能的實現(xiàn)，都是多源數(shù)據(jù)、不同算法結(jié)果的耦合。沒有哪種傳感器方案是完美的，比如 GPS RTK 作為廣泛使用的方案，容易受衛(wèi)星狀況、天氣狀況、 數(shù)據(jù)鏈傳輸狀況影響，在隧道內(nèi)、室內(nèi)和高樓密集區(qū)無法使用。再者，激光雷達雖然具有運算量小，提供深度信息，不受光照影響等優(yōu)點，但信息稀疏，造價目前還十分昂貴，還不具備大批量車輛裝配能力。相比較而言，攝像頭提供的視覺信息，雖然會受到光照、天氣影響，但是成本低，內(nèi)容豐富，是目前輔助駕駛方案主要數(shù)據(jù)源，在地圖定位方面也具有很大潛力。

由于主流基于視覺定位算法的核心思想一脈相承，所以本文僅從一系列重要算法框架組件角度，介紹了目前實踐中最常用的、基于特征點的全局定位算法，即在地圖坐標系下進行定位。本文省略了其中涉及到的優(yōu)化、幾何約束公式推導(dǎo)，旨在給同學(xué)們一個定位算法的宏觀介紹，具體細節(jié)可以參考相關(guān)文獻和書籍。

基于特征點全局定位算法

視覺全局定位，指的是根據(jù)當前圖像，求出相機在地圖坐標系中的 6 個自由度 (Degree of freedom, DoF) 位姿 (Pose) , 即  (x, y, z) 坐標，以及環(huán)繞三個坐標軸的角度偏轉(zhuǎn) (yaw, pitch, roll) 。目前主要可以分類為基于 3D 結(jié)構(gòu)的方法、基于 2D 圖像的方法、基于序列圖像的方法、基于深度學(xué)習的方法。其中，基于深度學(xué)習的方法屬于端到端 (End-to-end) 的方法，而其它多階段 (Multi-stage) 非端到端方法雖然流程有所差別，但算法思路大都如 Fig. 1 所示:

Figure 1: 根據(jù)查詢圖像，計算 2D-3D 轉(zhuǎn)換矩陣，求解相機位姿

基于已建的地圖，匹配歷史中最相似的地圖子集（圖像/點云/特征點），根據(jù)匹配到的地圖子集所提供的歷史位姿真值、特征點坐標真值，計算點對間的變換矩陣，求解當前相機位姿。

所以，其核心包含圖像描述、建圖查詢、特征匹配，位姿計算四個方面。這里僅僅是技術(shù)層面的宏觀分類，實際算法框架不一定按照此順序執(zhí)行，而學(xué)者在研究中主要針對這些技術(shù)進行改進。整體而言，基于特征點的圖像描述基本成熟，發(fā)展較少。而位姿計算由于是基于幾何約束的優(yōu)化問題，所以方法也較為固定。相對地，建圖查詢和特征匹配中改進技術(shù)較多。根據(jù)數(shù)據(jù)源不同，建圖查詢、匹配可以是2D-2D，2D-3D，3D-3D。2D 圖像由相機得到，3D 點云可以由提供深度的雙目相機、RGB-D 相機產(chǎn)生。

特征點提取

2D 圖像本身是一個由亮度、色彩組成的矩陣，對視角、光照、色調(diào)變化等很敏感，直接使用十分困難。所以，一般會使用具有代表性的點進行相關(guān)計算。人們希望這樣的點具有旋轉(zhuǎn)、平移、尺度、光照不變性等優(yōu)點。這些點稱為圖像的特征 (Feature) 點，包含關(guān)鍵點(Key-points) 和描述子 (Descriptor) 兩部分。關(guān)鍵點表達了特征點的位置，而描述子則是對于特征點視覺特性的描述，大多為向量形式。一般而言，描述子主要是以某種模式，統(tǒng)計關(guān)鍵點周圍的灰度/色彩梯度變化。一種魯棒的描述子，在不同圖像 的不同情況下，同一特征點的描述子的距離 (Distance) 應(yīng)當較小。

描述子一般是人為手工設(shè)計的 (Hand-crafted features) 。經(jīng)典的描述如 HOG(Histogram of oriented gradients)[1]，SIFT(Scale-invariant feature transform)[2]，SURF(Speeded up robust features)[3]，AKAZE(Accelerated KAZE)[4] 等。

為了實時性的要求，一些計算速度更快的二值模式描述子被設(shè)計出來，如 LBP(Local binary patterns)[5]，BRIEF(Binary robust independent elementary features)，ORB(Oriented FAST and rotated BRIEF)[6]，BRISK(Binary robust invariant scalable key-point)[7]，F(xiàn)REAK(Fast retina key-point)[8] 等。

在深度學(xué)習流行之前，這些手工特征一直引領(lǐng)著整個計算視覺產(chǎn)業(yè)，直到今天，這些特征在那些缺少標注數(shù)據(jù)、約束較多的場景下，依然被廣泛應(yīng)用。下面簡單介紹兩類常用的描述子。

SIFT

SIFT 描述子可以算是 CV 界最具影響力的技術(shù)之一。從關(guān)鍵點檢測層面，主要使用高斯差分 (Difference of Gaussian, DoG) 方法檢測多尺度空間上的極值點，作為關(guān)鍵點。而 Babaud 等人 [9] 證明了高斯平滑是唯一的能用多尺度空間平滑濾波核，為相關(guān)方法提供了充足的理論支持。

那么為什么這樣的方法可以找到特征關(guān)鍵點呢?

由于高斯核可以通過模糊的方式把圖像縮放到不同尺度空間，而梯度變化較小的平滑區(qū)域在不同尺度空間的值差距較小。相反，邊緣、點、角、紋理等區(qū)域則差距較大。這樣通過對相鄰尺度的圖像做差分，最終可以算得多尺度空間的極值點。但是，不同的圖像細節(jié)本身就處于不同的尺度中。比如一副人物畫像中，人臉可能經(jīng)過較小的模糊就會被平滑為一片，而畫框的角則可能需要更大尺度的平滑才會體現(xiàn)出局部“極值”。

因此，如 Fig. 2 所示，首先利用圖像金字塔將圖像先分組 (Octave) ，每組中再使用不同尺度的高斯核，形成一系列的層。這種方式比單純地使用更多尺度的高斯核效果更好，可以檢測到更多的特征點。需要注意的是，雖然 SIFT 使用了 DoG 進行關(guān)鍵點檢測，但是其它檢測方法也是可行的，并不影響 SIFT 描述子的建立。

Figure 2: 高斯差分方法

SIFT 特征點的描述子，可以理解為一種簡單統(tǒng)計版的 HOG。如 Fig. 3所示，以檢測到的關(guān)鍵點為中心，選取周圍 16 × 16 的區(qū)域，將區(qū)域再組織為 4 個 4 × 4 的塊(Patch)。對每一個塊，使用 8-bins 的直方圖對梯度進行統(tǒng)計，梯度方向決定落入哪個 bin，而梯度的模決定值的大小。為了保證尺度一致性，梯度大小需要進行歸一化。為了保證旋轉(zhuǎn)不變性，會根據(jù) 16 × 16 的區(qū)域內(nèi)的所有梯度計算出一個主方向， 所有梯度按照主方向進行旋轉(zhuǎn)。最終形成 4 × 4 × 8 的 128 維向量。

Figure 3: 基于梯度分塊統(tǒng)計的 SIFT 描述子

二值描述子

雖然在 SIFT 提出后，又產(chǎn)生了一些改進算法如 SURF、AKAZE 等，但是即使放在 2019 年的今天， 依然難以保證一些場景對算法實時性的要求。例如，手持設(shè)備一般算力有限。而無人駕駛中，CPU、GPU資源需要被多個計算密集型模塊同時調(diào)度。因此，效率是考察算法實用性的重要指標。

為了提高效率，一些二值描述子被學(xué)者們提出。一般地，這些方法都是在特征關(guān)鍵點周圍進行點采樣。然后比較一對點的灰度大小，結(jié)果以 0/1 表示，形成 N 維的二進制描述向量，構(gòu)成特征點的二值模式。而不同二值描述子最大的差別，主要在于特征采樣模式不同、點對選取方法不同。

Figure 4: LBP 描述子采樣模式

如 Fig. 4所示，LBP 描述子采用對關(guān)鍵點周圍，進行環(huán)形采樣，并與中心關(guān)鍵點的灰度進行比較的方案。圓環(huán)上展示了灰度比較結(jié)果，黑色的點是 0，白色的點是 1。LBP 是二值描述子最簡單的形式，而 ORB 改進了 BRIEF 特征，是目前比較常用的二值描述子。如 Fig. 5所示，在點對選取上，與單純使用中心點不同，ORB 采用了隨機的方式，更全面地描述局部細節(jié)。但點對的相關(guān)性會比較大，從而降低描述子的判別性(Discriminative)。ORB 直接采用了貪婪法、窮舉法解決這一問題，尋找相關(guān)性低的隨機點對。

Figure 5: ORB 描述子點對選取模式

以上二值描述子的采樣方式和點對選取方式符合人們一般直覺，而 BRISK、FREAK 等描述子則提供了更加規(guī)則化、自帶尺度信息的二值模式構(gòu)建方法。例如，F(xiàn)REAK 描述子模仿了人眼的視覺采樣模式。如 Fig. 6所示，每個采樣點的值是紅色圓圈范圍內(nèi)的灰度均值，藍線則表示點對選取方案。

Figure 6: FREAK 描述子采樣、點對選取摸式

二值描述子的高效率，主要體現(xiàn)在三個方面。

(1)二值描述子使用二進制向量作為特征描述，只需要 比較點對大小而不需要計算具體梯度。

(2)兩個描述子之間比較可以使用計算更快，更容易優(yōu)化的漢明距離 (Hamming distance)。

(3)由于每個二進制向量都對應(yīng)一個十進制數(shù)，所以其本身也代了表一種模 式，而不需要像 SIFT 一樣使用直方圖進行表示。

二值描述子一般判別性不如 SIFT 家族描述子，但在特定場景下，配合并行化編程，可以在保證相似判別能力的同時，效率高出幾十甚至百倍。

數(shù)據(jù)庫建立與查詢

數(shù)據(jù)庫可以理解為于地圖 + 索引的集成。地圖可以是由單純的 2D 圖像組成，也可以是由 3D 點云地圖組成，也可以是 2D 圖像和 3D 點云的結(jié)合。3D 點云地圖生成主要使用三維重建的方法 SfM(Structure from motion)，從時間序列的 2D 圖像中推算 3D 信息。如果有雙目、RGB-D 相機提供深度，可以獲得 更準確的 3D 點信息。其中也包含了一些諸如關(guān)鍵幀(Key-frame)的選取策略，具體方法超出了本文的討論范圍，有興趣的同學(xué)可以自行查閱相關(guān)資料。數(shù)據(jù)庫的作用在于:

對于一張輸入的觀測圖像，通過數(shù)據(jù)庫，查詢建圖歷史（圖像/點云/特征點），得到當前圖像最可能觀測到的地圖子集（圖像/點云/特征點），將地圖與觀測信息進行匹配，計算變換矩陣，得到觀測相機的位姿。

索引則是加速這一過程的關(guān)鍵。數(shù)據(jù)庫本身往往是巨大的。以美團的小袋機器人在北京朝陽大悅城二層試運營為例，安裝有 3 個深度相機，即使經(jīng)過篩選，也使用了將近 8 萬張 900 × 600 的圖片?？紤]到定位所需要的實時性，查詢時不可能每次都和 8 萬張圖片一一對比，所以要使用索引技術(shù)加速整個算法。這方面技術(shù)與 SLAM 中的回環(huán)測試，視覺中的圖像檢索、位置識別等高度重合，以下僅介紹一般方法。

一張圖像內(nèi)有若干特征點，需要先對特征點進行編碼，如 VLAD(Vector of locally aggregated descriptors) 編碼，用局部描述子形成圖像的全局描述。再使用索引，如 kd-tree，進行圖像級查詢。當然，編碼和索引也可以同時進行，如層次化詞袋模型(Bag-of-words，BoW)+ 正向索引 + 逆向索引的方法。

VLAD 編碼

VLAD(Vector of locally aggregated descriptors)[10]，如 Fig. 7所示，是一種通過聚合局部描述子形成碼本 (Codebook) ，通過累加計算描述子與碼詞 (Word) 的距離，進行全局編碼的簡單方法。一個  維描述子  通過  個碼詞的碼本進行編碼，可以形成一個  維的描述向量，向量中的值是描述子與第  個碼詞在第  維的差。之后進行  歸一化，形成最后的 VLAD 向量。

Figure 7: VLAD 通過描述子與碼詞的距離進行編碼

這里要特別提介紹一下 DenseVLAD[11] 和 NetVLAD[12] 。Torii 等人證明，DenseSIFT 在查詢、匹配上都優(yōu)于標準 SIFT。DenseVLAD 在四個尺度，以 2 個像素間隔的網(wǎng)格狀采樣模式，提取 SIFT 點。在全局隨機采樣 25M 個描述子，用 k-means 算法生成 128 個碼詞的碼本。VLAD 向量在歸一化后使用 PCA(Principal component analysis) 降維，形成最后 4096 維的 DenseVLAD 向量。如 Fig. 8所示，使用DenseSIFT 匹配后的內(nèi)點（綠）數(shù)量更多。

Figure 8: DenseSIFT 和標準 SIFT 特征點，匹配后內(nèi)點(綠)對比

而 NetVLAD，將 VLAD 中加入了監(jiān)督信息，加強 VLAD 編碼的判別性。如 Fig. 9所示，假設(shè)紅、綠兩個描述子來源于不應(yīng)匹配到一起的兩張圖片。由于它們都離 VLAD 中心(×)半徑較大且距離相似，經(jīng)過 L2 歸一化，它們編碼后值也會很相似。而加入了紅、綠描述子所對應(yīng)圖片不匹配的監(jiān)督信息后，NetVLAD 生成的中心點(★)則可以更好地區(qū)分兩個描述子，增加他們編碼后的距離（半徑）差。

Figure 9: NetVLAD 聚類中心(×)與 VLAD 聚類中心(★)對比BoW 編碼 + 索引基于詞袋模型 BoW[13, 14] 的特征編碼及其設(shè)計思想在計算機視覺發(fā)展中具有舉足輕重的地位，這里不再展開介紹。本文以 2D 查詢圖像匹配 2D 圖像數(shù)據(jù)庫為例，介紹一種常見的 BoW 編碼、索引一體化的模型。如 Fig. 10所示，詞典 (Vocabulary) 生成采用層次化方法，對于數(shù)據(jù)集中的所有描述子，按樹狀結(jié)構(gòu)進行空間劃分，每一層都是由 k-means 聚類計算。最終葉子節(jié)點就相當于碼詞(Fig. 10中有 9個碼詞)。

Figure 10: 帶正向索引、逆向索引的層次化 BoW 模型

樹的構(gòu)造過程，實際上就是將原始圖像編碼的過程。但是編碼本身并不能加快搜索過程，與 VLAD 相似，還是需要與數(shù)據(jù)庫中的圖像逐一比較。因此，這里設(shè)計了一種逆向索引(Inverse index) ，不需要比較編碼后的向量。其原理如 Fig. 11所示，對于一張查詢圖像 (Query image) ，將提取的描述子輸入到 BoW 中，最終會落入碼詞葉子結(jié)點 (Visual word) k 中。而每個碼詞對應(yīng)一個索引，記錄碼詞  對于數(shù)據(jù)庫中第  張圖的權(quán)重  (Fig.10)。這里權(quán)重使用 TF-IDF(Term frequency–inverse document frequency) 計算。即如果一個詞  在某個圖像  中出現(xiàn)頻率高，在其它圖像出現(xiàn)頻率低，則這個詞對于圖像判別性較好，權(quán)重值  較高。最終通過投票 (Voting) 機制，選出匹配圖像。同樣需要注意的是，逆向索引不一定建立在樹形結(jié)構(gòu)的 BoW 上，它僅僅是提供一種快速查詢的方法。

Figure 11: 通過逆向索引 + 投票機制，直接查詢圖像

而正向索引 (Direct Index) 的作用主要是記錄構(gòu)造 BoW 時，數(shù)據(jù)庫圖片的特征點都落入了哪些結(jié)點中，這樣當查詢到圖像后，不需要計算特征點，可以直接通過索引提取特征點。

3D 點云查詢

2D 圖像查詢中，是先從語意層面查詢圖像，因此可以通過圖像對特征點的空間范圍進行約束。3D 點云查詢沒有這樣的約束，所以具諸多難點。如需要考慮空間連續(xù)性，查詢到的點是否都在可觀測范圍內(nèi)等。這里僅介紹 Sattler 在 TPAMI 2016 上發(fā)表的方法 [15]，經(jīng)過多年的打磨，這套方法框架相對簡潔、完善。由于其中的詞典編碼搜索步驟與上節(jié)內(nèi)容有所重疊，這里僅介紹 Active Search 和 Visbility Filtering 兩種機制。

Active Search 主要是為了使得匹配到的 3D 點盡可能空間中臨近、有幾何意義。如 Fig. 12所示，紅 色的點通過一系列編碼、精化過程(紅線)，匹配到了點云中一個點。根據(jù)所提出優(yōu)先排序(Prioritization) 框架，從點云中找到一個概率最大的 3D 點，并反向(藍線)匹配查詢圖像中的一個對應(yīng)的 2D 點。

Figure 12: Active Search

Figure 13: Visbility Filtering Visbility Filtering

主要是為了讓匹配到的點盡可能可以被相機觀測到（定位是無監(jiān)督的，并不能知道所匹配到的點是否正確）。這里采用的方法是在使用 SfM 建立 3D 點云地圖時，同時建立一個雙向可見圖 (Bipartite visibility graph) 。如 Fig. 13（左）所示，當一個點可以同時被兩個相機觀測時，則建立拓撲關(guān)系。Fig. 13（中）里，藍色的點為匹配到的點，它們從觀測視角上存在沖突。通過在已有拓撲上進 行圖聚類，將相機兩兩分組，如 Fig. 13（右）。這樣就可以生成新的圖拓撲關(guān)系。之后通過判斷每個子圖(Sub-graph)間的重合情況，過濾掉那些那大概率不可見的點。

需要說明的是，雖然雙目相機和 RGB-D 相機可以獲取深度，查詢 2D 圖像也可以獲得限定范圍內(nèi)的 3D 特征點坐標，但是由于目前技術(shù)限制，在室內(nèi)材質(zhì)復(fù)雜，室外大尺度場景下，深度并不可靠。所以 2D圖像點和 3D 點云地圖的匹配依然是一種重要的方法。

特征點匹配

特征點匹配過程可以是在數(shù)據(jù)庫查詢中自適應(yīng)完成的，這多見于基于 3D 結(jié)構(gòu)的查詢。匹配也可以是在查詢后單獨進行，多見于基于 2D 圖像查詢。特征匹配的目的是，為后續(xù)的變換矩陣計算提供匹配的點對集，實現(xiàn)位姿的解算。

經(jīng)典 RANSAC

隨機抽樣一致算法 (Random sample consensus，RANSAC)[16] 是一種經(jīng)典的數(shù)據(jù)過濾、參數(shù)擬合算法。它假設(shè)數(shù)據(jù)（內(nèi)點，Inliers）分布符合一定的數(shù)學(xué)模型，通過迭代計算，去除外點 (Outliers) 、噪聲點， 同時獲取概率上最佳的模型參數(shù)。在全局定位中，內(nèi)點指正確的匹配，外點指錯誤的匹配，參數(shù)模型指匹配點對的空間變換矩陣。如 Fig. 14所示，經(jīng)過 RANSAC 算法優(yōu)化后，匹配更加合理。RANSAC 所期望找到的匹配子集需要滿足兩個指標：內(nèi)點重投影誤差盡可能??；內(nèi)點數(shù)量盡可能多。所以基本流程如下：

①采樣初始子集。

②計算變換矩陣。

③ 根據(jù)變換矩陣計算匹配點的重投影誤差。

④ 去除誤差較大的點

⑤ 循環(huán)①-④，保留最滿足指標的匹配方案。

Figure 14: （上）原始特征匹配；（下）經(jīng)過 RANSAC 算法優(yōu)化后的匹配

其中，初始候選匹配是根據(jù)描述子之間的距離產(chǎn)生的，但重投影誤差則只和關(guān)鍵點的空間位置有關(guān)， 與描述子本身無關(guān)。具體投影矩陣方法請參考“2.4 位姿計算”。需要指出的是，RANSAC 算法受到原始匹 配誤差和參數(shù)選擇的影響，只能保證算法有足夠高的概率合理，不一定得到最優(yōu)的結(jié)果。算法參數(shù)主要包括閾值和迭代次數(shù)。RANSAC 得到可信模型的概率與迭代次數(shù)成正比，所得到的匹配數(shù)量和閾值成反比。因此實際使用時，可能需要反復(fù)嘗試不同的參數(shù)設(shè)置才能得到較優(yōu)的結(jié)果。

學(xué)者們對經(jīng)典 RANSAC 算法進行了很多改進，如 Fig. 15所示，提出了全局 RANSAC(Universal- RANSAC)[17] 的結(jié)構(gòu)圖，形成了具有普適性的 RANSAC 架構(gòu)，涵蓋了幾乎所有的 RANSAC 的改進方 面，如預(yù)濾波、最小子集采樣、由最小子集生成可靠模型、參數(shù)校驗、模型精化。

Figure 15: Universal-RANSAC 通用算法框架

可微分 RANSAC

由于手工描述子在定位領(lǐng)域依然表現(xiàn)出較高的性能，所以一些學(xué)者開始探索使用深度學(xué)習代替算法框架中的某些部分，而不是直接使用端到端的位姿估計模型完全代替?zhèn)鹘y(tǒng)方法。可微分 RANSAC(Differentiable RANSAC，DSAC)[18] 旨在用概率假說選擇代替確定性假說選擇，使得 RANSAC 過程可以被求導(dǎo)，流程如 Fig. 16所示，其中“Scoring”步驟依然采用重投影誤差作為指標，所不同的是，誤差是基于整張圖像而不是特征點，而原先篩選特征點匹配的過程被換為了直接以概率篩選相機位姿假設(shè) h 的過程。雖然目 前方法局限性比較大，但 DSAC 為如何在當前無監(jiān)督為主的定位算法框架中加入先驗知識，提供了一種可行的思路。

Figure 16: 差分 RANSAC 算法框架

位姿計算

對于已獲得的正確匹配點對，需要通過幾何約束計算相應(yīng)的變換矩陣  (Transformation matrix) 。由于數(shù)據(jù)庫中的點坐標，采樣時的相機位姿已知，所以可以通過匹配點對于地圖點的變換矩陣，得到當前相機的位姿。此處定義一些基本符號。相機內(nèi)參為  ，變換矩  的齊次形式為:

其中， 為旋轉(zhuǎn)矩陣，   為平移矩陣。

2.4.1 2D-2D 變換矩陣計算

Figure 17: 2D-2D 變換矩陣計算中的對極幾何

對于兩張二維圖像中的已匹配好的特征點對 () ，它們在歸一化平面上的坐標為 ()，需要通過對極約束計算出相應(yīng)的變換矩陣。如 Fig. 17所示，其幾何意義是  三者共面，這個面也被稱為極平面， 稱為基線，  稱為極線。對極約束中同時包含了平移和旋轉(zhuǎn)，定義為:

其中， 是  在歸一化平面上的坐標，∧ 是外積運算符。將公式中間部分計為基礎(chǔ)矩陣  和本質(zhì)矩陣  ，則有:

由于本質(zhì)矩陣  不具有尺度信息，所以 E 乘以任意非零常數(shù)后對極約束依然成立。 可以通過經(jīng)典的 8 點法求解(Eight-point-algorithm)，然后分解得到 , 。因此可以看出 2D-2D 的變換矩陣求解方式有兩個缺點，首先單目視覺具有尺度不確定性，尺度信息需要在初始化中由  提供。相應(yīng)地，單目初始化不能只有純旋轉(zhuǎn)，必須要有足夠程度的平移，否則會導(dǎo)致  為零。

2.4.2 2D-3D 變換矩陣計算

2D-3D 匹配是位姿估計中重要的一種方法。一般使用 PnP 法，即已知  對 2D-3D 匹配點，求解變換矩陣，以獲得相機位姿。我們將 3D 點 P(X, Y, Z) 投影到相機成像平面 () 上：

其中， 為尺度， 。這個等式的求解可以化為一個線性方程問題，每個特征可以提供兩個線性約束:

這樣，最少可以通過 6 對匹配點進行求解，而匹配數(shù)大于 6 時可以使用 SVD 等方法通過構(gòu)造最小二乘 求解。P3P 法可以看作是 PnP 法的特殊解法，如 Fig. 18所示，利用三角形相似性質(zhì)增加更多約束，只需要 3 對點就可以求解。其它解法還有直接線性變換法 (Direct linear transformation，DLT)，EPnP(Efficient PnP) 法，和 UPnP(Uncalibrated PnP)等。相對于以上線性優(yōu)化方法，非線性優(yōu)化方法如Bundle Adjustment(BA) 也有著廣泛的應(yīng)用。BA 方法在視覺 SLAM 中是一種“萬金油”的存在，可以同時優(yōu)化多個變量，這樣可以一定程度緩解局部誤差帶來的系統(tǒng)不魯棒，感興趣的同學(xué)可以翻閱相關(guān)資料更深入地進行了解。

Figure 18: 2D-3D 變換矩陣計算中的 P3P 方法

3D-3D 變換矩陣計算

3D 點之間的變換矩陣可以用迭代最近點(Iterative closet point, ICP)算法求解。假設(shè)點對匹配 () 結(jié)果正確，則求得的轉(zhuǎn)換矩陣應(yīng)當盡量減少重投影誤差 ?？梢允褂?SVD 求解最小二乘問題:

或者使用建立在李代數(shù)上的非線性優(yōu)化方法 Bundle Adjustment 求解

其中， 表示相機位姿。這里的優(yōu)化目標與 2D-3D 匹配中的 Bundle Adjustment 的類似，但是不需要考慮相機內(nèi)參  ，因為通過雙目相機或者 RGB-D 深度相機，已經(jīng)把原本圖像上的 2D 點從相機成像平面投影到 3D 世界中。

ICP 問題已經(jīng)被證明存在唯一解和無窮多解的情況。因此，在存在唯一解的情況下，優(yōu)化函數(shù)相當于是一個凸函數(shù)，極小值即是全局最優(yōu)解，無論采取何種初始化，都可以求得這一唯解。這是 ICP 方法的一大優(yōu)點。

本文從圖像描述、建圖查詢、特征匹配，位姿計算四個方面介紹了基于特征點的位姿估計算法。雖然傳統(tǒng)視覺全局定位方法目前依然是實際應(yīng)用中的首選，但是，傳統(tǒng)方法是建立在特征點被正確定義、正確提取、正確匹配、正確觀測的前提下進行的，這一前提對于視覺本身而言就是巨大的挑戰(zhàn)。其次，由于傳統(tǒng)方法是 multi-stage 框架，而非 end-to-end，所以中間每個環(huán)節(jié)，環(huán)節(jié)之間的交互，都需要眾多參數(shù)調(diào)整，每個環(huán)節(jié)的技術(shù)都可以作為一個單獨的研究方向。實際應(yīng)用時，也需要加入對應(yīng)具體場景的大量tricks，工程上比較復(fù)雜。

而人們對 end-to-end 方法的期望催生出了如 PoseNet，VLocNet，HourglassNet 等網(wǎng)絡(luò)，在 benchmark上取得了不錯的成績。筆者認為目前 end-to-end 的方法還存在很多問題，主要有 loss function 缺少幾何 約束，建圖時位姿的 6 自由度空間并不連續(xù)，與輸入空間難以形成良好映射，而且缺少相應(yīng)的位姿回歸、 精化機制等。不能否認，作為非線性空間最有力的建模工具，深度學(xué)習在未來會更多地出現(xiàn)在定位領(lǐng)域中。

回歸到視覺定位本身，由于視覺最重要的優(yōu)勢就是成本低、語意豐富、使用場景限制少。因此，以視覺為主，其它低成本傳感器為輔的定位融合方案在未來也將會是一個重要的課題。