夏琳琳，蒙 躍，王 策，劉瑞敏

（東北電力大學(xué) 自動(dòng)化工程學(xué)院，吉林 132012）

近年來，基于視覺的同時(shí)定位與建圖（Visual Simultaneous Localization and Mapping, VSLAM）促進(jìn)了實(shí)時(shí)三維重建技術(shù)的發(fā)展。在單目重建領(lǐng)域，VSLAM 利用視覺傳感器獲取圖像信息，可快速計(jì)算機(jī)器人相對運(yùn)動(dòng)位姿，使在線重建成為可能。與離線運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)（Structure From Motion, SFM）[1]思想相比，更具應(yīng)用前景[2]。

對于基于VSLAM 的實(shí)時(shí)三維重建而言，提高圖像的輸入質(zhì)量并處理是進(jìn)行有效三維重建的前提[3]，然而，室內(nèi)場景普遍存在低光照、光照不均等問題，所獲取的圖像對比度低，極大地增加了VSLAM 前端特征點(diǎn)提取與匹配的難度。2016 年開源的單目稀疏直接法里程計(jì)（Direct Sparse Odometry, DSO）采用最小化光度誤差來尋求圖像中點(diǎn)與點(diǎn)的對應(yīng)關(guān)系，在以連續(xù)幀取得同一物體成像的過程中，考慮對相機(jī)進(jìn)行光度標(biāo)定[4]。然而，突出的環(huán)境光強(qiáng)變化問題會影響到DSO 視覺子系統(tǒng)的工作。基于此，本文提出由基于入射-反射模型的Retinex 理論[5]，構(gòu)建一個(gè)自適應(yīng)的圖像增強(qiáng)模型，針對室內(nèi)光線不均、不平衡問題，對圖像不同區(qū)域進(jìn)行OSTU 動(dòng)態(tài)閾值分割判別，以圖像預(yù)處理思想替代DSO 的光度標(biāo)定。

為了消除位姿漂移累積誤差的問題，本文旨在開發(fā)一類基于稀疏直接法單尺度Retinex 的SLAM 系統(tǒng)（Retinex DSO with loop closure detection, RDSOL）。設(shè)計(jì)上繼承了DSO 的主體框架。

主旨設(shè)計(jì)包括：一、對圖像進(jìn)行光度矯正以解決光照不均帶來的影響，提高SLAM算法魯棒性的同時(shí)，避免傳統(tǒng)DSO 光度標(biāo)定的時(shí)間消耗，提升三維重建的速度。二、提出一種閉環(huán)檢測策略，利用該策略對關(guān)鍵幀的圖像特征點(diǎn)進(jìn)行提取并構(gòu)建詞袋（Bag-of-Words，BoW），通過詞袋判斷兩幀之間的相似度，實(shí)現(xiàn)閉環(huán)檢測。三、進(jìn)行全局位姿圖優(yōu)化，減少機(jī)器人運(yùn)動(dòng)過程幀間遞推的累積誤差，最終構(gòu)建出全局一致的三維稠密點(diǎn)云地圖。

1 RDSOL 系統(tǒng)框架

所設(shè)計(jì)的RDSOL 系統(tǒng)框架如圖1 所示，整個(gè)系統(tǒng)可劃分為3 個(gè)模塊。在圖像預(yù)處理模塊，相機(jī)獲取新圖像時(shí)，首先對圖像進(jìn)行OTSU 二值化處理并統(tǒng)計(jì)其灰度值，以判斷當(dāng)前場景的光照是否均勻、是否需進(jìn)行單尺度Retinex（Single Scale Retinex, SSR）圖像增強(qiáng)。

圖1 RDSOL 系統(tǒng)框架 Fig.1 RDSOL framework

后端模塊繼承了DSO 的典型功能，即使用一個(gè)由若干系統(tǒng)關(guān)鍵幀組成的滑動(dòng)窗口（Sliding Window）[6]組成優(yōu)化后端，采用邊緣化策略，對關(guān)鍵幀中的路標(biāo)點(diǎn)和相機(jī)進(jìn)行聯(lián)合優(yōu)化，并將優(yōu)化過程限制在有限的關(guān)鍵幀窗口中。

