基于改進(jìn)ORB-SLAM2 算法的RGB-D 稠密地圖構(gòu)建

韓彥峰 ，唐超超，肖科

（重慶大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，重慶 400044）

近年來，自動(dòng)駕駛技術(shù)、VR 技術(shù)、無人機(jī)日新月異的發(fā)展，對(duì)地圖和定位的精度需求愈來愈高.作為各種創(chuàng)新性技術(shù)中的關(guān)鍵環(huán)節(jié)，地圖和定位精度的可靠性和準(zhǔn)確性具有重要的意義，同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）成為目前國(guó)內(nèi)外學(xué)者的熱門研究領(lǐng)域［1-4］.SLAM 技術(shù)可搭載雷達(dá)、相機(jī)等傳感器，因此基于相機(jī)傳感器的視覺SLAM 技術(shù)相比于激光SLAM 技術(shù)具有設(shè)備元件成本低、功耗低等優(yōu)點(diǎn)，更受到研究者們的青睞.相機(jī)可提供魯棒性更優(yōu)質(zhì)的圖像信息，能夠更準(zhǔn)確地識(shí)別位置信息，而位置識(shí)別是SLAM 技術(shù)系統(tǒng)中的關(guān)鍵模塊.

2007 年，Klein 等人［5］提出的PTAM 系統(tǒng)，是視覺SLAM 發(fā)展中重要的里程碑，一舉開創(chuàng)了多線程SLAM 系統(tǒng)的先河，將前端跟蹤線程和后端優(yōu)化建圖線程并行化處理，后端優(yōu)化開創(chuàng)性地采用了非線性優(yōu)化方案，而非傳統(tǒng)的濾波器優(yōu)化（如卡爾曼濾波［6］、粒子濾波［7］）.2014 年，Endres［8］團(tuán)隊(duì)設(shè)計(jì)完成了基于深度相機(jī)的RGB-D SLAMV2 系統(tǒng)，首次利用RGB-D 相機(jī)特性進(jìn)行稠密點(diǎn)云地圖的構(gòu)建.2015年，Mur-Artal 等人［9］提出了ORB-SLAM 系統(tǒng)，在PTAM 算法框架上增加了地圖初始化和閉環(huán)檢測(cè)環(huán)節(jié)，能夠很好地消除運(yùn)行產(chǎn)生的累積誤差，顯著提升了地圖跟蹤的快速性和構(gòu)建地圖的準(zhǔn)確性，也成為了一個(gè)完整的SLAM 系統(tǒng).2016 年，原團(tuán)隊(duì)在此基礎(chǔ)上提出了ORB-SLAM2 系統(tǒng)［10］，在回環(huán)檢測(cè)模塊后又增加了全局BA（Bundle Adjustment）環(huán)節(jié)，提高了系統(tǒng)的魯棒性；支持單目、雙目、RGB-D 等多種常見相機(jī).2018 年，Bescos 等人［11］提出基于ORB-SLAM2的DynaSLAM 算法系統(tǒng)，采用Mask-CNN 算法檢測(cè)潛在動(dòng)態(tài)物體，增加了動(dòng)態(tài)目標(biāo)檢測(cè)和背景修復(fù)的能力，在單目、立體和RGB-D 配置的動(dòng)態(tài)場(chǎng)景中非常強(qiáng)大；同年，清華大學(xué)提出了DS-SLAM 算法［12］，采用SegNet 進(jìn)行語(yǔ)義分割并建立語(yǔ)義八叉樹地圖，精確定位圖像場(chǎng)景下的各個(gè)物體.2021年，中國(guó)科學(xué)技術(shù)大學(xué)團(tuán)隊(duì)提出一種新的基于稀疏特征的視覺SLAM算法（DP-SLAM［13］），該算法基于移動(dòng)概率傳播模型進(jìn)行動(dòng)態(tài)關(guān)鍵點(diǎn)檢測(cè)，結(jié)合幾何約束和語(yǔ)義分割的結(jié)果，在貝葉斯概率估計(jì)框架中跟蹤動(dòng)態(tài)關(guān)鍵點(diǎn)，克服了幾何約束和語(yǔ)義信息的偏差，提高了視覺SLAM系統(tǒng)的準(zhǔn)確性和魯棒性.

