基于ORBSLAM2 改進(jìn)的單線程雙目SLAM 系統(tǒng)

王劭靖

（北方工業(yè)大學(xué) 信息學(xué)院，北京 100144）

0 引言

視 覺 SLAM（Simultaneous Localization And Mapping）是計(jì)算機(jī)視覺和機(jī)器人技術(shù)界的一個(gè)熱門研究課題。目前視覺SLAM 技術(shù)廣泛應(yīng)用于航天機(jī)器人、AR／VR，和自動(dòng)駕駛等領(lǐng)域。

視覺SLAM 系統(tǒng)主要分成前端和后端兩大部分。前端又被稱為視覺里程計(jì)，主要分為基于直接法和基于特征點(diǎn)法兩種；后端主要分為基于濾波優(yōu)化和基于非線性優(yōu)化兩種［1］。近些年來(lái)，基于直接法的代表性研究成果有SVO 和DSO，但容易受光照的影響導(dǎo)致位姿估計(jì)失敗，并且沒(méi)有閉環(huán)檢測(cè)功能，使得估計(jì)的相機(jī)軌跡存在較大漂移，在跟蹤丟失后不容易重定位；而基于特征點(diǎn)法的視覺SLAM 系統(tǒng)對(duì)光照和動(dòng)態(tài)物體不敏感并且穩(wěn)定，還可以利用基于描述子的閉環(huán)算法消除相機(jī)軌跡的漂移，減少累計(jì)誤差，因此基于特征點(diǎn)法的視覺SLAM 系統(tǒng)是目前主流且成熟的視覺方案，并且大多使用基于非線性優(yōu)化的后端。PTAM 是首個(gè)基于特征點(diǎn)法的實(shí)時(shí)視覺SLAM 系統(tǒng)，并且首創(chuàng)了前端和后端并行的雙線程結(jié)構(gòu)［2］。隨后出現(xiàn)的許多優(yōu)秀的視覺SLAM 系統(tǒng)都是基于PTAM 進(jìn)行改進(jìn)的，其中ORBSLAM2 被業(yè)界人士認(rèn)為是當(dāng)前最穩(wěn)定的視覺SLAM 系統(tǒng)，可應(yīng)用于多種場(chǎng)景，在視覺SLAM 領(lǐng)域具有里程碑的意義。

ORBSLAM2 使用了四線程結(jié)構(gòu)。跟蹤線程主要對(duì)提取的特征點(diǎn)利用描述子進(jìn)行匹配，從而構(gòu)造數(shù)據(jù)關(guān)聯(lián)，并根據(jù)數(shù)據(jù)關(guān)聯(lián)關(guān)系對(duì)相機(jī)位姿實(shí)時(shí)地進(jìn)行估計(jì)。在當(dāng)前幀確定為關(guān)鍵幀后，建圖線程使用局部BA 優(yōu)化共視圖（Co-visibility graph）中的關(guān)鍵幀和三維空間點(diǎn)。閉環(huán)線程中優(yōu)秀的閉環(huán)檢測(cè)算法和基于本質(zhì)圖（Essential graph）的位姿圖優(yōu)化保證了ORBSLAM2 能夠有效地減少累計(jì)誤差。此外，全局BA 單獨(dú)占用一個(gè)線程同時(shí)優(yōu)化所有的關(guān)鍵幀和三維空間點(diǎn)。因此，ORBSLAM2 比其他現(xiàn)有視覺SLAM 系統(tǒng)具有更好的準(zhǔn)確性和魯棒性，但其四線程結(jié)構(gòu)給CPU 帶來(lái)了較重的負(fù)擔(dān)，很難將其移植到嵌入式設(shè)備上。此外，航天設(shè)備上需要多種技術(shù)共同協(xié)作，SLAM 的資源占用率的減少不僅能夠降低硬件成本，還能滿足其他技術(shù)對(duì)資源的需求。因此，本文將對(duì)ORBSLAM2 進(jìn)行改進(jìn)，使其能夠以單線程的形式在CPU 上實(shí)時(shí)運(yùn)行并且準(zhǔn)確地估計(jì)相機(jī)位姿。