閉環(huán)檢測關(guān)系到估計(jì)的軌跡和地圖在長時(shí)間下的正確性[7]。在閉環(huán)檢測模塊，本文提出一種將ORB 特征與原始點(diǎn)選擇策略結(jié)合的匹配方法，并通過TUM數(shù)據(jù)集、KITTI 數(shù)據(jù)集及實(shí)際場景測試，驗(yàn)證閉環(huán)軌跡效果。以下詳盡探討各模塊中算法的設(shè)計(jì)與實(shí)現(xiàn)。

2 算法設(shè)計(jì)與實(shí)現(xiàn)

2.1 位姿估計(jì)

室內(nèi)場景存在明暗變化，在確保不形成局部圖像梯度的條件下，RDSOL 采集圖像特征點(diǎn)位置及像素灰度信息，利用直接法最小化特征塊重投影光度誤差來優(yōu)化相機(jī)的位姿，其計(jì)算過程如下：

設(shè)Ii、Ij為相鄰兩幀圖像，若將第i幀中點(diǎn)p投影到第j幀，則其最小化光度誤差函數(shù)Epj為：

式中，R、t分別為相機(jī)的旋轉(zhuǎn)矩陣和平移向量，由兩幀之間計(jì)算可得；∏c、分別表示重投影、逆重投影變換。

2.2 單尺度Retinex 圖像增強(qiáng)

室內(nèi)場景需布置光源來確保光照。相機(jī)在圖像采集過程中，因環(huán)境光照不佳或物體表面采光不均等原因，常呈現(xiàn)圖像光照不均問題。直接法位姿估計(jì)以像素灰度值為參考，為確?；叶戎禍?zhǔn)確性，DSO 采用光度標(biāo)定來矯正光照不均[8]。然而，光度標(biāo)定需固定場景，使相機(jī)在不同曝光時(shí)間下，重復(fù)拍照獲得一系列圖像，此過程耗時(shí)巨大，且僅在當(dāng)前環(huán)境有效。即使基于精細(xì)的相機(jī)成像模型，若環(huán)境不同，或環(huán)境相同場景不同，都難以準(zhǔn)確標(biāo)定成像過程中的光度參數(shù)，實(shí)現(xiàn)圖像亮度值的校正。

由于 DSO 的光度標(biāo)定實(shí)現(xiàn)條件苛刻，為使RDSOL 具有普遍適用性，本文采用分類判斷且有針對性的單尺度Retinex 圖像增強(qiáng)策略（因所獲圖像所有區(qū)域的光照并非都不均勻）用于圖像預(yù)處理階段。

圖像的分類判斷與處理基于全局二值化算法OTSU[9]，即統(tǒng)計(jì)整個(gè)圖像的直方圖特性以自動(dòng)選取二值化閾值。具體地，對圖像灰度級0～255 進(jìn)行遍歷搜索，若灰度值為某個(gè)值的時(shí)候，前景和背景的方差最大（類間方差最大），則當(dāng)前圖像灰度值便是所求閾值T。利用該閾值T對子圖像進(jìn)行分類：當(dāng)T＞圖像灰度時(shí)，判定某區(qū)域圖像光照均勻；當(dāng)T＜圖像灰度時(shí)，判定某區(qū)域圖像光照不均，需進(jìn)行Retinex 圖像增強(qiáng)。

Retinex 是一種以色感一致性為基礎(chǔ)的圖像增強(qiáng)方法，其目的是在獲取輸入圖像原始信息 I( x,y) 后，計(jì)算出光源照射的入射分量L( x,y)，通過對L( x,y)的濾除來減少光照不平衡的影響，獲得被測物體原本的反射分量 R(x,y)。具體原理為：

式中，（x,y）為輸入圖像的每個(gè)點(diǎn)。

本文采用單尺度Retinex 算法求解R（x,y），即[10]：