ORB-SLAM2 系統(tǒng)具有精確度高、實(shí)時(shí)穩(wěn)定性好、應(yīng)用場(chǎng)景多等優(yōu)點(diǎn)，但存在如下缺點(diǎn)：所構(gòu)建的地圖比較稀疏而非稠密，點(diǎn)云地圖存儲(chǔ)空間大，無法有效描述空間占據(jù)狀態(tài)導(dǎo)致無法滿足導(dǎo)航、路徑規(guī)劃以及高層次的人機(jī)交互［14-17］.針對(duì)上述問題，本文提出一種基于RGB-D 模式下ORB-SLAM2 框架的VSLAM 方案，添加稠密建圖線程，實(shí)時(shí)記錄關(guān)鍵幀位姿進(jìn)行點(diǎn)云拼接，引入雙層濾波算法降低點(diǎn)云冗余度，位姿稠密回環(huán)處理優(yōu)化點(diǎn)云空間位置，并最終轉(zhuǎn)換成八叉樹地圖.本文測(cè)試多個(gè)開源數(shù)據(jù)集，在軌跡精度和建圖效果等方面均有提高，驗(yàn)證了改進(jìn)算法的有效性.

1 ORB-SLAM2系統(tǒng)框架簡(jiǎn)介

ORB-SLAM2 是一種前端視覺里程計(jì)基于“Ori?ented FAST”關(guān)鍵點(diǎn)和BRIEF 描述子，實(shí)現(xiàn)特征點(diǎn)的提取匹配，后端基于非線性優(yōu)化BA 方式的視覺SLAM 系統(tǒng).系統(tǒng)共有三個(gè)線程，由跟蹤線程（Track?ing）、局部建圖線程（Local Mapping）和閉環(huán)檢測(cè)線程（Loop Closing）組成［18］.RGB-D 模式下，跟蹤線程負(fù)責(zé)根據(jù)提供的特征點(diǎn)深度信息，進(jìn)行實(shí)時(shí)位姿的定位跟蹤并優(yōu)化處理.局部建圖線程通過所獲得的關(guān)鍵幀創(chuàng)建新地圖點(diǎn)并剔除地圖外點(diǎn)，將關(guān)鍵幀的位姿進(jìn)行局部BA 優(yōu)化，刪除冗余關(guān)鍵幀和地圖點(diǎn).閉環(huán)檢測(cè)線程利用數(shù)學(xué)模型評(píng)價(jià)相鄰關(guān)鍵幀的相似性程度，判斷關(guān)鍵幀閉環(huán)情況，有助于減少軌跡的累積漂移.

2 改進(jìn)RGB-D ORB-SLAM2系統(tǒng)框架

本文VSLAM 系統(tǒng)方案在原有RGB-D 模式下的ORB-SLAM2算法框架上進(jìn)行擴(kuò)展，添加稠密建圖功能，使其能夠?qū)χ車鷪?chǎng)景實(shí)時(shí)構(gòu)建點(diǎn)云地圖，并轉(zhuǎn)換為可表征周圍環(huán)境的3D 八叉樹地圖.系統(tǒng)整體構(gòu)架如圖1 所示，圖中虛線方框內(nèi)表示本文主要的擴(kuò)展工作.

圖1 改進(jìn)RGB-D ORB-SLAM2系統(tǒng)架構(gòu)Fig.1 Improved RGB-D ORB-SLAM2 system architecture

2.1 稠密點(diǎn)云地圖與八叉樹地圖構(gòu)建

2.1.1 稠密點(diǎn)云地圖構(gòu)建原理

RGB-D 相機(jī)提供的深度圖像和彩色圖像實(shí)現(xiàn)二維坐標(biāo)轉(zhuǎn)換為三維點(diǎn)云數(shù)據(jù).定義任意一個(gè)空間三維點(diǎn)X，坐標(biāo)表示為X=[xw，yw，zw]，在相機(jī)坐標(biāo)系下三維點(diǎn)所對(duì)應(yīng)已知點(diǎn)的像素坐標(biāo)表示為：x=[u，v，1]，根據(jù)相機(jī)針孔模型成像原理可以變換得到點(diǎn)云的空間位置信息：