1 相關(guān)工作

視覺SLAM 系統(tǒng)前端主要負(fù)責(zé)數(shù)據(jù)關(guān)聯(lián)與位姿估計(jì)。良好的位姿初值是建立準(zhǔn)確數(shù)據(jù)關(guān)聯(lián)的前提，有效的運(yùn)動(dòng)模型能夠提供一個(gè)良好的位姿初值，從而加速特征匹配的速度和精度，并且加快位姿估計(jì)的速度。常見的運(yùn)動(dòng)模型通常在相機(jī)相鄰兩幀之間應(yīng)用平滑的運(yùn)動(dòng)約束［3］，即假設(shè)前后兩幀的運(yùn)動(dòng)速度相似，DP-PTAM、DT-SLAM 和ORBSLAM2 都利用恒速運(yùn)動(dòng)模型假設(shè)相機(jī)前后兩幀的運(yùn)動(dòng)速度相同，進(jìn)而預(yù)測(cè)相機(jī)的位姿初值。SVO 和LSD-SLAM直接令相機(jī)當(dāng)前時(shí)刻的位姿等于上一時(shí)刻的位姿，即假設(shè)當(dāng)前幀的運(yùn)動(dòng)速度為0。但在實(shí)際應(yīng)用中，相機(jī)的運(yùn)動(dòng)速度是經(jīng)常變化的，不符合上述的假設(shè)，因此本文將相機(jī)的運(yùn)動(dòng)假設(shè)為速度不斷變化的加速運(yùn)動(dòng)，將平滑運(yùn)動(dòng)約束的思想應(yīng)用到前后幀的加速度上，假設(shè)前后幀的加速度相同來(lái)計(jì)算當(dāng)前時(shí)刻的速度，最終預(yù)測(cè)出當(dāng)前時(shí)刻的位姿初值，以提高數(shù)據(jù)關(guān)聯(lián)的效率和正確性。

由于BA 同時(shí)優(yōu)化大量的三維空間點(diǎn)坐標(biāo)和相機(jī)位姿，因此計(jì)算復(fù)雜度高導(dǎo)致運(yùn)算時(shí)間過(guò)長(zhǎng)，很難實(shí)現(xiàn)快速而準(zhǔn)確的收斂［4］，通常需要開啟多個(gè)線程來(lái)分擔(dān)計(jì)算量以滿足實(shí)時(shí)性的要求。Chatterjee［5］等人提出了一種基于L1范數(shù)的旋轉(zhuǎn)平均（Rotation averaging）的方法能夠魯棒、快速地單獨(dú)優(yōu)化相機(jī)的朝向。近些年來(lái)許多研究都證實(shí)了基于旋轉(zhuǎn)平均的解耦合優(yōu)化方法比BA 的效率和精度更高，能夠快速、精確地優(yōu)化出相機(jī)的位姿。Fan［6］等人提出了將三維空間點(diǎn)的位置、相機(jī)的朝向和平移進(jìn)行解耦合優(yōu)化的方法來(lái)提高優(yōu)化的效率和精確性，并且假設(shè)噪聲服從拉普拉斯分布，以提高對(duì)存在大噪聲數(shù)據(jù)優(yōu)化的魯棒性。本文采用基于平均的解耦合優(yōu)化方法代替ORBSLAM2 中的Local BA，以提高優(yōu)化的效率、魯棒性和精確性，并減少SLAM 的線程數(shù)量。

