基于壓縮地圖的間接法視覺定位研究

王澤威，曾春年，楊 旭，羅 杰，胡錦敏

(1.武漢理工大學(xué) 自動化學(xué)院，湖北 武漢 430070；2.深圳市路暢智能科技有限公司武漢分公司，湖北 武漢 430205)

基于已知地圖的定位是自動駕駛、移動機(jī)器人[1-2]、增強(qiáng)現(xiàn)實(shí)等應(yīng)用領(lǐng)域的重要組成部分。出于經(jīng)濟(jì)性考慮，這些應(yīng)用通常使用嵌入式主板作為計(jì)算平臺，而已知地圖由于數(shù)據(jù)量大，會占用大量存儲空間。傳統(tǒng)意義上，基于已知地圖的視覺定位可以看成是一個在地圖中的特征檢索問題。通過使待定位圖片的局部特征與地圖中關(guān)鍵幀的局部特征建立匹配關(guān)系，經(jīng)過幾何校正后基于RANSAC算法求解PnP問題就能得到待定位圖片的六自由度位姿。

視覺定位根據(jù)步驟的差異可分為間接法與直接法。間接法定位過程中需要通過圖像召回得到地圖中與待定位圖片相似的地圖關(guān)鍵幀，使待定位圖片的局部特征與召回的地圖關(guān)鍵幀進(jìn)行特征匹配，從而建立關(guān)聯(lián)。SCHONBERGER等[3]給出了完整的視覺建圖與定位的框架Colmap，對單目相機(jī)采集的圖像提取描述子并進(jìn)行特征匹配，基于增量式運(yùn)動恢復(fù)結(jié)構(gòu)進(jìn)行三維稀疏重建。在視覺定位方面，Colmap基于詞匯樹來計(jì)算待定位圖像與地圖關(guān)鍵幀之間的相似分?jǐn)?shù)，并進(jìn)行圖像召回和后續(xù)的2D-2D圖像匹配。而基于單目視覺的三維稀疏重建在實(shí)際應(yīng)用過程中容易建圖失敗，無法得到符合真實(shí)物理世界的尺度，且在增量式運(yùn)動恢復(fù)結(jié)構(gòu)過程中當(dāng)前三維結(jié)構(gòu)的尺度容易發(fā)生漂移，導(dǎo)致最終得到的三維模型扭曲、不完整，同時Colmap依賴SIFT[4]特征進(jìn)行圖像召回和特征匹配，使得定位速度較慢。ORB SLAM[5]的回環(huán)檢測部分采用詞袋模型進(jìn)行圖像召回，然后提取ORB特征[6]使召回的圖像與待定位圖像進(jìn)行兩兩匹配。由于采用了二進(jìn)制描述子，該方法實(shí)時性較好，且對在線地圖進(jìn)行重定位效果較好,但二進(jìn)制描述子很難勝任光照變化明顯的場景。與間接法不同的是，SATTLER等[7]在直接法中舍棄了圖像召回這一步，通過詞匯樹和最近鄰搜索使待定位圖像的局部特征與地圖中的特征建立關(guān)聯(lián)。直接法利用事先訓(xùn)練好的詞匯樹，基于最近鄰搜索使地圖中的特征向量與待定位圖像中的特征向量與詞匯樹中的詞匯產(chǎn)生關(guān)聯(lián)，并將這種關(guān)聯(lián)作為特征匹配的先驗(yàn)，從而減小了特征搜索的范圍，提高了匹配速度。SATTLER等[8]又對直接法進(jìn)行了補(bǔ)充，在得到待定位圖片與地圖中路標(biāo)點(diǎn)的2D-3D對應(yīng)關(guān)系后，利用KD-Tree[9]在已匹配上的3D點(diǎn)附近尋找其他未建立匹配的3D點(diǎn)，然后通過一個比較粗糙的詞匯樹反向搜索待定位圖片中的特征并進(jìn)行匹配，從而增加有效匹配的數(shù)目，提高定位成功率?？傊g接法更適合大尺度地圖中的定位，直接法在小尺度地圖定位中定位速度更快。