式中：K代表相機(jī)內(nèi)參，由相機(jī)標(biāo)定所得；R代表旋轉(zhuǎn)矩陣，t代表平移向量；z是深度值和實(shí)際空間距離的比例關(guān)系因子.

世界坐標(biāo)系原點(diǎn)與相機(jī)坐標(biāo)系原點(diǎn)重合，即沒有旋轉(zhuǎn)和平移，所以R為單位矩陣，t為零矩陣，通過公式（1）變換得到點(diǎn)云的三維空間位置坐標(biāo)：

2.1.2 八叉樹地圖的構(gòu)建與更新

本文提出了一種在導(dǎo)航中比較常用的、壓縮性能好的構(gòu)建八叉樹地圖方式.如圖2（a）所示，一個(gè)大立方體不斷均勻地分成八塊，直到變成最小的方塊為止，白色表示未被占據(jù)，黑色表示被占據(jù)不可再分割.圖2（b）中為八叉樹結(jié)構(gòu)，每一個(gè)節(jié)點(diǎn)類比成體素，未被占據(jù)的節(jié)點(diǎn)繼續(xù)展開形成八個(gè)子節(jié)點(diǎn)，被信息占據(jù)的節(jié)點(diǎn)停止展開.當(dāng)向地圖中添加信息時(shí)，由于實(shí)際的物體經(jīng)常連在一起，空白的地方也會(huì)常常連在一起，所以大多數(shù)八叉樹節(jié)點(diǎn)無須展開到葉子層面［19］，因此八叉樹地圖比點(diǎn)云地圖節(jié)省了大量存儲(chǔ)空間.

圖2 八叉樹示意圖Fig.2 Diagram of the octree

八叉樹節(jié)點(diǎn)存儲(chǔ)了它是否被占據(jù)的信息，從點(diǎn)云層面來講，可以用0 表示空白，1 表示被占據(jù).由于噪聲的影響，會(huì)造成節(jié)點(diǎn)所存儲(chǔ)的占據(jù)信息判別不準(zhǔn)確，出現(xiàn)未占據(jù)、占據(jù)兩種情況往復(fù)循環(huán)的現(xiàn)象，甚至存在未知狀態(tài).因此本文提出使用概率對(duì)數(shù)值（Log-odds）來表達(dá)某節(jié)點(diǎn)是否被占據(jù).設(shè)y∈R為概率對(duì)數(shù)值，x為0～1 的概率，那么它們之間的變換由公式（3）給出logit變換描述：

某節(jié)點(diǎn)不斷觀測(cè)到占據(jù)時(shí)，y值增加；觀測(cè)到空白時(shí)，y值減小.用數(shù)學(xué)形式來表示，設(shè)某節(jié)點(diǎn)為n，觀測(cè)數(shù)據(jù)為z，那么從開始時(shí)刻到t時(shí)刻某節(jié)點(diǎn)的概率對(duì)數(shù)值為L(zhǎng)(n|z1：t)，t+1時(shí)刻為：

通過公式（4）可以計(jì)算節(jié)點(diǎn)某時(shí)刻對(duì)數(shù)概率值，εoccupy為預(yù)先設(shè)定的參數(shù)，用于計(jì)算節(jié)點(diǎn)某時(shí)刻的概率對(duì)數(shù)值：

通過所得的對(duì)數(shù)概率值，結(jié)合RGB-D 數(shù)據(jù)就可以更新出整個(gè)八叉樹地圖.在RGB-D 圖像中觀測(cè)到某個(gè)像素帶有深度d，就說明在深度值對(duì)應(yīng)的空間上觀察到了一個(gè)占據(jù)信息，并且從相機(jī)光心出發(fā)到這個(gè)點(diǎn)的線段上應(yīng)該是沒有物體的，否則會(huì)被遮擋［20-24］.利用此信息就可以很好地對(duì)八叉樹地圖進(jìn)行更新，并且能處理運(yùn)動(dòng)的結(jié)構(gòu).

2.2 雙層濾波處理