位姿圖優(yōu)化是SLAM 進(jìn)行全局優(yōu)化的重要手段，能夠有效減少相機(jī)軌跡的漂移，從而減少累積誤差。但隨著相機(jī)的不斷探索，位姿圖的規(guī)模會(huì)越來(lái)越大，使得優(yōu)化效率受限。Kretzschmar［7］等人提出了基于信息論的方法，減少位姿圖優(yōu)化的規(guī)模，通過(guò)計(jì)算互信息和信息增益的期望挑選出需要邊緣化的節(jié)點(diǎn)，并提出了一種基于Chow-Liu 樹的近似邊緣化方法保證位姿圖的稀疏性，但理論復(fù)雜且實(shí)現(xiàn)較困難；Johannsson［8］、Zeng［9］等人認(rèn)為當(dāng)連續(xù)幀連續(xù)檢測(cè)到閉環(huán)時(shí)，說(shuō)明相機(jī)正重新經(jīng)過(guò)之前探索過(guò)的區(qū)域，就將連續(xù)檢測(cè)到閉環(huán)的多個(gè)幀視為冗余，并將多個(gè)冗余幀和回環(huán)幀之間的約束進(jìn)行合并，直接將冗余節(jié)點(diǎn)去除，不進(jìn)行邊緣化操作，但不適用于大場(chǎng)景；Latif［10］等人認(rèn)為在二維平面上運(yùn)動(dòng)且不能原地旋轉(zhuǎn)的機(jī)器人在做直線運(yùn)動(dòng)時(shí)朝向是幾乎不變的，因此直線運(yùn)動(dòng)中的約束對(duì)位姿圖優(yōu)化提供的是線性約束，于是就將直線運(yùn)動(dòng)中的多個(gè)約束合并為一個(gè)約束，只對(duì)位姿圖中朝向改變較大的位姿節(jié)點(diǎn)進(jìn)行優(yōu)化，但對(duì)相機(jī)的運(yùn)動(dòng)條件限制嚴(yán)格，不適用于三維場(chǎng)景中能夠自由轉(zhuǎn)動(dòng)的相機(jī)；車聯(lián)網(wǎng)領(lǐng)域，Keogh［11］等人提出的開放窗口算法是一種快速壓縮軌跡數(shù)據(jù)的簡(jiǎn)單方法，該算法將車輛軌跡根據(jù)彎曲程度用多個(gè)線段子軌跡表示。本文將車聯(lián)網(wǎng)領(lǐng)域中軌跡壓縮的思想應(yīng)用到SLAM 中，并對(duì)Latif 等人的方法進(jìn)行改進(jìn)，以此只優(yōu)化朝向變化較大的位姿節(jié)點(diǎn)，減少位姿圖優(yōu)化的規(guī)模，并且適用于在三維場(chǎng)景中自由轉(zhuǎn)動(dòng)的相機(jī)。

2 具體改進(jìn)

本文提出的SLAM 系統(tǒng)是基于ORBSLAM2 的框架改進(jìn)的，本文SLAM 系統(tǒng)的流程圖如圖1 所示，主要改進(jìn)：

圖1 本文SLAM 系統(tǒng)的流程圖Fig.1 The flow chart diagram of our SLAM system

（1）使用加速運(yùn)動(dòng)模型預(yù)測(cè)相機(jī)的位姿，并結(jié)合環(huán)形匹配策略提高特征匹配的精度和速度；

（2）在當(dāng)前幀確定為關(guān)鍵幀后，利用基于開放窗口的關(guān)鍵幀聚類算法對(duì)其進(jìn)行聚類，減少位姿圖優(yōu)化的規(guī)模，加速位姿圖優(yōu)化的速度；

（3）在噪聲服從拉普拉斯分布假設(shè)的前提下，使用解耦合優(yōu)化三維點(diǎn)空間、相機(jī)的朝向和平移的方法代替Local BA，以此提高優(yōu)化的速度和精度。

2.1 基于平滑運(yùn)動(dòng)約束的加速運(yùn)動(dòng)模型

良好的位姿初值不僅僅能提高數(shù)據(jù)關(guān)聯(lián)的正確性，還可以減少優(yōu)化的迭代次數(shù)，加速收斂速度。本文考慮到相機(jī)的運(yùn)動(dòng)大部分情況是速度不斷變化的加速運(yùn)動(dòng)，勻速運(yùn)動(dòng)的情況較少，并且勻速運(yùn)動(dòng)和減速運(yùn)動(dòng)也分別是一種加速度為0 和加速度為負(fù)數(shù)的特殊加速運(yùn)動(dòng)。因此本文利用加速運(yùn)動(dòng)模型估計(jì)相機(jī)位姿的初值，并將平滑運(yùn)動(dòng)約束的思想應(yīng)用到運(yùn)動(dòng)模型的加速度上，假設(shè)相鄰兩幀的加速度相同，則有式（1）：