式中，i表示第i個(gè)顏色通道，分為紅色、綠色和藍(lán)色三通道，分別代表物體對長波、中波和短波光線的反射能力；*表示卷積運(yùn)算；F（x,y）為高斯平滑函數(shù)，其表達(dá)式為：

式中，K為歸一化常數(shù)，滿足∫∫F(x,y)dxdy=1；C是函數(shù)所選的尺度參數(shù)。

Retinex 算法通過形態(tài)學(xué)閉合的方式，彌補(bǔ)了圖像細(xì)節(jié)的損失，使得高質(zhì)量的圖像輸出更利于后續(xù)SLAM 光流跟蹤及圖像特征提取。為便于說明，在此做了一組光照對比實(shí)驗(yàn)，如圖2 所示。

圖2 室內(nèi)圖像增強(qiáng)效果Fig.2 Indoor image enhancement results

圖2(a)為某實(shí)驗(yàn)室的真實(shí)場景，由于光照不均勻，場景上存在明顯的背光現(xiàn)象。隨著相機(jī)的運(yùn)動(dòng)，由于無法將新的特征點(diǎn)加入到構(gòu)建的地圖中，地圖無法隨相機(jī)視角的前移而進(jìn)行拓展，SLAM 系統(tǒng)無法繼續(xù)運(yùn)行。對圖2(a)紅色框選中的過亮（灰度值較高）部分進(jìn)行還原，獲得單尺度Retinex 處理后的圖2(c)，并分別繪制兩圖像的灰度分布直方圖，如圖2(b)、圖2(d)所示。可以看出，圖2(c)的亮度更為均衡，圖2(d)的像素個(gè)數(shù)隨亮度的分布呈現(xiàn)更為均勻，表明Retinex算法有效提高了低光照條件下室內(nèi)圖像的顏色細(xì)節(jié)、保真度、對比度和清晰度，使得增強(qiáng)后的圖像視覺效果和梯度變換明顯改善，更利于SLAM 后端的像素追蹤與閉環(huán)檢測特征點(diǎn)提取。

2.3 閉環(huán)檢測

本文將ORB-SLAM 中基于圖像特征描述子的閉環(huán)檢測策略與DSO 結(jié)合，提出一種將ORB 特征與原始點(diǎn)選擇策略結(jié)合的匹配方法，其核心思想是將原始圖像進(jìn)行向下采樣得到不同分辨率的圖像金字塔，然后對不同層的圖像金字塔并行進(jìn)行ORB 特征提取與DSO 梯度點(diǎn)提取，該方法在高效提取圖像梯度的同時(shí)，確保了所提取的特征匹配描述子的尺度不變性，提高了特征匹配的精度。當(dāng)確定了相應(yīng)的閉合環(huán)路特征描述子之后，計(jì)算歷史幀與閉環(huán)候選幀之間的位姿變換。將位姿轉(zhuǎn)換到相似變換群Sim(3)上，并利用相似變換位姿圖進(jìn)行全局優(yōu)化，進(jìn)而分散累積誤差，消除其對系統(tǒng)的影響。RDSOL 閉環(huán)檢測框架如圖3 所示。

圖3 閉環(huán)檢測框架Fig.3 Designed loop closure detection framework

在閉環(huán)檢測模塊中，對每一個(gè)新的圖像幀進(jìn)行預(yù)處理，篩選出關(guān)鍵幀和普通幀，結(jié)合前文所提到的直接法由關(guān)鍵幀估計(jì)相機(jī)位姿。同時(shí)，利用算法為每個(gè)關(guān)鍵幀計(jì)算ORB 描述符并將其轉(zhuǎn)化為詞向量，使用開源詞袋模型DBoW3 構(gòu)建詞袋數(shù)據(jù)庫BoW，通過查詢BoW 為當(dāng)前關(guān)鍵幀選取閉環(huán)匹配子（從被邊緣化的關(guān)鍵幀中選?。?。對于每個(gè)匹配子，在獲得精確3D信息之后，將當(dāng)前幀與窗口中所有關(guān)鍵幀進(jìn)行匹配，最終得到地圖中3D 點(diǎn)和當(dāng)前幀中2D點(diǎn)之間的匹配關(guān)系。本文利用EPnP 計(jì)算相機(jī)位姿，并采用隨機(jī)采樣一致性（RANSAC）算法[11]剔除離群點(diǎn)。在計(jì)算出初始 SE(3)變化后，建立閉環(huán)誤差模型，使用Gauss-Newton 法對其迭代優(yōu)化，并將校正后Sim(3)上的位姿轉(zhuǎn)換至特殊歐式群SE(3)。所定義的閉環(huán)誤差項(xiàng)為：

