韓彥峰 ，唐超超，肖科

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

近年來，自動駕駛技術(shù)、VR 技術(shù)、無人機(jī)日新月異的發(fā)展，對地圖和定位的精度需求愈來愈高.作為各種創(chuàng)新性技術(shù)中的關(guān)鍵環(huán)節(jié)，地圖和定位精度的可靠性和準(zhǔn)確性具有重要的意義，同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）成為目前國內(nèi)外學(xué)者的熱門研究領(lǐng)域［1-4］.SLAM 技術(shù)可搭載雷達(dá)、相機(jī)等傳感器，因此基于相機(jī)傳感器的視覺SLAM 技術(shù)相比于激光SLAM 技術(shù)具有設(shè)備元件成本低、功耗低等優(yōu)點，更受到研究者們的青睞.相機(jī)可提供魯棒性更優(yōu)質(zhì)的圖像信息，能夠更準(zhǔn)確地識別位置信息，而位置識別是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)隊設(shè)計完成了基于深度相機(jī)的RGB-D SLAMV2 系統(tǒng)，首次利用RGB-D 相機(jī)特性進(jìn)行稠密點云地圖的構(gòu)建.2015年，Mur-Artal 等人［9］提出了ORB-SLAM 系統(tǒng)，在PTAM 算法框架上增加了地圖初始化和閉環(huán)檢測環(huán)節(jié)，能夠很好地消除運行產(chǎn)生的累積誤差，顯著提升了地圖跟蹤的快速性和構(gòu)建地圖的準(zhǔn)確性，也成為了一個完整的SLAM 系統(tǒng).2016 年，原團(tuán)隊在此基礎(chǔ)上提出了ORB-SLAM2 系統(tǒng)［10］，在回環(huán)檢測模塊后又增加了全局BA（Bundle Adjustment）環(huán)節(jié)，提高了系統(tǒng)的魯棒性；支持單目、雙目、RGB-D 等多種常見相機(jī).2018 年，Bescos 等人［11］提出基于ORB-SLAM2的DynaSLAM 算法系統(tǒng)，采用Mask-CNN 算法檢測潛在動態(tài)物體，增加了動態(tài)目標(biāo)檢測和背景修復(fù)的能力，在單目、立體和RGB-D 配置的動態(tài)場景中非常強(qiáng)大；同年，清華大學(xué)提出了DS-SLAM 算法［12］，采用SegNet 進(jìn)行語義分割并建立語義八叉樹地圖，精確定位圖像場景下的各個物體.2021年，中國科學(xué)技術(shù)大學(xué)團(tuán)隊提出一種新的基于稀疏特征的視覺SLAM算法（DP-SLAM［13］），該算法基于移動概率傳播模型進(jìn)行動態(tài)關(guān)鍵點檢測，結(jié)合幾何約束和語義分割的結(jié)果，在貝葉斯概率估計框架中跟蹤動態(tài)關(guān)鍵點，克服了幾何約束和語義信息的偏差，提高了視覺SLAM系統(tǒng)的準(zhǔn)確性和魯棒性.

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

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

ORB-SLAM2 是一種前端視覺里程計基于“Ori?ented FAST”關(guān)鍵點和BRIEF 描述子，實現(xiàn)特征點的提取匹配，后端基于非線性優(yōu)化BA 方式的視覺SLAM 系統(tǒng).系統(tǒng)共有三個線程，由跟蹤線程（Track?ing）、局部建圖線程（Local Mapping）和閉環(huán)檢測線程（Loop Closing）組成［18］.RGB-D 模式下，跟蹤線程負(fù)責(zé)根據(jù)提供的特征點深度信息，進(jìn)行實時位姿的定位跟蹤并優(yōu)化處理.局部建圖線程通過所獲得的關(guān)鍵幀創(chuàng)建新地圖點并剔除地圖外點，將關(guān)鍵幀的位姿進(jìn)行局部BA 優(yōu)化，刪除冗余關(guān)鍵幀和地圖點.閉環(huán)檢測線程利用數(shù)學(xué)模型評價相鄰關(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ū)χ車鷪鼍皩崟r構(gòu)建點云地圖，并轉(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 稠密點云地圖與八叉樹地圖構(gòu)建

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

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

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

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

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

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

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

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

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

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

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

2.2 雙層濾波處理

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

2.2.1 離群點濾波

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

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

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

2.2.2 降采樣體素濾波

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

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

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

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

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

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

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

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

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

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

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

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

3.1 稠密建圖效果評估

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

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

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

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

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

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

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

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

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

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

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

3.2 軌跡誤差評估分析

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

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

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

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

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

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

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

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

表3 RGB-D ORB-SLAM2與RGB-D SLAMV2絕對軌跡誤差評價指標(biāo)對比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)場景下或低動態(tài)場景下，本文系統(tǒng)的絕對軌跡誤差（ATE）與相對位姿誤差（APE）的RMSE、Mean、SD 均小于RGB-D SLAMV2、DynaSLAM 計算所得，本文改進(jìn)系統(tǒng)的兩項評價指標(biāo)至少都有50%～70%的提升，特別是在低動態(tài)幀序列數(shù)據(jù)集“fr1_xyz”與“fr3_long_household”下的絕對軌跡誤差（ATE）評價指標(biāo)對比上，本文系統(tǒng)提升幅度明顯，而動態(tài)場景“fr3_walking_xyz”、“fr3_walking_halfsphere”與Dy?naSLAM 相比還有一定差距.驗證了本文系統(tǒng)在低動態(tài)場景下具有更好的定位精度.

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

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

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

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

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

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

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

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

表7 點云地圖與八叉樹地圖存儲空間Tab.7 Point cloud map and octree map storage space

5 結(jié)語

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

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

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

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