其中，Tt-1、Tt-2、Tt-3分別代表t-1 幀和t-2 幀和t -3 幀的位姿。

因此t幀的速度和預(yù)測(cè)的位姿可由以上公式推導(dǎo)而出，式（4）和式（5）：

由此能夠根據(jù)t幀的預(yù)測(cè)位姿將t -1 幀觀測(cè)到的三維空間點(diǎn)投影至t幀進(jìn)行匹配。為了使特征匹配更可靠，本文加入了環(huán)形匹配策略。在ORBSLAM2 中，每一幀左右圖像的特征匹配關(guān)系是在每一幀初始化時(shí)確定的，因此在通過(guò)投影得到前后幀左圖像中特征點(diǎn)的對(duì)應(yīng)關(guān)系后，可直接得到前后幀右圖像中特征點(diǎn)的對(duì)應(yīng)關(guān)系，利用描述子驗(yàn)證環(huán)形是否閉合，最終確定匹配成功的特征點(diǎn)，如圖2所示。特征匹配結(jié)果對(duì)比如圖3 所示，可以看出本文的特征匹配數(shù)量相比于ORBSLAM2 少，但準(zhǔn)確度更高，進(jìn)一步剔除了更多的誤匹配。

圖2 環(huán)形匹配示意圖Fig.2 Schematic diagram of circle matching

圖3 特征匹配結(jié)果對(duì)比示意圖Fig.3 Schematic diagram of feature matching results comparison

2.2 基于開放窗口的關(guān)鍵幀聚類算法

為了挑選出運(yùn)動(dòng)過(guò)程中朝向變化較大的位姿節(jié)點(diǎn)，本文提出了基于開放窗口的關(guān)鍵幀聚類算法。該算法首先對(duì)位姿圖中的位姿節(jié)點(diǎn)在?3空間中聚類，以此將位姿圖劃分為多個(gè)線段子軌跡，每個(gè)線段子軌跡都是一段直線運(yùn)動(dòng)，每個(gè)直線運(yùn)動(dòng)的尾結(jié)點(diǎn)和下一個(gè)直線運(yùn)動(dòng)的首節(jié)點(diǎn)之間會(huì)發(fā)生較大的朝向變化。此外，由于相機(jī)在直線運(yùn)動(dòng)過(guò)程中也會(huì)出現(xiàn)朝向變化較大的情況，因此每一段在?3空間中確定的子軌跡需要繼續(xù)在SO（3）空間中進(jìn)行聚類，以此有效挑選出朝向變化較大的節(jié)點(diǎn)進(jìn)行優(yōu)化，減小位姿圖優(yōu)化規(guī)模。

?3空間中關(guān)鍵幀聚類示意如圖4 所示，在當(dāng)前幀確定為關(guān)鍵幀時(shí)，令其為xcurrent，窗口中的位姿節(jié)點(diǎn)為xstart和xend之間的所有位姿節(jié)點(diǎn)，根據(jù)xstart和xend可確定直線L，由此可以計(jì)算窗口內(nèi)所有位姿節(jié)點(diǎn)相對(duì)直線L的偏移量，即位姿節(jié)點(diǎn)到直線L的垂直距離，即式（6）：

圖4 ? 3空間中關(guān)鍵幀聚類示意圖Fig.4 Schematic diagram of keyframe clustering in ? 3

其中，t∈?3，代表位姿節(jié)點(diǎn)的平移部分。

當(dāng)偏移距離最大的xi的偏移距離大于閾值時(shí)，將xstart和xi之間的所有節(jié)點(diǎn)聚為一類，表示在?3空間中的聚類結(jié)果，用xstart和xi代表這段子軌跡，并更新窗口，即令xstart為xi＋1，令xend為xcurrent。

每一段在?3空間中確定的子軌跡再分為兩步在SO（3）空間中進(jìn)行聚類：