基于大尺度地圖的構(gòu)建、壓縮和間接法視覺定位問題，已有研究存在單目建圖算法建圖成功率低、地圖數(shù)據(jù)冗余且文件龐大、定位速度慢等不足。因此，筆者在Colmap單目稀疏重建的基礎(chǔ)上進(jìn)行改進(jìn)，提出一種基于雙目視覺的三維稀疏重建算法，給出一種地圖關(guān)鍵幀和路標(biāo)點(diǎn)的篩選策略和新的地圖數(shù)據(jù)組織格式，使得在不影響定位成功率的前提下把地圖文件尺寸降低到原來的20%以下，并在視覺定位過程中把描述子從SIFT替換成HF-Net[10],加入共視過濾，提高了定位成功率。

1 地圖構(gòu)建與壓縮

1.1 稀疏三維重建

地圖構(gòu)建是一個離線過程，主要分為稀疏三維重建和地圖關(guān)鍵幀局部特征的計(jì)算。其中，地圖的稀疏三維結(jié)構(gòu)主要通過運(yùn)動恢復(fù)結(jié)構(gòu)(structure from motion)得到[11]。運(yùn)動恢復(fù)結(jié)構(gòu)是利用三維結(jié)構(gòu)在不同視角圖像上的投影來進(jìn)行重建的過程。增量式運(yùn)動恢復(fù)結(jié)構(gòu)則是一個包含了迭代步驟的串行流程:先提取圖像的特征并匹配，再進(jìn)行幾何校正去除匹配中的錯誤匹配；將得到的匹配關(guān)系圖作為稀疏重建的輸入，選擇兩個關(guān)鍵幀并計(jì)算三維結(jié)構(gòu)，得到初始模型；增量式地注冊新圖像，三角化特征點(diǎn)，過濾外點(diǎn)，利用集束調(diào)整(bundle adjustment)對重建結(jié)果進(jìn)行優(yōu)化，最終得到整個地圖的稀疏三維結(jié)構(gòu)。

特征匹配能夠找到輸入圖像I={Ii|i=1，2，…,Ni}中的共視場景，并得到同一個點(diǎn)在不同圖像中的投影，最終得到一系列經(jīng)過幾何校正的圖像對和一個包含每個路標(biāo)點(diǎn)到地圖關(guān)鍵幀投影的關(guān)系圖。對于每張圖片Ii，提取Ii的特征點(diǎn)和對應(yīng)的描述子，得到特征點(diǎn)和描述子的集合Fi={(xj,fj)|j=1,2,…,Ni}，其中i表示對圖像的索引；j表示對一個圖像中的特征點(diǎn)和描述子的索引；xj∈R2表示特征點(diǎn)在圖像中的坐標(biāo)；fj表示對應(yīng)的描述子向量。描述子具備尺度不變性和旋轉(zhuǎn)不變性，從而使得運(yùn)動恢復(fù)結(jié)構(gòu)過程能夠在不同圖像中對描述子進(jìn)行識別。然后，將描述子作為對圖像外觀的描述，檢測圖像中看到相同場景的圖像。由于圖像序列是按時間順序采集的，故可以根據(jù)時間順序?qū)D像進(jìn)行特征匹配。這是因?yàn)樵趫D像的采集過程中，時間上接近的圖像通常有共同觀測到的場景，它們之間可以順利地建立特征點(diǎn)匹配。因此對于圖像Ii，使Ii與Ii+1,Ii+2,…,Ii+r按序進(jìn)行匹配。然而圖像的采集可能多次經(jīng)過同一場景，這使得有些圖像在時間上不接近，但是它們的采集地點(diǎn)相似，依然可以順利地建立特征匹配。對于這種情況，需要搜索采集時間早于Ii的圖像Ii′(i′

Si,i′=‖h(Ii)-h(Ii′)‖2

(1)

原始的特征匹配存在錯誤匹配，會對稀疏重建的精度帶來負(fù)面影響，因此需要通過幾何校正去除錯誤匹配，從而保證所有互相匹配的特征點(diǎn)都對應(yīng)三維空間中的同一個路標(biāo)點(diǎn)。幾何校正根據(jù)隨機(jī)抽樣一致算法估計(jì)每一組圖像對的投影變換，使得圖像上的特征點(diǎn)能夠根據(jù)幾何關(guān)系進(jìn)行互相映射。根據(jù)每一對圖像的幾何特點(diǎn)，選擇不同的映射描述它們的幾何關(guān)系。單應(yīng)矩陣H描述了一個旋轉(zhuǎn)或者移動的相機(jī)拍攝一個平面的幾何關(guān)系，對極幾何通過本質(zhì)矩陣E或者基礎(chǔ)矩陣F描述了一個移動的相機(jī)在不同位置上拍攝的圖像之間的幾何關(guān)系。如果一個映射能夠?qū)⒁粚D像中足夠數(shù)目的特征點(diǎn)進(jìn)行互相映射，那么這些能夠互相映射的特征點(diǎn)就被認(rèn)為是通過幾何校正的正確匹配。通過先后估計(jì)單應(yīng)矩陣H和本質(zhì)矩陣E,幾何校正保留了滿足單應(yīng)變換和對極幾何的特征匹配，去除了不滿足條件的匹配，而保留下來的特征匹配作為輸出，用于后續(xù)的增量式運(yùn)動恢復(fù)結(jié)構(gòu)。

