ORB-SLAM系統(tǒng)優(yōu)化框架分析概述

邱笑晨，趙晨旭，張 海，許 輝

(1.北京航空航天大學(xué)自動化學(xué)院，北京 100083；2.北京航空航天大學(xué)中法工程師學(xué)院，北京 100083；3.北京公共交通控股(集團)有限公司，北京 100161)

0 引言

同步定位與地圖構(gòu)建[1](Simultaneous Localization and Mapping, SLAM)是指搭載特定傳感器的運動主體，在沒有先驗環(huán)境信息的條件下，在運動過程中建立環(huán)境的模型，并估計自己的實時運動狀態(tài)的方法[2]。若這里的傳感器為相機，則稱之為視覺SLAM。

目前，主流的視覺SLAM方法按照估計框架可以分為基于濾波的方法和基于關(guān)鍵幀光束平差法(Bundle Adjustment, BA)[3]。按照圖像信息的利用程度可以分為基于特征點法和基于直接法(直接利用像素信息)?；跒V波的視覺SLAM的主要思想是將每一時刻t的系統(tǒng)狀態(tài)用一個高斯概率模型表示，且系統(tǒng)狀態(tài)由一個濾波器不斷更新，從而得到不同的SLAM系統(tǒng)，其中最具代表性的是MonoSLAM[2]和MSCKF[4]。并行追蹤與制圖(Parallel Tracking and Mapping, PTAM)由Klein等[5]于2007年提出，是最早期基于關(guān)鍵幀光束平差法的單目視覺SLAM系統(tǒng)，利用傳統(tǒng)的特征點法實現(xiàn)。ORB-SLAM是Mur-Artal等[6]于2015年提出的基于PTAM算法框架改進的視覺SLAM系統(tǒng)，它利用了ORB(Oriented FAST and Rotated, BRIEF)特征，是目前性能最好的單目視覺SLAM系統(tǒng)之一，其二代版本加入了雙目和RGB-D模式，進一步提高了其對不同傳感器配置的適應(yīng)能力。DTAM[7]和LSD-SLAM[8-9]是兩種基于直接法的具有代表性的單目視覺SLAM算法，其主要特點是不依賴特征點的提取與匹配，通常在圖像模糊、特征缺失等場景下魯棒性更好。

從性能上，ORB-SLAM具有良好的泛用性，支持單目、雙目、RGB-D等多種模式[10]；其回環(huán)檢測算法保證了ORB-SLAM能夠有效地抑制累計誤差，并且利用相同手段實現(xiàn)的重定位功能使得系統(tǒng)在失效后能夠迅速重定位；ORB-SLAM使用3個線程完成系統(tǒng)，實現(xiàn)了快速跟蹤和建圖，能夠保證軌跡和建圖的一致性[11]。

從功能上，可以將ORB-SLAM分為視覺里程計(Visual Odometry,VO)前端部分和回環(huán)(Loop Detection)后端部分。本文主要針對單目ORB-SLAM的系統(tǒng)架構(gòu)進行簡要介紹，以視覺導(dǎo)航中關(guān)鍵視覺里程計為對象，對影響系統(tǒng)精度的不同環(huán)節(jié)進行分析，結(jié)論中提出可能的改進措施。

1 ORB-SLAM系統(tǒng)概述

ORB-SLAM系統(tǒng)的算法部分主要由3個并行的線程組成，分別是：跟蹤(Tracking)、局部建圖(Local Mapping)和回環(huán)檢測(Loop Closing)。此外，在初始化構(gòu)建地圖時會臨時發(fā)起2個線程分別求解單應(yīng)矩陣和本質(zhì)矩陣，執(zhí)行完畢后自行停止；在每次執(zhí)行完回環(huán)位姿圖優(yōu)化后，會在回環(huán)檢測線程中臨時發(fā)起1個線程，執(zhí)行全局BA優(yōu)化，該線程在執(zhí)行完畢后自行停止，或者被另一次全局BA優(yōu)化打斷。本節(jié)主要介紹單目ORB-SLAM的具體架構(gòu)及優(yōu)化框架。