第一步，在xstart與xi構(gòu)成的子軌跡中判斷相鄰位姿節(jié)點(diǎn)之間的朝向變化，當(dāng)xj和xj＋1朝向變化大于閾值時(shí)，就將這段子軌跡繼續(xù)劃分，即xstart和xj之間的所有位姿節(jié)點(diǎn)聚為一段子軌跡，并繼續(xù)判斷后面相鄰位姿節(jié)點(diǎn)之間的朝向變化，如圖5 所示；

圖5 SO（3）空間中基于相鄰關(guān)鍵幀朝向變化的關(guān)鍵幀聚類示意圖Fig.5 Schematic diagram of keyframe clustering based on the orientation change of adjacent keyframes in SO（3）

第二步，考慮到存在相鄰幀之間朝向變化不大而不相鄰兩幀之間朝向變化較大的情況，本文將第一步中的子軌跡在SO（3）空間中進(jìn)行第二次聚類，即當(dāng)xstart與xk之間朝向變化大于閾值時(shí)，就將xstart和xj這段子軌跡繼續(xù)劃分，即xstart和xk-1之間的所有位姿節(jié)點(diǎn)聚為一段子軌跡，并繼續(xù)判斷后面不相鄰位姿節(jié)點(diǎn)之間的朝向變化，如圖6 所示。具體的基于開放窗口的關(guān)鍵幀聚類算法如算法1 所示，其中Dthreshold代表在?3空間中進(jìn)行聚類的偏移距離閾值，thetaThreshold代表在SO（3）空間中進(jìn)行聚類的角度閾值。

圖6 SO（3）空間中基于不相鄰關(guān)鍵幀朝向變化的關(guān)鍵幀聚類示意圖Fig.6 Schematic diagram of keyframe clustering based on the orientation change of non-adjacent keyframes in SO（3）

在KITTI 07 序列上應(yīng)用該聚類算法的結(jié)果示意圖如圖7 所示。圖7 中的大節(jié)點(diǎn)是朝向變化較大的節(jié)點(diǎn)，即需要優(yōu)化的位姿節(jié)點(diǎn)，可以看出本文提出的基于開放窗口的關(guān)鍵幀聚類算法能夠有效減少位姿圖優(yōu)化的規(guī)模。針對(duì)其余未進(jìn)行位姿圖優(yōu)化的節(jié)點(diǎn)位姿，本文使用位姿傳播的方式更新。

圖7 基于開放窗口的關(guān)鍵聚類算法在KITTI07 上的聚類結(jié)果示意圖Fig.7 Schematic diagram of the clustering results of the keyframe clustering algorithm based on the open window on KITTI07

2.3 解耦合優(yōu)化方法

為了在SLAM 后端避免復(fù)雜的BA 優(yōu)化，本文在假設(shè)噪聲服從拉普拉斯分布的前提下構(gòu)建L1范數(shù)誤差，使用解耦合優(yōu)化三維空間點(diǎn)、相機(jī)朝向和平移的方法代替Local BA。

得益于根據(jù)數(shù)據(jù)關(guān)聯(lián)建立的共視圖，一個(gè)三維空間點(diǎn)在多個(gè)關(guān)鍵幀中的觀測(cè)能夠快速被找到。因此，三維空間點(diǎn)在多個(gè)關(guān)鍵幀的觀測(cè)根據(jù)雙目視差能夠計(jì)算出多個(gè)坐標(biāo)，如圖8 所示。本文采用?3上的IRLS（Iterative Reweighted Least Squares）算法優(yōu)化三維空間點(diǎn)坐標(biāo)為

圖8 利用雙目視差多次重建三維空間點(diǎn)示意圖Fig.8 Schematic diagrams of 3D space points are reconstructed multiple times according to binocular disparity

對(duì)于相機(jī)朝向的優(yōu)化，本文利用數(shù)據(jù)關(guān)聯(lián)關(guān)系快速得到當(dāng)前關(guān)鍵幀與多個(gè)共視關(guān)鍵幀的特征匹配關(guān)系，從而使用五點(diǎn)法求解出當(dāng)前關(guān)鍵幀和多個(gè)共視關(guān)鍵幀之間本質(zhì)矩陣（Essential matrix），因此可得到當(dāng)前關(guān)鍵幀的多個(gè)朝向，使用SO（3）上的IRLS（Iteration Reweight Least Square）算法優(yōu)化相機(jī)的朝向?yàn)?/p>

