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

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

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

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

1 視覺(jué)全局定位概念

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

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

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

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

2 特征點(diǎn)提取

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

描述子一般是人為手工設(shè)計(jì)的 (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í)時(shí)性的要求，一些計(jì)算速度更快的二值模式描述子被設(shè)計(jì)出來(lái)，如 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é)習(xí)流行之前，這些手工特征一直引領(lǐng)著整個(gè)計(jì)算視覺(jué)產(chǎn)業(yè)，直到今天，這些特征在那些缺少標(biāo)注數(shù)據(jù)、約束較多的場(chǎng)景下，依然被廣泛應(yīng)用。下面簡(jiǎn)單介紹兩類(lèi)常用的描述子。

SIFT

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

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

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

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

Figure 2: 高斯差分方法

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

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

二值描述子

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

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

Figure 4: LBP 描述子采樣模式

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

Figure 5: ORB 描述子點(diǎn)對(duì)選取模式

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

Figure 6: FREAK 描述子采樣、點(diǎn)對(duì)選取摸式

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

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

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

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

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

3 數(shù)據(jù)庫(kù)建立與查詢(xún)

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

對(duì)于一張輸入的觀測(cè)圖像，通過(guò)數(shù)據(jù)庫(kù)，查詢(xún)建圖歷史（圖像/點(diǎn)云/特征點(diǎn)），得到當(dāng)前圖像最可能觀測(cè)到的地圖子集（圖像/點(diǎn)云/特征點(diǎn)），將地圖與觀測(cè)信息進(jìn)行匹配，計(jì)算變換矩陣，得到觀測(cè)相機(jī)的位姿。

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

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

VLAD 編碼

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

Figure 7: VLAD 通過(guò)描述子與碼詞的距離進(jìn)行編碼

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

Figure 8: DenseSIFT 和標(biāo)準(zhǔn) SIFT 特征點(diǎn)，匹配后內(nèi)點(diǎn)(綠)對(duì)比

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

Figure 9: NetVLAD 聚類(lèi)中心(×)與 VLAD 聚類(lèi)中心(★)對(duì)比

BoW 編碼 + 索引

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

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

樹(shù)的構(gòu)造過(guò)程，實(shí)際上就是將原始圖像編碼的過(guò)程。但是編碼本身并不能加快搜索過(guò)程，與 VLAD 相似，還是需要與數(shù)據(jù)庫(kù)中的圖像逐一比較。因此，這里設(shè)計(jì)了一種逆向索引(Inverse index) ，不需要比較編碼后的向量。其原理如 Fig. 11所示，對(duì)于一張查詢(xún)圖像 (Query image) ，將提取的描述子輸入到 BoW 中，最終會(huì)落入碼詞葉子結(jié)點(diǎn) (Visual word) k 中。

而每個(gè)碼詞對(duì)應(yīng)一個(gè)索引，記錄碼詞  對(duì)于數(shù)據(jù)庫(kù)中第  張圖的權(quán)重  (Fig.10)。這里權(quán)重使用 TF-IDF(Term frequency–inverse document frequency) 計(jì)算。即如果一個(gè)詞  在某個(gè)圖像  中出現(xiàn)頻率高，在其它圖像出現(xiàn)頻率低，則這個(gè)詞對(duì)于圖像判別性較好，權(quán)重值  較高。最終通過(guò)投票 (Voting) 機(jī)制，選出匹配圖像。同樣需要注意的是，逆向索引不一定建立在樹(shù)形結(jié)構(gòu)的 BoW 上，它僅僅是提供一種快速查詢(xún)的方法。

Figure 11: 通過(guò)逆向索引 + 投票機(jī)制，直接查詢(xún)圖像

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

3D 點(diǎn)云查詢(xún)

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

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

Figure 12: Active Search

Figure 13: Visbility Filtering 

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

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

4 特征點(diǎn)匹配

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

經(jīng)典 RANSAC

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

• ①采樣初始子集。
• ②計(jì)算變換矩陣。
• ③ 根據(jù)變換矩陣計(jì)算匹配點(diǎn)的重投影誤差。
• ④ 去除誤差較大的點(diǎn)
• ⑤ 循環(huán)①-④，保留最滿(mǎn)足指標(biāo)的匹配方案。

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

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

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

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

可微分 RANSAC

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

Figure 16: 差分 RANSAC 算法框架

5 位姿計(jì)算

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

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

2.4.1 2D-2D 變換矩陣計(jì)算

Figure 17: 2D-2D 變換矩陣計(jì)算中的對(duì)極幾何

對(duì)于兩張二維圖像中的已匹配好的特征點(diǎn)對(duì) () ，它們?cè)跉w一化平面上的坐標(biāo)為 ()，需要通過(guò)對(duì)極約束計(jì)算出相應(yīng)的變換矩陣。如 Fig. 17所示，其幾何意義是  三者共面，這個(gè)面也被稱(chēng)為極平面， 稱(chēng)為基線(xiàn)，  稱(chēng)為極線(xiàn)。對(duì)極約束中同時(shí)包含了平移和旋轉(zhuǎn)，定義為:

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

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

2D-3D 變換矩陣計(jì)算

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

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

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

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

3D-3D 變換矩陣計(jì)算

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

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

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

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

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

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

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