式中，p'uv為當(dāng)前幀中匹配點(diǎn)坐標(biāo)，d-1為其逆深度值；puv為閉環(huán)候選幀中匹配點(diǎn)坐標(biāo)；c∏為當(dāng)前幀的重投影變換，為閉環(huán)候選幀的逆重投影變換；Scr為兩幀之間的相似變換。

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

本節(jié)設(shè)計(jì)了多組實(shí)驗(yàn)，展示RDSOL 系統(tǒng)的閉環(huán)檢測精度、光度增強(qiáng)的速度優(yōu)勢及低紋理環(huán)境下Retinex 光度增強(qiáng)對特征提取的積極效果。數(shù)據(jù)集及實(shí)際場景實(shí)驗(yàn)均在PC機(jī)上采用Ubuntu16.04的環(huán)境運(yùn)行，平臺參數(shù)如表1 所示。

表1 運(yùn)行平臺Tab.1 Operating platform

3.1 TUM 數(shù)據(jù)集實(shí)驗(yàn)

開源單目數(shù)據(jù)集TUM-mono 包含50 個(gè)光度校準(zhǔn)序列，提供光度標(biāo)定圖和相機(jī)在閉環(huán)位置處的一段軌跡（無完整的實(shí)際地面軌跡），非常適合通過對累積漂移的估算，比較不同VSLAM 系統(tǒng)的準(zhǔn)確性及魯棒性。

選取TUM-mono 中50 個(gè)序列的軌跡對齊誤差ealign表征 VSLAM 系統(tǒng)的累積漂移，分別考察ORB-SLAM 關(guān)/開閉環(huán)檢測線程及DSO/RDSOL 的ealign取值（每1 個(gè)序列運(yùn)行十次），結(jié)果如圖4、圖5所示。

圖4 中X 軸為數(shù)據(jù)集序列號，圖中每一個(gè)色塊代表VO/SLAM 對齊軌跡間的絕對軌跡誤差A(yù)TE（作為ealign指標(biāo)），VO代表ORB-SLAM關(guān)閉閉環(huán)檢測線程。可以看出，VO 在整體序列上的 ATE 要比下部ORB-SLAM 大的多，表明添加閉環(huán)檢測線程的VSLAM 能夠提供更高精度的定位支持。

圖4 ORB-SLAM 有/無回環(huán)下對齊誤差對比Fig.4 ORB-SLAM alignment error comparison with/without loop

圖5 給出了相同序列上DSO 及RDSOL 的測試結(jié)果對比，進(jìn)一步驗(yàn)證了添加閉環(huán)后的RDSOL 可有效消除VSLAM 累積誤差，有助于更好的跟蹤及地圖創(chuàng)建效果。

圖5 DSO 與RDSOL 對齊誤差對比Fig.5 Alignment error comparison between DSO and RDSOL

選取TUM-mono數(shù)據(jù)集的Sequence20序列，分別測試DSO/RDSOL（無/有閉環(huán)檢測線程）對實(shí)際室內(nèi)場景中機(jī)器人位姿估計(jì)的影響，三維重建效果分別如圖6(a)、圖6(b)所示。

圖6 稠密三維重建效果Fig.6 Dense 3D reconstruction results