增量式運(yùn)動恢復(fù)結(jié)構(gòu)的輸入是圖像的匹配關(guān)系圖，輸出是每張圖像的六自由度位姿和三維路標(biāo)點(diǎn)的集合。運(yùn)動恢復(fù)結(jié)構(gòu)首先需要進(jìn)行初始化，初始化選擇兩張?zhí)卣髌ヅ涑渥?、空間分布合理的左目圖像用于兩視圖單目重建，然后基于雙目約束計(jì)算尺度，并對兩視圖的重建結(jié)果基于尺度進(jìn)行相似變換。初始化過程中兩張圖像的選擇十分重要，錯誤的初始化會導(dǎo)致最終的稀疏重建失敗。

初始模型示意圖如圖1所示，C1和C2、C3和C4分別是兩個雙目相機(jī)的左目和右目,初始化時將C1坐標(biāo)系作為世界坐標(biāo)系，點(diǎn)P是根據(jù)x1和x3的匹配關(guān)系得到的世界坐標(biāo)系下的3D點(diǎn)，它不具備真實(shí)物理世界的尺度，x1、x2、x3代表了C1、C2、C3對點(diǎn)P觀測的齊次坐標(biāo)。則尺度的最優(yōu)解?*可通過優(yōu)化式(2)所示目標(biāo)函數(shù)得到。

圖1 初始模型示意圖

(2)

式中：TRL為左相機(jī)坐標(biāo)系到右相機(jī)坐標(biāo)系的相對變換矩陣；TC1W為世界坐標(biāo)系到C1坐標(biāo)系的變換矩陣；n為C1、C2、C3坐標(biāo)系共同觀測到的3D點(diǎn)的個數(shù)；Pk為C1、C2、C3坐標(biāo)系共同觀測到的3D點(diǎn)中第k個3D點(diǎn)的齊次坐標(biāo)向量；K為相機(jī)內(nèi)參矩陣；x2,k為Pk在C2坐標(biāo)系中的坐標(biāo)向量；π(·)為投影函數(shù)。由于初始化時C1坐標(biāo)系就是世界坐標(biāo)系，因此TC1W為單位矩陣。

(3)

(4)

得到初始的三維結(jié)構(gòu)后，對其他圖像進(jìn)行注冊，根據(jù)特征匹配關(guān)系和當(dāng)前重建結(jié)果輸出的3D點(diǎn)，得到未注冊圖像與當(dāng)前重建結(jié)果的2D-3D匹配關(guān)系，通過求解PnP問題得到其他圖像的六自由度位姿。由于2D-3D匹配關(guān)系通常存在錯誤匹配，因此仍然需要通過隨機(jī)抽樣一致算法進(jìn)行計(jì)算。新注冊的圖像必須觀測到當(dāng)前重建結(jié)果中的路標(biāo)點(diǎn)，同時能夠通過三角化增加路標(biāo)點(diǎn)。根據(jù)新注冊圖像與已注冊圖像之間的匹配關(guān)系和已知的六自由度位姿，通過三角化得到新的路標(biāo)點(diǎn)并加入到當(dāng)前重建結(jié)果。三角化是運(yùn)動恢復(fù)結(jié)構(gòu)中的重要步驟，可提高當(dāng)前重建結(jié)果的穩(wěn)定性，增加新注冊圖像與當(dāng)前重建結(jié)果之間的2D-3D匹配關(guān)系。