1.1 系統(tǒng)架構(gòu)分析

ORB-SLAM系統(tǒng)架構(gòu)如圖 1[6]所示，結(jié)合圖示分析3個主要線程的基本功能如下所示。

圖1 ORB-SLAM系統(tǒng)架構(gòu)Fig.1 Overview of ORB-SLAM system structure

1)跟蹤：線程輸入為每一幀圖像，在未初始化時，嘗試利用兩幀圖像進行初始化。初始化完成后，對獲得的每一幀圖像，通過特征描述子實現(xiàn)圖像特征點與局部地圖點的有效匹配，運用光束平差法來最小化重投影誤差，從而優(yōu)化當前幀相機位姿，實現(xiàn)每幀圖像采集時刻相機的跟蹤和定位。滿足特定條件時，將當前幀確定為關(guān)鍵幀。

2)局部建圖：線程的輸入為跟蹤線程插入的關(guān)鍵幀?；谛绿砑拥年P(guān)鍵幀，維護和拓展新的局部地圖點，并運用光束平差法優(yōu)化局部地圖中所有關(guān)鍵幀的位姿及局部地圖中的所有地圖點。同時對關(guān)鍵幀進行篩選，剔除冗余關(guān)鍵幀。

3)回環(huán)檢測：線程的輸入為經(jīng)過局部建圖篩選過的關(guān)鍵幀。將當前關(guān)鍵幀的詞袋(Bag of Words，BoW)向量存入全局詞袋數(shù)據(jù)庫中，從而加速后續(xù)幀的匹配。同時檢測是否存在回環(huán)，若存在則通過執(zhí)行位姿圖(Pose Graph)優(yōu)化來優(yōu)化全體關(guān)鍵幀位姿，抑制累計漂移誤差。在位姿圖優(yōu)化完成之后，會臨時發(fā)起一個獨立線程執(zhí)行全局光束平差法，來得到整個系統(tǒng)最優(yōu)結(jié)構(gòu)(地圖點)和運動(關(guān)鍵幀位姿)的結(jié)果[12]。

1.2 單目ORB-SLAM中的光束平差法

ORB-SLAM系統(tǒng)在若干個部分采用光束平差法[13]進行優(yōu)化，包括在跟蹤過程中優(yōu)化當前幀的位姿(純運動BA)，在局部建圖線程中優(yōu)化局部地圖中的關(guān)鍵幀位姿和地圖點位置(局部BA)，以及在回環(huán)檢測后優(yōu)化所有的關(guān)鍵幀位姿和地圖點位置(全局BA)。這三部分的主要內(nèi)容分別如下：

(1)

式中，ρ是Huber代價函數(shù)；Σ是協(xié)方差矩陣，與特征點的尺度相關(guān)；πm是針孔相機投影函數(shù)，定義如下

(2)

其中，(fx,fy)是焦距，(cx,cy)是主點坐標。

由于上述過程不對地圖點位置進行優(yōu)化，相當于進行PnP，但ORB-SLAM作者稱之為純運動BA。

2)局部BA：同樣通過最小化重投影誤差，同時優(yōu)化當前關(guān)鍵幀位姿、與其存在共視關(guān)系的局部關(guān)鍵幀的位姿TL以及局部關(guān)鍵幀所能觀測到的所有地圖點的三維坐標PL，并將與局部關(guān)鍵幀形成共視但不與當前關(guān)鍵幀構(gòu)成共視的關(guān)鍵幀的位姿TF作為約束項參與優(yōu)化。優(yōu)化的過程中不改變TF，從而使得優(yōu)化結(jié)果關(guān)于全局地圖存在一致性。上述過程由公式描述如下

(3)

其中，χj為第j個局部關(guān)鍵幀匹配到地圖點的特征點集合。