可以看出，兩系統(tǒng)的三維重建結(jié)果相類似，但DSO給出的閉環(huán)處（圖6(a)左下起點(diǎn)與終點(diǎn)處）存在較大偏差（即重影現(xiàn)象），表明起點(diǎn)與終點(diǎn)軌跡無法擬合，該累積誤差的產(chǎn)生將導(dǎo)致位姿漂移。而如圖6(b)左下部所示，軌跡在起點(diǎn)和終點(diǎn)處很好地?cái)M合在了一起，表明RDSOL 通過閉環(huán)檢測有效地消除了軌跡累積誤差，能夠?yàn)闄C(jī)器人定位與三維地圖重建提供更精準(zhǔn)的位姿約束，魯棒性好。

在計(jì)算速度方面，選取TUM數(shù)據(jù)集中具有不同圖像幀數(shù)的前10個(gè)序列Sequence01～Sequence 10，比較DSO與RDSOL的平均每幀耗時(shí)，結(jié)果如表2所示。由于添加了閉環(huán)檢測線程，RDSOL需對圖像關(guān)鍵幀進(jìn)行特征檢測并對后端進(jìn)行全局地圖維護(hù)，增加了每幀耗時(shí)，故在圖像數(shù)目較多、地圖場景更為復(fù)雜的Sequence 04及Sequence05中，平均每幀耗時(shí)高于DSO。但由于省去DSO計(jì)算相機(jī)光度參數(shù)的耗時(shí)，RDSOL 在幀數(shù)較少的情形中體現(xiàn)了更優(yōu)的執(zhí)行效率，前10個(gè)序列下對DSO的平均運(yùn)行加速比達(dá)到了10.91%，實(shí)時(shí)性有保證。

表2 DSO與RDSOL平均每幀耗時(shí)對比Tab.2 Averagetime consumption/ framecomparison between DSOandRDSOL

3.2 KITTI數(shù)據(jù)集實(shí)驗(yàn)

TUM-mono數(shù)據(jù)集僅有一部分真實(shí)軌跡，無法對機(jī)器人實(shí)際運(yùn)行的所有軌跡進(jìn)行對比。為此，選取KITTI數(shù)據(jù)集中的Sequence07序列（兼顧大尺度場景、多種程度光照變化及軌跡閉環(huán)），繪制軌跡跟蹤效果圖，并做出定量分析。

圖7 為DSO 與RDSOL在KITTI 圖像序列上運(yùn)行軌跡及真實(shí)軌跡（groundtruth 提供）情況。從圖中可以看出，DSO呈現(xiàn)較大的軌跡偏移誤差，產(chǎn)生此現(xiàn)象的原因是相機(jī)運(yùn)動(dòng)通過同一位置后并沒有將初始環(huán)境閉環(huán)，導(dǎo)致位姿漂移；而RDSOL 在初始位置處修正了位姿，故可以很好地恢復(fù)真實(shí)軌跡（圖8的x、y、z軸軌跡位置均更接近真實(shí)場景）。

圖7 DSO/RDSOL/真實(shí)軌跡對比Fig.7 The trajectory comparison of DSO/RDSOL/groundtruth

圖8 DSO/RDSOL/真實(shí)軌跡位置對比Fig.8 The trajectory position comparison of DSO/RDSOL/groundtruth

如圖9 所示，在歐拉角變化方面，盡管在初始0-10 s 階段，RDSOL 中翻滾角和偏航角出現(xiàn)了波動(dòng)，但之后的追蹤過程RDSOL 都貼合了真實(shí)的歐拉角變化，追蹤精度優(yōu)于DSO。

圖9 DSO/RDSOL/真實(shí)角度對比Fig.9 The Euler angle comparison of DSO/RDSOL/groundtruth

進(jìn)一步對DSO 及RDSOL 在KITTI 上的表現(xiàn)做出量化分析。選取KITTI 上具有挑戰(zhàn)的10 個(gè)圖像序列Sequence 00～Sequence 09，給出DSO/RDSOL 在其上APE 的均方根誤差（RMSE）值，同時(shí)給出當(dāng)前廣泛應(yīng)用的ORB-SLAM2 的APE RMSE 以作比較，如圖10 所示?？梢钥闯?，RDSOL 與ORB-SLAM2 的表現(xiàn)相當(dāng)，且在特征缺失的Sequence 01 與Sequence 06 序列中表現(xiàn)出更好的跟蹤精度，RDSOL 將平均APE RMSE 由60.22 m（DSO）大幅降為13.97 m，表明其在取得較好實(shí)時(shí)性的同時(shí)，準(zhǔn)確性有保證。