實(shí)際建圖過程中，RGB-D 相機(jī)的深度量程有限，則需要根據(jù)實(shí)際相機(jī)的量程來設(shè)定上下限，濾除掉采集不到的點(diǎn)云.為了建圖效果達(dá)到更優(yōu)，本文對(duì)所采集到的點(diǎn)云進(jìn)行體素網(wǎng)格降采樣濾波器和外點(diǎn)去除濾波器雙層濾波處理，使得稠密地圖更加清晰、準(zhǔn)確，帶來更好的視覺效果.

2.2.1 離群點(diǎn)濾波

由于在點(diǎn)云拼接過程中，常常會(huì)產(chǎn)生稀疏的離群點(diǎn)，導(dǎo)致稠密點(diǎn)云配準(zhǔn)失敗，出現(xiàn)稠密點(diǎn)云地圖效果不好的問題.為了解決此問題，使用外點(diǎn)去除濾波器對(duì)每個(gè)點(diǎn)云的鄰域進(jìn)行一次統(tǒng)計(jì)學(xué)分析，設(shè)置一定閾值修剪掉那些不符合一定標(biāo)準(zhǔn)的點(diǎn).

本文通過對(duì)輸入數(shù)據(jù)中點(diǎn)到其鄰近點(diǎn)的距離分布計(jì)算，得到每個(gè)點(diǎn)到其所有鄰近點(diǎn)的平均距離.由于結(jié)果近似為高斯分布，均值μ和標(biāo)準(zhǔn)差σ決定其形狀分布，平均距離在標(biāo)準(zhǔn)范圍（由全局距離平均值和方差定義）之外的點(diǎn)，可被定義為離群點(diǎn)，并可從數(shù)據(jù)集中去除掉.根據(jù)高斯分布d～N(μ，σ)模型化距離參數(shù)，計(jì)算所有點(diǎn)與其鄰近點(diǎn)的均值和標(biāo)準(zhǔn)差，公式如下：

式中：dij表示每個(gè)點(diǎn)到其鄰近點(diǎn)的距離，i=[1，…，m]表示共有m個(gè)數(shù)據(jù)點(diǎn)，j=[1，…，k]表示每個(gè)點(diǎn)都有k個(gè)相鄰點(diǎn)，表示其與鄰近點(diǎn)的距離均值.

2.2.2 降采樣體素濾波

鄰近關(guān)鍵幀相互存在視野重疊，重疊區(qū)域會(huì)存在大量位置相近的點(diǎn)，占用大量?jī)?nèi)存空間.本文對(duì)余下點(diǎn)云使用體素網(wǎng)格濾波器進(jìn)行降采樣，保證在某個(gè)一定大小的立方體（體素）內(nèi)只保留位于立方體重心上的點(diǎn)［20］.這樣既可以保持點(diǎn)云的形狀特征，又能極大地降低全局點(diǎn)云的冗余，減少了點(diǎn)云存儲(chǔ)所占用的內(nèi)存空間.

根據(jù)坐標(biāo)集合，求取X、Y、Z三個(gè)坐標(biāo)軸上的最大值xmax、ymax、zmax和最小值xmin、ymin、zmin，然后根據(jù)三個(gè)坐標(biāo)軸上的最大值和最小值求得點(diǎn)云最小包圍盒的邊長(zhǎng)lx、ly、lz：

設(shè)置體素小柵格邊長(zhǎng)為lcell，將三個(gè)坐標(biāo)軸分別均等化分為M、N、L份，最終將整個(gè)空間劃分成Sum個(gè)體素小柵格：

對(duì)所劃分的每個(gè)體素小柵格進(jìn)行標(biāo)號(hào)處理，標(biāo)號(hào)為(i，j，k)，確定每個(gè)點(diǎn)云所屬的小柵格坐標(biāo)位置：

計(jì)算每個(gè)體素小柵格重心坐標(biāo)，以重心代替該小柵格內(nèi)的所有點(diǎn)云數(shù)據(jù)；當(dāng)重心點(diǎn)坐標(biāo)不存在柵格內(nèi)時(shí)，取小柵格內(nèi)在距離上最接近重心點(diǎn)的點(diǎn)坐標(biāo)表示該小柵格內(nèi)的所有數(shù)據(jù)點(diǎn)云的坐標(biāo)，至此完成整個(gè)精簡(jiǎn)過程：