盡管圖像注冊和三角化的輸出數(shù)據(jù)緊密相關(guān)，但它們的步驟是各自獨(dú)立的，相機(jī)位姿的不確定性會傳遞到三角化后的路標(biāo)點(diǎn)，反之亦然。如果沒有進(jìn)一步的優(yōu)化，運(yùn)動恢復(fù)結(jié)構(gòu)通常會產(chǎn)生嚴(yán)重的漂移，導(dǎo)致重建失敗。因此，需要引入集束調(diào)整(bundle adjustment)，通過最小化重投影誤差來對相機(jī)位姿和路標(biāo)點(diǎn)坐標(biāo)進(jìn)行聯(lián)合優(yōu)化。聯(lián)合優(yōu)化分為局部優(yōu)化和全局優(yōu)化，局部優(yōu)化是對新注冊圖像附近的局部重建結(jié)果進(jìn)行聯(lián)合優(yōu)化，減小局部累積誤差；全局優(yōu)化是對當(dāng)前整個重建結(jié)果進(jìn)行聯(lián)合優(yōu)化，減小全局累積誤差。由于采用的是雙目圖像，故在進(jìn)行局部優(yōu)化和全局優(yōu)化時還需要加入右目約束。右目殘差定義為：

ER=π(TRL(TC1WPW,K))-x2

(5)

稀疏三維重建流程框圖如圖2所示，圖像通過特征提取、特征匹配和幾何校正建立圖像之間的2D-2D匹配關(guān)系，然后選擇兩對圖片進(jìn)行初始化，得到一個初始的三維模型。在其余圖像中，選擇新的圖像進(jìn)行注冊，根據(jù)新圖像與已有三維模型之間的匹配關(guān)系三角化新的特征點(diǎn)，然后進(jìn)行局部優(yōu)化并剔除重投影誤差較大的3D點(diǎn)(outlier)，完成局部優(yōu)化后，若當(dāng)前模型比上一次執(zhí)行全局優(yōu)化時的模型有較大增長，則執(zhí)行全局優(yōu)化并再次剔除outlier。當(dāng)所有圖像均完成注冊后，則可輸出一個完整的三維模型。

圖2 稀疏三維重建流程框圖

1.2 圖像特征處理

近年來,基于深度學(xué)習(xí)的圖像特征被廣泛運(yùn)用于多個領(lǐng)域，成為傳統(tǒng)手工描述子的替代。卷積神經(jīng)網(wǎng)絡(luò)能夠快速計(jì)算稠密像素級別的圖像特征，為圖像匹配和定位提供魯邦的描述子表示。然而，硬件資源受限的計(jì)算平臺難以匹配稠密特征，需要提取稀疏特征點(diǎn)，并對稠密特征進(jìn)行稀疏采樣。采樣后的基于卷積神經(jīng)網(wǎng)絡(luò)的圖像特征比傳統(tǒng)手工描述子在抗光照變化上有更強(qiáng)的性能。

圖像特征通過卷積神經(jīng)網(wǎng)絡(luò)HF-Net進(jìn)行提取。HF-Net可以計(jì)算圖像的特征點(diǎn)、稀疏局部特征、稠密局部特征和全局特征。對于地圖關(guān)鍵幀中的特征點(diǎn)，需要通過雙線性插值方法從稠密局部特征中進(jìn)行插值，從而得到與特征點(diǎn)對應(yīng)的256維局部特征向量。HF-Net能夠計(jì)算地圖關(guān)鍵幀的全局描述子，其維度達(dá)到4 096維，過高的維數(shù)導(dǎo)致全局描述子存在維度冗余。為了減少描述子維度，降低地圖尺寸，需要通過主成分分析法進(jìn)行降維。設(shè)地圖中共有Z個關(guān)鍵幀，第i個關(guān)鍵幀對應(yīng)的全局特征向量為gi，則全局特征向量的均值為：

(6)

(7)