其中，代表相機(jī)朝向的初值，本文取多個(gè)朝向中的任意一個(gè)朝向作為初值，R*代表優(yōu)化后的相機(jī)朝向。

最后利用優(yōu)化后的三維空間點(diǎn)和相機(jī)的朝向根據(jù)重投影誤差優(yōu)化相機(jī)的平移為

其中，ui代表三維空間點(diǎn)Pi在當(dāng)前關(guān)鍵幀上的二維觀測(cè)點(diǎn)坐標(biāo)，π（R Pi ＋t）代表三維空間點(diǎn)Pi對(duì)應(yīng)在當(dāng)前幀上的投影。

3 實(shí)驗(yàn)結(jié)果與分析

本文的實(shí)驗(yàn)環(huán)境為Ubuntu 18.04、配置為Inter Core i7-5500 CPU、主頻2.4 GHz、8 G RAM。為了檢測(cè)本文SLAM 算法的性能，本文從精度和速度兩個(gè)方面在KITTI 數(shù)據(jù)集以及New Tsukuba 數(shù)據(jù)集上進(jìn)行測(cè)試評(píng)估。KITTI 數(shù)據(jù)集采集了大型室外場(chǎng)景下的視頻序列，并提供了真實(shí)軌跡的數(shù)據(jù)；New Tsukuba 數(shù)據(jù)集是由計(jì)算機(jī)合成的靜態(tài)室內(nèi)小場(chǎng)景。本文選擇與目前開源的Viso2 和ORBSLAM2 算法進(jìn)行對(duì)比，使用均方根誤差（RMSE）來(lái)衡量相機(jī)真實(shí)位姿與估計(jì)的位姿之間的誤差。本文在算法1中使用的偏移距離閾值為0.5，角度閾值為0.2 rad。

Viso2、ORBSLAM2 和本文方法在KITTI 數(shù)據(jù)集上和New Tsukuba 數(shù)據(jù)集的測(cè)試結(jié)果見表1，可以看出本文方法的精度比ORBSLAM2 的精度與Viso2 的精度高，并且由于Viso2 沒(méi)有閉環(huán)檢測(cè)功能，在具有閉環(huán)的序列中表現(xiàn)差距更加明顯。

表1 Viso2、ORBSLAM2 和本文方法在KITTI 和New Tsukuba 數(shù)據(jù)集上的RMSE（m）對(duì)比Tab.1 RMSE（m）comparison of Viso2，ORBSLAM2 and our method on KITTI and New Tsukuba

本文算法在精度方面的優(yōu)勢(shì)主要得益于加速運(yùn)動(dòng)模型以及環(huán)形匹配策略提高了數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性，盡管在數(shù)據(jù)關(guān)聯(lián)過(guò)程中錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)并不能完全消除，但基于構(gòu)建L1范數(shù)誤差的解耦合優(yōu)化方法提高了對(duì)存在異常值數(shù)據(jù)進(jìn)行的優(yōu)化魯棒性。

ORBSLAM2 與本文方法在New Tsukuba 數(shù)據(jù)集上處理每一幀的時(shí)間對(duì)比圖如圖9 所示，其中細(xì)折線代表ORBSLAM2 處理每一幀的時(shí)間，粗折線代表本文方法處理每一幀的時(shí)間，可以看出本文方法處理每一幀的時(shí)間整體上要低于ORBSLAM2 處理每一幀的時(shí)間。本文的SLAM 算法與ORBSLAM2 在New Tsukuba 數(shù)據(jù)集上的計(jì)算效率和線程數(shù)量對(duì)比見表2，可以看出本文方法的計(jì)算效率高于ORBSLAM2，并且能夠以單線程的形式運(yùn)行。