式中：cijk、pi、k分別為體素小柵格的重心、數(shù)據(jù)點(diǎn)、點(diǎn)數(shù).

2.3 稠密點(diǎn)云回環(huán)處理

ORB-SLAM2 系統(tǒng)回環(huán)融合時(shí)，將執(zhí)行全局BA優(yōu)化.如圖3（a）所示，通過圖優(yōu)化模型［21-23］，將關(guān)鍵幀位姿和地圖點(diǎn)設(shè)置成圖頂點(diǎn)為待優(yōu)化變量，使用特征點(diǎn)法獲取兩個(gè)位姿節(jié)點(diǎn)之間相對(duì)運(yùn)動(dòng)的估計(jì)作為圖邊，以此省卻大量特征點(diǎn)優(yōu)化計(jì)算，僅把它們當(dāng)作位姿估計(jì)節(jié)點(diǎn)的約束，從而構(gòu)建成位姿圖（Pose Graph），如圖3（b）所示.保留了關(guān)鍵幀位姿并優(yōu)化，并將系統(tǒng)中的位移累積誤差均攤到每一個(gè)關(guān)鍵幀上，以此校正整個(gè)軌跡，使其更加準(zhǔn)確.

圖3 位姿優(yōu)化原理Fig.3 Principle of position optimization

本文在此基礎(chǔ)上提出一種稠密點(diǎn)云回環(huán)處理的方法，具體流程如圖4 所示.系統(tǒng)基于詞袋模型進(jìn)行回環(huán)檢測(cè)優(yōu)化時(shí)，記錄每一個(gè)關(guān)鍵幀位姿數(shù)據(jù)，系統(tǒng)判定為回環(huán)狀態(tài)后，執(zhí)行全局BA 優(yōu)化更新關(guān)鍵幀的位姿與地圖點(diǎn)坐標(biāo)，將優(yōu)化后每個(gè)關(guān)鍵幀位姿的點(diǎn)云重新進(jìn)行拼接并進(jìn)行濾波處理，更新優(yōu)化局部點(diǎn)云地圖，極大地提高了系統(tǒng)的魯棒性，減小了誤差，保證稠密點(diǎn)云地圖的全局一致性.

圖4 稠密點(diǎn)云回環(huán)流程圖Fig.4 Dense point cloud loopback flow chart

3 試驗(yàn)結(jié)果和分析

3.1 稠密建圖效果評(píng)估

為了驗(yàn)證本文改進(jìn)算法的性能效果，與參考文獻(xiàn)［24］同樣基于ORB-SLAM2 系統(tǒng)提出的建圖算法（GX ORB-SLAM2）進(jìn)行試驗(yàn)比較評(píng)估.本文選取了部分慕尼黑工業(yè)大學(xué)發(fā)布的開源TUM 數(shù)據(jù)集［25］，詳細(xì)信息見表1.

表1 TUM數(shù)據(jù)集信息Tab.1 TUM dataset information

搭建測(cè)試環(huán)境配備CPU：Intel Core i7-9700 @3.00GHZ、GPU：NVIDIA GTX1650、24G 內(nèi)存、512G 固態(tài)和Ubuntu18.04操作系統(tǒng).實(shí)驗(yàn)中使用G2O、Pango?lin、Eigen、OpenCV 等第三方軟件庫(kù)進(jìn)行算法優(yōu)化與可視化展示，本文設(shè)置濾波參數(shù)深度范圍為0.01

本文RGB-D ORB-SLAM2 系統(tǒng)與GX ORBSLAM2 系統(tǒng)都可以實(shí)時(shí)構(gòu)建場(chǎng)景的稠密地圖，對(duì)其所挑選的4 個(gè)數(shù)據(jù)集進(jìn)行試驗(yàn)對(duì)比，兩者建圖效果如圖5和圖6所示.

圖5 fr2_xyz數(shù)據(jù)集下的稠密點(diǎn)云地圖Fig.5 Dense point cloud map under fr2_xyz dataset