建圖與定位流程框圖如圖3所示，離線部分代表地圖的預(yù)處理部分，地圖中的關(guān)鍵幀經(jīng)過HF-Net處理后得到稠密的局部特征和全局特征向量，結(jié)合2D點(diǎn)對稠密局部特征進(jìn)行雙線性插值，得到與2D點(diǎn)一一對應(yīng)的稀疏局部特征。全局特征經(jīng)過PCA降維后得到低維的全局特征和對應(yīng)的投影矩陣。在線部分描述了對待定位圖像進(jìn)行定位的過程。待定位圖像經(jīng)由HF-Net處理得到對應(yīng)的全局特征、特征點(diǎn)和稀疏局部特征。對全局特征進(jìn)行降維后，利用N近鄰搜索算法在地圖關(guān)鍵幀的全局特征中搜索若干個接近的對象，即圖像召回。根據(jù)召回圖像的共視關(guān)系進(jìn)行聚類，使待定位圖像與每個類別的召回圖像進(jìn)行塊匹配并利用RANSAC PnP算法求解位姿。

圖3 建圖與定位流程框圖

1.3 地圖稀疏化

增量式運(yùn)動恢復(fù)結(jié)構(gòu)輸出的稀疏重建包含了密集的地圖關(guān)鍵幀和大量3D路標(biāo)點(diǎn)，導(dǎo)致地圖文件尺寸過大，數(shù)據(jù)冗余。為了減小地圖尺寸，去除冗余數(shù)據(jù)，需要對地圖關(guān)鍵幀和3D路標(biāo)點(diǎn)進(jìn)行稀疏化，從而得到一個緊湊的地圖，使地圖僅保留對視覺定位最有用的信息。

間接法視覺定位需要對地圖關(guān)鍵幀進(jìn)行圖像召回，即先計(jì)算待定位圖像的全局描述子與地圖關(guān)鍵幀的全局描述子的歐式距離，再通過排序取前幾張歐式距離最小的地圖關(guān)鍵幀作為結(jié)果返回，并進(jìn)行后續(xù)的特征匹配。密集的地圖關(guān)鍵幀增加了圖像召回的耗時和地圖信息冗余。地圖關(guān)鍵幀的稀疏化是根據(jù)相鄰關(guān)鍵幀的關(guān)系對關(guān)鍵幀進(jìn)行篩選實(shí)現(xiàn)的，即遍歷地圖關(guān)鍵幀，對于當(dāng)前幀判斷以下指標(biāo)：①當(dāng)前地圖關(guān)鍵幀與上一個被選中的關(guān)鍵幀之間匹配的特征點(diǎn)數(shù)目nt；②當(dāng)前地圖關(guān)鍵幀與上一個被選中的關(guān)鍵幀之間的距離l；③當(dāng)前地圖關(guān)鍵幀與上一個被選中的關(guān)鍵幀之間的關(guān)鍵幀數(shù)目y。如果nt小于閾值，或者l、y大于閾值，則選中當(dāng)前幀，作為保留下來的地圖關(guān)鍵幀。

對地圖關(guān)鍵幀進(jìn)行稀疏化后，需要對3D路標(biāo)點(diǎn)進(jìn)行稀疏化。通常情況下，每個3D路標(biāo)點(diǎn)被觀測的次數(shù)越多就越穩(wěn)定，對視覺定位越有利，反之則越不穩(wěn)定，是噪點(diǎn)的概率越大。對3D點(diǎn)的稀疏化需要盡可能保留被觀測次數(shù)較多的點(diǎn)，舍棄被觀測次數(shù)較少的點(diǎn)。3D點(diǎn)的稀疏化可以直觀地構(gòu)建成一個整數(shù)線性規(guī)劃問題。整數(shù)線性規(guī)劃的求解復(fù)雜度會隨著待求變量維度的增加而呈幾何倍數(shù)的增加，若不進(jìn)行其他處理，則會導(dǎo)致壓縮大尺度地圖的過程十分緩慢。為了適用于大尺度地圖，采用圖分割算法將地圖分成若干個子地圖，再運(yùn)用整數(shù)線性規(guī)劃對每個子地圖的3D路標(biāo)點(diǎn)進(jìn)行稀疏化。