3)全局BA：具體過程與局部BA類似，其優(yōu)化對象包括除了第一幀的其余所有關(guān)鍵幀位姿及全局地圖中所有的地圖點。初始幀位置被固定是為了保持全局一致性，消除隨機化[12]。

2 系統(tǒng)精度分析

ORB-SLAM作為基于關(guān)鍵幀BA的視覺SLAM系統(tǒng)，在其運行過程中光束平差法的效果對于系統(tǒng)精度的影響至關(guān)重要。在單目ORB-SLAM視覺里程計部分(不包括回環(huán)和全局BA后端)，針對關(guān)鍵幀位姿的優(yōu)化共有三種情況：第一種是當前幀成功匹配足夠多的已有地圖點后進行的第一次純運動BA，優(yōu)化得到當前幀的初始位姿；第二種是成功更新當前幀共視的局部關(guān)鍵幀和局部地圖點后進行的第二次純運動BA，進一步優(yōu)化當前幀的位姿；第三種是當前幀被確定為關(guān)鍵幀后，針對當前關(guān)鍵幀及其所對應(yīng)的局部關(guān)鍵幀集合和局部地圖點集合的局部BA優(yōu)化，實現(xiàn)局部地圖的整體優(yōu)化，三種BA的優(yōu)化作用分析如下。

2.1 首次純運動BA優(yōu)化

系統(tǒng)在成功初始化后，會對新來的圖像幀進行ORB特征點的提取和對應(yīng)描述子的計算，然后通過基于速度的模型或基于參考關(guān)鍵幀的模型進行圖像特征點與已有地圖點的匹配，并獲得當前幀的初始位姿。其中基于速度的模型主要適用于載體勻速運動的情況，而基于參考關(guān)鍵幀的模型則適用于其他情況。在成功獲得足夠多的匹配點對后，通過純運動BA優(yōu)化(相當于進行PnP)當前幀位姿。此次純運動BA的輸入量是當前幀已得到的特征點與地圖點的匹配關(guān)系，輸出量為優(yōu)化后的當前幀位姿。

圖 2展示了2組來自KITTI數(shù)據(jù)集[14]在不同場景下的數(shù)據(jù)，在首次純運動BA優(yōu)化前后的關(guān)鍵幀位姿與真值進行尺度與坐標系對齊[15]后，得到的運動軌跡示意圖。

通過對圖 2(b)中的軌跡曲線分析可知：若SLAM系統(tǒng)在場景中的運動較為平滑，則在純運動BA優(yōu)化前基于速度模型或者參考關(guān)鍵幀模型得到的位姿初值相比于真值的誤差已經(jīng)不是很大，此時此次BA優(yōu)化的效果并不顯著。通過軌跡曲線圖也可以看出，優(yōu)化前后的關(guān)鍵幀位置幾乎重合。

(a)基于KITTI數(shù)據(jù)集004序列得到的軌跡曲線圖

(b)基于010序列得到的軌跡曲線圖圖2 首次純運動BA優(yōu)化后的KITTI數(shù)據(jù)集軌跡曲線Fig.2 Trajectory curves of sequences of KITTIdataset after first motion-only BA optimization

而從圖 2(a)中的曲線圖可以看出，由于實際場景中存在載體或傳感器抖動，即便是在直路行駛的車輛，其位置在微觀上也會存在抖動(注意橫軸坐標間距相對縱軸而言很小)；又由于傳感器測量誤差和算法等因素影響，其估計結(jié)果存在更加明顯的抖動。但從圖中可以看出，相比較而言純運動BA優(yōu)化前的抖動更加劇烈。純運動BA能夠有效地平滑估計結(jié)果，利用已有匹配地圖點的位置約束修正當前幀的位姿，從而在一定程度上抑制積累誤差，避免將過大的誤差帶入到后續(xù)的優(yōu)化過程中。

2.2 第二次純運動BA優(yōu)化