圖6 fr1_desk2數(shù)據(jù)集下的稠密點(diǎn)云地圖Fig.6 Dense point cloud map under fr1_desk2 dataset

從圖5 和圖6 可以看出：本文RGB-D ORBSLAM2 系統(tǒng)與GX ORB-SLAM2 系統(tǒng)在“fr2_xyz”和“fr1_desk2”數(shù)據(jù)集下都能實(shí)現(xiàn)三維場(chǎng)景稠密構(gòu)建的目標(biāo)，稠密點(diǎn)云地圖并未出現(xiàn)明顯位置錯(cuò)位且還原度較高，滿足場(chǎng)景構(gòu)建的基本需求.如圖5（b）和圖6（b）紅色橢圓部分，可以看出GX ORB-SLAM2 系統(tǒng)得到的未濾波稠密點(diǎn)云地圖存在大量的空間無用雜點(diǎn)，占用空間內(nèi)存；本文系統(tǒng)經(jīng)過雙層濾波處理后其稠密地圖濾去了超過精確測(cè)量范圍外的點(diǎn)云，基本不存在冗余雜點(diǎn).兩個(gè)SLAM 系統(tǒng)在四個(gè)數(shù)據(jù)集下的點(diǎn)云數(shù)量進(jìn)行對(duì)比，本文RGB-D ORB-SLAM2 系統(tǒng)構(gòu)建的稠密點(diǎn)云地圖的點(diǎn)云數(shù)均少于GX ORBSLAM2系統(tǒng)，點(diǎn)云數(shù)量大幅度降低，詳細(xì)對(duì)比數(shù)據(jù)如表2所示.

表2 稠密點(diǎn)云數(shù)量對(duì)比Tab.2 Comparison of the number of dense point clouds

下面對(duì)數(shù)據(jù)集的稠密點(diǎn)云地圖中的某些細(xì)節(jié)部分進(jìn)一步詳細(xì)比較，看是否存在局部錯(cuò)位缺失與模糊殘影的情況，如圖7所示.

兩個(gè)系統(tǒng)在“fr1_desk2”數(shù)據(jù)集下的詳細(xì)放大部分如圖7（a）和圖7（b）所示，重點(diǎn)觀察圖中紅色橢圓部分.圖7（a）中為GX ORB-SLAM2 系統(tǒng)所得，最左邊的書本明顯出現(xiàn)了局部模糊殘影的情況，與中間的書本發(fā)生了輪廓模糊融合，紋路不清晰.圖7（b）中為本文RGB-D ORB-SLAM2 系統(tǒng)所得，三本書的輪廓清晰，還原度高，左側(cè)兩本書未發(fā)生模糊融合，分界輪廓線明顯.

圖7 稠密點(diǎn)云地圖細(xì)節(jié)部分Fig.7 Detail section of the dense point cloud map

綜上可知，本文RGB-D ORB-SLAM2 系統(tǒng)在點(diǎn)云數(shù)量、冗余度、建圖局部細(xì)節(jié)部分都明顯優(yōu)于GX ORB-SLAM2系統(tǒng).

3.2 軌跡誤差評(píng)估分析

TUM 數(shù)據(jù)集提供了相機(jī)位姿的真實(shí)軌跡，根據(jù)系統(tǒng)獲得的估計(jì)軌跡，利用Evo 評(píng)估軟件［26］可以分別對(duì)兩者的絕對(duì)軌跡誤差（ATE）進(jìn)行分析，適用于評(píng)價(jià)系統(tǒng)的性能；相對(duì)位姿誤差（RPE），適用于評(píng)估系統(tǒng)的漂移程度.

針對(duì)本文改進(jìn)的RGB-D ORB-SLAM2 系統(tǒng)與RGB-D SLAMV2 系統(tǒng)，分別對(duì)所選取的四個(gè)數(shù)據(jù)集進(jìn)行絕對(duì)軌跡誤差（ATE）和相對(duì)位姿誤差（APE）對(duì)比分析，具體分析結(jié)果如圖8～圖11所示.

圖8 fr1_xyz數(shù)據(jù)集下的絕對(duì)軌跡誤差Fig.8 Absolute trajectory error under fr1_xyz dataset