圖分割前需要將地圖轉(zhuǎn)化成一個拓?fù)鋱D，以地圖關(guān)鍵幀為節(jié)點(diǎn)，以地圖關(guān)鍵幀之間的共視關(guān)系為邊，將共視的3D點(diǎn)數(shù)作為邊的權(quán)值，構(gòu)建一幅拓?fù)鋱DGt。根據(jù)多路歸并算法將Gt分割成若干個子圖，使得每個子圖盡可能獨(dú)立，即連接不同子圖之間的邊最短。有的地圖關(guān)鍵幀之間存在公共觀測，即共視關(guān)系，根據(jù)關(guān)鍵幀的共視關(guān)系可以得到一個由頂點(diǎn)(關(guān)鍵幀)和邊(共視關(guān)系)組成的無向圖。利用多路歸并算法將無向圖分割成若干個連續(xù)的子圖，每個子圖都由地圖中部分空間上連續(xù)的關(guān)鍵幀組成。圖分割之后，對于每個子圖，選取其中被觀測次數(shù)最多的3D點(diǎn)用于視覺定位。為了使篩選的點(diǎn)在地圖中均勻分布，設(shè)置閾值b和u,使每個子圖中篩選的點(diǎn)不少于b，不大于u。通過最大化被觀測次數(shù)，求解以下整數(shù)線性規(guī)劃問題：

maxqTs

(8)

(9)

GTs≤u

(10)

s∈{0,1}M

(11)

設(shè)當(dāng)前子地圖中共有M個3D路標(biāo)點(diǎn),構(gòu)造待求變量s=[s0,s1,s2,…,sM-1]T和參數(shù)變量q=[q0,q1,q2,…,qM-1]T，Ai=[ai,0,ai,1,ai,2,…,ai,M-1]T、G=[1,1,1,…,1]T。q為一個M維向量，保存了每個路標(biāo)點(diǎn)被觀測的次數(shù)，qm為第m個路標(biāo)點(diǎn)被觀測的次數(shù)；sm為二進(jìn)制標(biāo)量,sm=1表示第m個路標(biāo)點(diǎn)被選中，sm=0表示第m個路標(biāo)點(diǎn)未被選中；ai,m為二進(jìn)制標(biāo)量，ai,m=1表示子地圖中第i個關(guān)鍵幀Ci觀測到了第m個路標(biāo)點(diǎn)；G為元素為1的M維常數(shù)向量，用來對s中非0元素進(jìn)行求和；GTs表示被選中的路標(biāo)點(diǎn)總數(shù)。構(gòu)造完整數(shù)線性規(guī)劃問題后，通過開源線性規(guī)劃求解庫Lpsolve對上述問題進(jìn)行求解,可得到最優(yōu)目標(biāo)變量s。

1.4 地圖格式設(shè)計(jì)

視覺地圖要求在視覺定位過程中能夠?qū)μ卣鬟M(jìn)行快速檢索，用高效精簡的數(shù)據(jù)格式進(jìn)行保存。Colmap使用易于檢索的Sqlite數(shù)據(jù)庫存儲地圖的2D信息，把稀疏重建的結(jié)果用另外的文件進(jìn)行存儲。然而數(shù)據(jù)庫文件尺寸較大且不易壓縮，同時在存儲稀疏重建結(jié)果時對部分?jǐn)?shù)據(jù)進(jìn)行了二次存儲，造成了數(shù)據(jù)冗余。因此把地圖數(shù)據(jù)分成了路標(biāo)點(diǎn)、觀測、描述子和映射關(guān)系，并使用矩陣格式以二進(jìn)制文件的形式進(jìn)行存儲，避免了數(shù)據(jù)冗余，提高了地圖加載速度。

地圖數(shù)據(jù)主要有3D路標(biāo)點(diǎn)、2D觀測、局部描述子、全局描述子、局部描述子到關(guān)鍵幀的映射和局部描述子到路標(biāo)點(diǎn)的映射6部分。其中，描述子在地圖文件中占據(jù)了大部分，故有效壓縮描述子能夠在很大程度上減小總體地圖文件尺寸。一個HF-Net描述子是一個256維的浮點(diǎn)型向量，浮點(diǎn)型描述子的每一維都需要用4個字節(jié)來存儲，這增加了文件尺寸，且常用的文件壓縮算法對浮點(diǎn)型數(shù)據(jù)的壓縮比1較低。因此把原始描述子的每一維乘以權(quán)值，并加上偏置，量化成單字節(jié)整型數(shù)據(jù)進(jìn)行存儲，可大幅度減小地圖尺寸。