若當前幀的初始位姿被純運動BA修正并剔除錯誤的匹配點對后，仍有足夠多的正確匹配，系統(tǒng)則判定跟蹤成功。此時系統(tǒng)會基于修正后的當前幀位姿嘗試匹配更多的地圖點，并更新當前幀的共視局部關(guān)鍵幀集合和局部地圖點集合，然后基于這些局部信息進行第二次純運動BA優(yōu)化，進一步修正當前位姿。此次純運動BA的輸入量為更新后的當前幀所有特征點與地圖點的匹配關(guān)系，輸出量為進一步修正的當前幀位姿。

圖 3所示為基于與2.1節(jié)相同的數(shù)據(jù)集得到的第二次純運動BA優(yōu)化前后當前幀位姿與真值的比較結(jié)果。

由圖 3(b)中的軌跡曲線可知，從宏觀角度看，若當前幀的位姿誤差已經(jīng)小于系統(tǒng)修正誤差的最小范圍，即當前幀的位姿已經(jīng)足夠準確，則此次純運動BA不再起到很明顯的作用。

(a)基于KITTI數(shù)據(jù)集004序列得到的軌跡曲線圖

(b)基于010序列得到的軌跡曲線圖圖3 第二次純運動BA優(yōu)化后的KITTI數(shù)據(jù)集軌跡曲線Fig.3 Trajectory curves of sequences of KITTIdataset after second motion-only BA optimization

而由圖 3(a)的比較結(jié)果可以看出，每次優(yōu)化前的當前幀位姿相比于真值已經(jīng)沒有特別大的偏差，但是相對優(yōu)化后的結(jié)果而言，部分關(guān)鍵幀位置仍然存在著一些不合理的跳躍現(xiàn)象，主要是因為首次純運動BA優(yōu)化當前幀位姿時并未充分地利用所有潛在的匹配地圖點的約束信息。因此，第二次純運動BA優(yōu)化當前幀位姿后，不僅進一步修正了誤差較大的幀位姿，同時在有限的地圖點位置約束下使不同時刻的當前幀的位姿的變化盡可能的光滑。此時較大的位姿誤差已經(jīng)得到盡可能的修正，接著會通過局部BA的優(yōu)化調(diào)整所有局部關(guān)鍵幀的位姿及局部地圖點的世界坐標系坐標，從而使得位姿變化更加光滑。

此處要求位姿變化是光滑的，是因為每次的當前關(guān)鍵幀位姿都是基于已被修正過的局部關(guān)鍵幀位姿得到的，在當前關(guān)鍵幀引入到系統(tǒng)的位姿誤差被約束到合理范圍的前提下，不同時刻的當前關(guān)鍵幀的位姿變化應(yīng)當是平滑的。

2.3 局部BA優(yōu)化

若第二次純運動BA修正當前幀位姿后仍能得到足夠多的有效匹配地圖點對，且當前幀滿足被設(shè)置成關(guān)鍵幀的條件，即當前幀與其局部關(guān)鍵幀的相同的地圖匹配點數(shù)量不超過90%，且滿足局部建圖線程空閑或距離上一次插入關(guān)鍵幀的時間超過閾值，當前幀便被設(shè)置成關(guān)鍵幀。接著基于當前關(guān)鍵幀三角化新的局部地圖點并剔除不合理的局部地圖點，然后針對局部地圖中所有的關(guān)鍵幀及地圖點進行一次局部BA優(yōu)化。此次局部BA優(yōu)化的輸入量是當前關(guān)鍵幀所在的局部地圖中所有的局部關(guān)鍵幀位姿和所有局部關(guān)鍵幀中特征點與地圖點的匹配關(guān)系，同時利用所有與局部關(guān)鍵幀形成共視關(guān)系，但不與當前關(guān)鍵幀形成共視關(guān)系的關(guān)鍵幀位姿來約束優(yōu)化結(jié)果，這些位姿在優(yōu)化過程中保持不變，輸出量是所有局部關(guān)鍵幀優(yōu)化后的位姿和局部地圖點優(yōu)化后的三維坐標。