圖8～圖11 的橫縱豎軸為三維空間下的X、Y、Z軸，灰色虛線reference 表示各個(gè)數(shù)據(jù)集真實(shí)軌跡值，彩色實(shí)線代表系統(tǒng)所獲得的估計(jì)軌跡，圖右側(cè)從藍(lán)到紅的色帶顯示誤差大小［19］.對(duì)比圖8 和圖9 可得，在數(shù)據(jù)集“fr1_xyz”和“fr2_xyz”場(chǎng)景下，本文RGB-D ORB-SLAM2 系統(tǒng)的紅色軌跡都遠(yuǎn)少于RGB-D SLAMV2 系統(tǒng)，表明前者真實(shí)位姿與估計(jì)位姿的軌跡最大誤差明顯小于后者.因此，驗(yàn)證得到改進(jìn)后的算法定位精度更準(zhǔn)確，軌跡更具有全局一致性.

圖9 fr2_xyz數(shù)據(jù)集下的絕對(duì)軌跡誤差Fig.9 Absolute trajectory error under fr2_xyz dataset

圖10 和圖11 給出了基于數(shù)據(jù)集“fr1_desk2”和“fr3_long_office_household”的相對(duì)位姿誤差分析，可以看出本文RGB-D ORB-SLAM2系統(tǒng)的軌跡重合度高于RGB-D SLAMV2 的軌跡重合度，絕大部分實(shí)線估計(jì)軌跡與虛線真實(shí)軌跡重合，無較大軌跡錯(cuò)位，說明本文改進(jìn)系統(tǒng)的漂移穩(wěn)定性更高.為了更加直觀地評(píng)價(jià)系統(tǒng)性能的提升，表3和表4使用均方根誤差（RMSE）和均值誤差（Mean），表5 使用均方根誤差和標(biāo)準(zhǔn)差（SD）作為主要評(píng)價(jià)指標(biāo)，取兩個(gè)指標(biāo)數(shù)據(jù)5次平均值來定量分析系統(tǒng)的定位精度和系統(tǒng)漂移.

圖10 fr1_desk2數(shù)據(jù)集下的相對(duì)位姿誤差Fig.10 Relative positional errors in the fr1_desk2 dataset

圖11 fr3_long_office_household 數(shù)據(jù)集下的相對(duì)位姿誤差Fig.11 Relative positional errors in the fr3_long_office_household dataset

表3 RGB-D ORB-SLAM2與RGB-D SLAMV2絕對(duì)軌跡誤差評(píng)價(jià)指標(biāo)對(duì)比Tab.3 Comparison of RGB-D ORB-SLAM2 and RGB-D SLAMV2 absolute trajectory error evaluation metrics

表3 和表4 的RMSE1、RMSE2、Mean1、Mean2 代表RGB-D SLAMV2、RGB-D ORB-SLAM2 系統(tǒng)所得的均方根誤差與中值誤差；表5 和表6 的RMSE、SD代表DynaSLAM 所得.具體分析數(shù)據(jù)可知，靜態(tài)場(chǎng)景下或低動(dòng)態(tài)場(chǎng)景下，本文系統(tǒng)的絕對(duì)軌跡誤差（ATE）與相對(duì)位姿誤差（APE）的RMSE、Mean、SD 均小于RGB-D SLAMV2、DynaSLAM 計(jì)算所得，本文改進(jìn)系統(tǒng)的兩項(xiàng)評(píng)價(jià)指標(biāo)至少都有50%～70%的提升，特別是在低動(dòng)態(tài)幀序列數(shù)據(jù)集“fr1_xyz”與“fr3_long_household”下的絕對(duì)軌跡誤差（ATE）評(píng)價(jià)指標(biāo)對(duì)比上，本文系統(tǒng)提升幅度明顯，而動(dòng)態(tài)場(chǎng)景“fr3_walking_xyz”、“fr3_walking_halfsphere”與Dy?naSLAM 相比還有一定差距.驗(yàn)證了本文系統(tǒng)在低動(dòng)態(tài)場(chǎng)景下具有更好的定位精度.