f′=wf+b

(12)

式中：f為原始的浮點(diǎn)型描述子；w為權(quán)值；b為偏置。

2 視覺定位

2.1 圖像召回

圖像召回是在地圖關(guān)鍵幀中搜索與待定位圖像相似的若干個地圖關(guān)鍵幀。利用式(7)對待定位圖像的原始全局特征gq進(jìn)行降維得到g′q，再基于歐氏距離di衡量與地圖中關(guān)鍵幀的相似度。

di=‖g′q-gi‖2

(13)

2.2 共視關(guān)系計(jì)算與聚類

圖像召回后，對得到的候選關(guān)鍵幀計(jì)算共視關(guān)系并聚類。聚類的目的是劃分不同的共視場景并提高匹配速度。一次有效的圖像召回得到的地圖關(guān)鍵幀中，會有相當(dāng)數(shù)量的地圖關(guān)鍵幀分布在待定位圖像在地圖中的正確位置附近，且通?；ハ喙惨?。經(jīng)過共視關(guān)系聚類后，有相同共視場景的關(guān)鍵幀被分成一類，于是就可以根據(jù)不同的共視場景，依次與待定位圖像進(jìn)行特征匹配?；诠惨曣P(guān)系的聚類通過廣度優(yōu)先搜索算法實(shí)現(xiàn)，分類后根據(jù)每個類中關(guān)鍵幀的數(shù)目從大到小依次排序。圖像召回得到的地圖關(guān)鍵幀可能存在與待定位圖像不符合的情況，這些關(guān)鍵幀通常單獨(dú)成類，而關(guān)鍵幀數(shù)目較多的類則與待定位圖像相符合的概率較大。

2.3 塊匹配

塊匹配是一種直接建立待定位圖像與地圖路標(biāo)點(diǎn)之間2D-3D匹配關(guān)系的匹配方法?；诘貓D關(guān)鍵幀的分類結(jié)果，遍歷每個類，取當(dāng)前類中所有關(guān)鍵幀的局部描述子和對應(yīng)地圖中的3D路標(biāo)點(diǎn)索引，分別構(gòu)建矩陣Wf、Wl，Wf中每一行表示一個256維的局部描述子向量，Wl中每一行表示一個3維的路標(biāo)點(diǎn)向量。設(shè)當(dāng)前待定位圖像的描述子矩陣為Wq,Wq中每一行表示待定位圖像中的一個256維局部描述子，使Wf中的行向量與Wq中的行向量進(jìn)行匹配，根據(jù)Wl中對應(yīng)的3D路標(biāo)點(diǎn)的坐標(biāo)向量，通過低比例測試(即滿足式(14)的匹配被認(rèn)為是有效匹配)，建立2D-3D匹配關(guān)系，進(jìn)而通過隨機(jī)抽樣一致算法計(jì)算待定位圖像的六自由度位姿。

‖f′j-f0‖2<0.7‖f′j-f1‖2

(14)

式中：f′j為待定位圖像Wq的第j個行向量；f0、f1為Wf中與f′j最接近的兩個行向量，且來自兩個不同的路標(biāo)點(diǎn)。

兩兩幀的特征匹配需要進(jìn)行大量的2D-2D幾何校正來保證匹配的準(zhǔn)確性，而2D-2D幾何校正正是特征匹配過程中最耗時的步驟。相較于兩兩幀的特征匹配，塊匹配基于低比例測試直接建立2D局部特征與3D點(diǎn)之間的匹配關(guān)系，無需進(jìn)行2D-2D幾何校正，故塊匹配更高效。

3 實(shí)驗(yàn)研究

3.1 實(shí)驗(yàn)設(shè)計(jì)