圖9 ORBSLAM2 與本文方法在New Tsukuba 數(shù)據(jù)集上處理每一幀的時(shí)間對(duì)比圖Fig.9 Time comparison of ORBSLAM2 and our method on the New Tsukuba dataset

表2 本文算法與ORBSLAM2 在New Tsukuba 數(shù)據(jù)集上的計(jì)算效率和線程數(shù)量對(duì)比Tab.2 Comparison of computational efficiency and number of threads between our algorithm and ORBSLAM2 on the New Tsukuba dataset

本文算法對(duì)比于ORBSLAM2 在計(jì)算效率和線程數(shù)量方面的優(yōu)勢(shì)主要?dú)w因于如下幾點(diǎn)：

（1）加速運(yùn)動(dòng)模型預(yù)測(cè)的良好位姿初值和較為準(zhǔn)確的數(shù)據(jù)關(guān)聯(lián)使得位姿估計(jì)過(guò)程中的僅運(yùn)動(dòng)BA（motion-only BA）計(jì)算的梯度更為準(zhǔn)確，能夠使位姿估計(jì)過(guò)程更快地收斂；

（2）解耦合的優(yōu)化方法計(jì)算復(fù)雜度比BA 低，由此減少了優(yōu)化時(shí)間；

（3）基于開放窗口的關(guān)鍵幀聚類算法能夠明顯減少位姿圖優(yōu)化的規(guī)模，使得全局優(yōu)化的效率得以提升；

（4）由于位姿估計(jì)過(guò)程并不需要太多數(shù)量的三維空間點(diǎn)，過(guò)多的三維空間點(diǎn)對(duì)位姿估計(jì)的精度提升并不明顯，還會(huì)增加計(jì)算的復(fù)雜性。因此本文只利用當(dāng)前幀的共視關(guān)鍵幀所觀測(cè)到的三維空間點(diǎn)對(duì)當(dāng)前幀進(jìn)行位姿估計(jì)，相比ORBSLAM2 利用當(dāng)前幀的共視關(guān)鍵幀以及共視關(guān)鍵幀的共視關(guān)鍵幀所觀測(cè)到的三維空間點(diǎn)對(duì)當(dāng)前幀進(jìn)行位姿估計(jì)的方法，能夠有效減少不必要的計(jì)算量，并且保證位姿估計(jì)的精度。

在KITTI 數(shù)據(jù)集和New Tsukuba 數(shù)據(jù)集上本文的方法、Viso2、ORBSLAM2 的軌跡和真實(shí)軌跡對(duì)比如圖10 所示，其中粗實(shí)線為真實(shí)軌跡，細(xì)實(shí)線為本文方法估計(jì)的軌跡，粗虛線為ORBSLAM2 估計(jì)的軌跡，細(xì)虛線為Viso2 估計(jì)的軌跡，可以看出本文方法估計(jì)出的軌跡更接近于真實(shí)軌跡。

圖10 KITTI 數(shù)據(jù)集和New Tsukuba 數(shù)據(jù)集上本文的方法、Viso2、ORBSLAM2 的軌跡和真實(shí)軌跡對(duì)比圖Fig.10 Comparison of trajectories and real trajectories of our method、Viso2、ORBSLAM2 on the KITTI dataset and the New Tsukuba dataset

4 結(jié)束語(yǔ)

本文主要針對(duì)ORBSLAM2 進(jìn)行3 方面改進(jìn)：

（1）使用加速運(yùn)動(dòng)模型預(yù)測(cè)相機(jī)的位姿，并結(jié)合環(huán)形匹配策略提高了數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性和位姿優(yōu)化的收斂速度；

（2）基于開放窗口算法提出了一種簡(jiǎn)單有效的關(guān)鍵幀聚類算法，從而減少位姿圖優(yōu)化的規(guī)模；

（3）使用基于拉普拉斯分布的解耦合優(yōu)化方法代替Local BA 提高位姿優(yōu)化的速度和精度。

在KITTI 和New Tsukuba 數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，本文改進(jìn)后的SLAM 系統(tǒng)能夠以單線程的形式運(yùn)行，并且運(yùn)行速度和精度都高于ORBSLAM2。