圖 4的三幅曲線圖是基于同一數(shù)據(jù)集局部BA優(yōu)化得到的軌跡曲線圖的不同表現(xiàn)形式。其中(a)和(b)圖的含義是：在一次共有83個關(guān)鍵幀生成的系統(tǒng)運行過程中，第74個和第75個關(guān)鍵幀分別插入時，當前所有的局部關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線圖以及全部軌跡真值，而(c)圖表示所有不同時刻的當前關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線以及真值。第一種方式側(cè)重于橫向展示多次局部BA對局部關(guān)鍵幀位姿的連續(xù)性影響，而第二種方式則側(cè)重于縱向展示每次BA對當前新插入關(guān)鍵幀的直接性影響。

從圖 4(c)的比較結(jié)果可以看出，當前關(guān)鍵幀的位姿在局部BA中得到了進一步的修正，因此軌跡曲線變得更加平滑。考慮到局部BA優(yōu)化是將所有局部關(guān)鍵幀中局部地圖點的重投影誤差作為代價函數(shù)，即在有效修正位姿誤差較大的關(guān)鍵幀的同時，不斷將系統(tǒng)模型帶來的誤差平均到所有局部關(guān)鍵幀位姿上。而從圖 4(a)和(b)的軌跡曲線可以看出，當前關(guān)鍵幀會在之后有限個關(guān)鍵幀插入時再次參與其局部BA的優(yōu)化，從而不斷平均系統(tǒng)位姿解算誤差，使得最終系統(tǒng)輸出的關(guān)鍵幀位姿曲線變得十分平滑，且每個關(guān)鍵幀位姿的誤差均在一定的范圍內(nèi)，如圖 5所示。

(a)

(b)

(c)圖4 (a)和(b)表示基于KITTI數(shù)據(jù)集004序列得到的，第74幀關(guān)鍵幀和第75幀關(guān)鍵幀插入時所有局部關(guān)鍵幀優(yōu)化前后的軌跡曲線示意圖;(c)表示不同時刻的當前關(guān)鍵幀在局部BA優(yōu)化前后的軌跡曲線圖Fig.4 (a) and (b) respectively show the trajectory curves of alllocal keyframes before and after optimization when the 74thkeyfra-me and the 75thkeyframe insert based on the sequence 004 ofKITTI dataset; (c) indicates the trajectory curve of the currentkeyframe before and after local BA optimization at different times

圖5 基于KITTI數(shù)據(jù)集004序列，Visual Odometry部分得到的最終優(yōu)化的全局關(guān)鍵幀軌跡示意圖Fig.5 Final optimized keyframe trajectory graph obtained by thevisual odometry part based on the 004 sequence of KITTI dataset

3 結(jié)論

作為基于關(guān)鍵幀光束平差法的代表方案，ORB-SLAM系統(tǒng)在有效修正誤差較大的全局關(guān)鍵幀位姿的同時，通過不斷調(diào)整局部關(guān)鍵幀位姿和局部地圖點在世界坐標系中的三維坐標，在系統(tǒng)誤差范圍內(nèi)保證位姿變化的連貫性和平滑性。此外，ORB-SLAM還通過各種約束條件和加速手段保證了系統(tǒng)的實時性和魯棒性，并通過回環(huán)檢測的方法進一步精細化修正全局地圖和關(guān)鍵幀位姿，有效地提高了系統(tǒng)的性能和精度。

然而，由于ORB-SLAM嚴重依賴特征點的提取與匹配效果，因此在如紋理缺失的環(huán)境下運行會無法得到足夠多的穩(wěn)定匹配點對，此時光束平差法缺乏足夠的輸入信息，無法有效地修正位姿偏差。這種情況下，可以引入外界的觀測如慣性測量單元(Inertial Measurement Unit, IMU)等，來約束和修正特征缺失帶來的偏差，針對單目情況，還能得到絕對尺度。近年來也有越來越多的研究偏向于視覺/慣性視覺里程計(Visual/Inertial Odometry)方向[16-18]，以期望解決純視覺SLAM存在的問題。