實(shí)驗(yàn)分為3個部分：①對開源數(shù)據(jù)集和自采數(shù)據(jù)集進(jìn)行基于雙目的稀疏建圖實(shí)驗(yàn)，并與Colmap原版單目重建進(jìn)行對比，其中自采數(shù)據(jù)集是2020年4月24日在某產(chǎn)業(yè)園區(qū)采集的數(shù)據(jù)；②對雙目稀疏重建得到的視覺地圖進(jìn)行壓縮，對比壓縮前后數(shù)據(jù)量；③對壓縮前后的地圖進(jìn)行視覺定位，觀察分析定位結(jié)果。視覺定位實(shí)驗(yàn)需要保證測試定位用的圖片在地圖范圍以內(nèi)采集，因此在實(shí)驗(yàn)時需要在不同時間對同一場景多次采集數(shù)據(jù)。實(shí)驗(yàn)采用的數(shù)據(jù)分別為2020年4月24日、4月26日、5月14日在同一個產(chǎn)業(yè)園區(qū)采集的數(shù)據(jù)。4月24日采集的數(shù)據(jù)用于雙目建圖，4月26日、5月14日采集的數(shù)據(jù)用于測試定位。實(shí)驗(yàn)平臺環(huán)境：CPU為i7-9700KF，RAM為16 GB，GPU為RTX 2070 Super，操作系統(tǒng)為Ubuntu 16.04。

3.2 實(shí)驗(yàn)結(jié)果及分析

(1)基于雙目圖像的稀疏建圖實(shí)驗(yàn)。原版Colmap單目建圖和改進(jìn)后雙目建圖的結(jié)果對比如圖4所示。原版Colmap單目建圖在一些數(shù)據(jù)集上不能得到完整的三維結(jié)構(gòu)，從而導(dǎo)致建圖失敗，而改進(jìn)后的基于雙目視覺的建圖算法能夠在這些數(shù)據(jù)集上建圖成功，得到具有全局一致性的稀疏三維結(jié)構(gòu)。

圖4 稀疏重建對比

(2)地圖壓縮實(shí)驗(yàn)。對基于雙目視覺建圖得到的地圖進(jìn)行壓縮，分析關(guān)鍵幀數(shù)目、路標(biāo)點(diǎn)數(shù)目和地圖文件尺寸。地圖壓縮前后結(jié)果對比如表1所示，可知壓縮后地圖中的關(guān)鍵幀數(shù)目和路標(biāo)點(diǎn)數(shù)目明顯減少，地圖文件尺寸也大幅度減少。

表1 地圖壓縮前后結(jié)果對比

對EuRoC-MH04序列數(shù)據(jù)集進(jìn)行雙目重建和地圖壓縮的結(jié)果如圖5所示，可知壓縮后地圖中的關(guān)鍵幀和3D點(diǎn)明顯均勻變少，但是地圖的整體輪廓依然完整。

(3)視覺定位實(shí)驗(yàn)。不同時段對同一場景采集圖像的視覺定位實(shí)驗(yàn)對比結(jié)果和軌跡圖分別如表2和圖6所示。由表2可知，改進(jìn)后的定位比原版Colmap定位的成功率更高，定位速度更快，所需地圖文件尺寸更小。從圖6可以看出，對于A組實(shí)驗(yàn)，Colmap原版定位和改進(jìn)后方法的定位軌跡基本一致；對于B組實(shí)驗(yàn)，改進(jìn)后方法的定位軌跡有更好的覆蓋率，在一些定位困難的地方依然能夠得到靠近軌跡的定位結(jié)果。

表2 不同時間對同一場景采集圖像的視覺定位實(shí)驗(yàn)對比結(jié)果

圖6 視覺定位實(shí)驗(yàn)軌跡圖

4 結(jié)論

(1)建圖對比實(shí)驗(yàn)證明了改進(jìn)后的雙目建圖算法比單目建圖能夠明顯提高建圖的成功率。

(2)通過圖分割和整數(shù)線性規(guī)劃對地圖進(jìn)行稀疏化處理，能夠有效去除地圖中的冗余信息，保留對視覺定位有利的路標(biāo)點(diǎn)和關(guān)鍵幀，且結(jié)合單字節(jié)整型量化后能夠大幅度減小地圖文件尺寸。

(3)利用地圖關(guān)鍵幀的共視關(guān)系對召回圖像進(jìn)行聚類，并與待定位圖像進(jìn)行塊匹配，加速了定位過程。而且，地圖壓縮過程中減小了噪點(diǎn)密度，改善了定位過程中的有效定位結(jié)果。

(4)地圖無法在定位過程中進(jìn)行更新，而實(shí)際環(huán)境變化多樣，地圖場景也會隨著時間變化，久而久之會降低定位成功率，因此如何在定位過程中實(shí)時更新地圖，是未來值得探索的一個方向。