表4 RGB-D ORB-SLAM2與RGB-D SLAMV2相對(duì)位姿誤差評(píng)價(jià)指標(biāo)對(duì)比Tab.4 Comparison of RGB-D ORB-SLAM2 and RGB-D SLAMV2 relative positional error evaluation metrics

表5 RGB-D ORB-SLAM2與DynaSLAM 絕對(duì)軌跡誤差評(píng)價(jià)指標(biāo)對(duì)比Tab.5 Comparison of RGB-D ORB-SLAM2 and DynaSLAM absolute trajectory error evaluation metrics

表6 RGB-D ORB-SLAM2與DynaSLAM 相對(duì)位姿誤差評(píng)價(jià)指標(biāo)對(duì)比Tab.6 Comparison of RGB-D ORB-SLAM2 and DynaSLAM relative positional error evaluation metrics

4 構(gòu)建八叉樹地圖測(cè)試

八叉樹地圖的可更新性和可優(yōu)化性明顯優(yōu)于稠密點(diǎn)云地圖，可滿足更高精度需求的導(dǎo)航任務(wù).本文對(duì)所選取的四個(gè)數(shù)據(jù)集進(jìn)行點(diǎn)云地圖轉(zhuǎn)換構(gòu)建到分辨率為0.05、具有顏色信息的三維八叉樹地圖［27］，具體建圖效果如圖12所示.

從圖12 可以看出，對(duì)所選取的四個(gè)數(shù)據(jù)集構(gòu)造的八叉樹地圖重要信息依然保留完整，如電腦、桌子等，可以清楚地分辨物體之間的位置關(guān)系，說明八叉樹地圖在節(jié)省大量存儲(chǔ)空間的同時(shí)，完整地保留了地圖的重要信息.表7 給出了在四個(gè)數(shù)據(jù)下八叉樹地圖與點(diǎn)云地圖所占內(nèi)存空間的大小.

圖12 帶顏色的八叉樹地圖Fig.12 Map of octree with colors

由表7 可知，在同一環(huán)境下，相比于稠密點(diǎn)云地圖，八叉樹地圖的存儲(chǔ)空間呈指數(shù)式下降，實(shí)現(xiàn)了由兆存儲(chǔ)到字節(jié)存儲(chǔ)的轉(zhuǎn)變，極大地減少了內(nèi)存消耗.八叉樹地圖用來存儲(chǔ)描述地圖的占據(jù)信息，可以更好地實(shí)時(shí)擴(kuò)展、低消耗地滿足未來移動(dòng)機(jī)器人對(duì)高精度導(dǎo)航的需求.

表7 點(diǎn)云地圖與八叉樹地圖存儲(chǔ)空間Tab.7 Point cloud map and octree map storage space

5 結(jié)語(yǔ)

本文提出了一種RGB-D ORB-SLAM2的改進(jìn)稠密建圖算法，通過體素濾波和去除離群點(diǎn)濾波算法有效剔除離群雜點(diǎn)，降低點(diǎn)云地圖的冗余度；通過稠密回環(huán)處理，進(jìn)一步優(yōu)化關(guān)鍵幀位姿，提升了點(diǎn)云位姿準(zhǔn)確度，加快了建圖速度.主要結(jié)論如下：

1）試驗(yàn)表明，在同一TUM 數(shù)據(jù)集下，相比于同樣基于ORB-SLAM2 系統(tǒng)的稠密建圖算法，本文的改進(jìn)算法從整體建圖質(zhì)量到局部建圖細(xì)節(jié)都有明顯的優(yōu)化提高，點(diǎn)云冗余度大幅度降低.

2）本文RGB-D ORB-SLAM2系統(tǒng)的絕對(duì)軌跡誤差與相對(duì)位姿誤差在靜態(tài)或低動(dòng)態(tài)均小于RGB-D SLAMV2、DynaSLAM 系統(tǒng)，系統(tǒng)的魯棒性和漂移性得到提升.

3）為了解決稠密點(diǎn)云地圖無法用于移動(dòng)機(jī)器人的導(dǎo)航與路徑規(guī)劃等問題，添加了八叉樹地圖的構(gòu)建環(huán)節(jié)，可降低內(nèi)存消耗，便于地圖的維護(hù)和更新優(yōu)化.