圖10 DSO/RDSOL/ORB-SLAM2 的APE 比較Fig.10 The APE comparison of DSO/RDSOL/ORB-SLAM2

3.3 實(shí)際場景測試

不同于數(shù)據(jù)集的固定場景及有限數(shù)據(jù)參考，實(shí)際場景測試可圍繞RDSOL 的兩個(gè)核心設(shè)計(jì)——單目Retinex 圖像增強(qiáng)、閉環(huán)檢測來開展。

選取ZED 系列雙目相機(jī)（單目工作模式）采集空曠的走廊場景（明顯弱紋理區(qū)且光照弱），圖像成像大小均為752×480。圖11 給出了對走廊3 個(gè)不同場景分別進(jìn)行原始圖像特征提取及單目Retinex 圖像增強(qiáng)后特征提取的對比圖，可以看出，后者可使機(jī)器人提取到更多的ORB 特征點(diǎn)，使RDSOL 追蹤像素點(diǎn)的穩(wěn)定性更高，該圖像預(yù)處理設(shè)計(jì)達(dá)到了稀疏直接法的魯棒性要求。

圖11 不同光照下的特征點(diǎn)匹配結(jié)果Fig.11 Feature point matching results under different lighting conditions

三維重建的閉環(huán)性測試需要機(jī)器人的運(yùn)動(dòng)軌跡構(gòu)成一個(gè)閉環(huán)場景，故令機(jī)器人從實(shí)驗(yàn)室前門出發(fā)，經(jīng)過走廊繞行一周之后，再從實(shí)驗(yàn)室后門到達(dá)初始位置，實(shí)驗(yàn)場景如圖12 所示。

圖12 實(shí)驗(yàn)室以及走廊場景Fig.12 The scene graphs of lab and corridor

RDSOL在實(shí)際場景測試中無相機(jī)真實(shí)位姿參考，可通過其創(chuàng)建的三維稠密點(diǎn)云地圖對軌跡閉環(huán)特性進(jìn)行驗(yàn)證?？梢钥闯?，RDSOL 部分還原了上述實(shí)驗(yàn)室及走廊場景，如圖13所示。

圖13RDSOL的實(shí)際場景三維重建效果Fig.13Thereal scene3D reconstruction result by RDSOL

機(jī)器人行走一周之后回到下圖中的前門初始位置（見圖13左上部，給出相機(jī)去畸變及Retinex 預(yù)處理后第2022幀，即最后一幀的灰度圖），軌跡構(gòu)成閉合。

4 結(jié)論

針對室內(nèi)弱光照、光照不均場景下機(jī)器人VSLAM三維重建問題，本文開發(fā)了一類基于稀疏直接法的RDSOL 系統(tǒng)，利用單尺度Retinex 圖像增強(qiáng)代替DSO光度標(biāo)定，確保RDSOL 追蹤圖像像素點(diǎn)的穩(wěn)定性；構(gòu)建詞袋數(shù)據(jù)庫BoW，通過添加閉環(huán)檢測線程，消除位姿漂移累積誤差。開源數(shù)據(jù)集TUM-mono、KITTI 的實(shí)驗(yàn)結(jié)果表明，RDSOL 能夠較好地解決DSO因缺少閉環(huán)檢測造成的跟蹤失敗，并縮短三維重建的平均耗時(shí)。弱紋理、弱光照的走廊實(shí)景測試則進(jìn)一步驗(yàn)證了Retinex 圖像增強(qiáng)策略可改善像素灰度，避免ORB特征缺失，更利于直接法三維稠密點(diǎn)云地圖的構(gòu)建。所開發(fā)的RDSOL為單目VSLAM室內(nèi)實(shí)時(shí)三維重建提供了一定的參考及技術(shù)支持。