最小化光度誤差先驗(yàn)的視覺(jué)SLAM算法

韓健英，王 浩，方寶富

(合肥工業(yè)大學(xué) 計(jì)算機(jī)與信息學(xué)院，合肥 230009)

1 引 言

近年來(lái)，自動(dòng)駕駛、機(jī)器人、無(wú)人機(jī)、三維重建和AR/VR等技術(shù)得到飛速發(fā)展，SLAM(Simultaneous Localization and Mapping)也成為了研究的熱點(diǎn)問(wèn)題.因?yàn)樵谖粗h(huán)境中，機(jī)器人準(zhǔn)確定位需要依靠精確的環(huán)境地圖；而想要構(gòu)建精確地圖，機(jī)器人也要知道自己在環(huán)境中的準(zhǔn)確位置，SLAM可以構(gòu)建一個(gè)完整的、全局一致的環(huán)境地圖，以便為后期的導(dǎo)航、避障、運(yùn)動(dòng)規(guī)劃等應(yīng)用提供必要的支持[1-3].

如今，相比基于激光雷達(dá)實(shí)現(xiàn)的激光SLAM[4,5]，視覺(jué)傳感器(單目、雙目、RGB-D相機(jī))不僅更便宜，還可以提供更豐富的環(huán)境信息.其中，RGB-D相機(jī)可以同時(shí)提供RGB圖像和對(duì)應(yīng)的深度圖，節(jié)省大量的計(jì)算深度的運(yùn)算開(kāi)銷(xiāo).因此，在室內(nèi)的定位建圖中，用RGB-D相機(jī)實(shí)現(xiàn)視覺(jué)SLAM越來(lái)越受歡迎.

視覺(jué)里程計(jì)(Visual Odometry，VO)作為視覺(jué)SLAM的前端[6]，根據(jù)相機(jī)采集到的圖像信息，利用對(duì)極幾何[7]、ICP(Iterative Closest Point)[8]或PnP(Perspective-n-Point)[9]等算法計(jì)算出相機(jī)的位姿，為后端優(yōu)化提供了初始值.

Davison[10]提出了一個(gè)實(shí)時(shí)的視覺(jué)SLAM系統(tǒng)(MonoSLAM)，該系統(tǒng)使用的是單目相機(jī)，在前端跟蹤稀疏特征點(diǎn)以加快系統(tǒng)運(yùn)行速度，在后端使用擴(kuò)展卡爾曼濾波(EKF)，把地圖中所有的路標(biāo)點(diǎn)和單目相機(jī)的當(dāng)前位姿作為狀態(tài)量，以此來(lái)維護(hù)和更新?tīng)顟B(tài)量的均值和協(xié)方差矩陣.但MonoSLAM缺少回環(huán)檢測(cè)[11]的能力，地圖中路標(biāo)點(diǎn)的數(shù)量不能過(guò)多，在跟蹤特征點(diǎn)時(shí)也很容易丟失，一般應(yīng)用在比較小的場(chǎng)景.

而自從RGB-D傳感器出現(xiàn)以來(lái)，基于RGB-D相機(jī)構(gòu)建視覺(jué)SLAM系統(tǒng)也獲得越來(lái)越多研究人員的關(guān)注.最早的RGB-D SLAM系統(tǒng)是由Newcombe等人[12]提出的KinectFusion，使用ICP算法計(jì)算最優(yōu)的相機(jī)位姿，但其同樣不能檢測(cè)回環(huán)，容易產(chǎn)生較大誤差，只有在小范圍的空間內(nèi)才能保證精度.由Whelan等人[13]提出的Kintinuous是對(duì) KinectFusion 的改進(jìn)，借助于一個(gè)循環(huán)緩沖器并使用重定位、位姿圖優(yōu)化等方法達(dá)到實(shí)現(xiàn)回環(huán)檢測(cè)的目的，可以在一個(gè)較大的未知環(huán)境中構(gòu)建地圖.而Endres等人[14]提出一個(gè)開(kāi)源的RGB-D SLAM系統(tǒng)，該算法使用SURF[15]特征算法來(lái)檢測(cè)特征點(diǎn)并計(jì)算特征描述子，使用改進(jìn)的ICP算法來(lái)計(jì)算相機(jī)的位姿.

當(dāng)采用直接法估計(jì)相機(jī)位姿時(shí)，需要根據(jù)兩幀圖片之間的像素灰度值差異來(lái)約束計(jì)算.隨著直接法的發(fā)展，其精度得到了極大的提高.Newcombe等人[16]提出了DTAM，這是一個(gè)單目的視覺(jué)SLAM，可以實(shí)時(shí)建立場(chǎng)景的三維模型.它使用了逆深度[17]參數(shù)，在數(shù)值穩(wěn)定性上效果更好，可以做到穩(wěn)定跟蹤.但是DTAM建立了稠密的地圖，并采用了全局位姿優(yōu)化，因此其運(yùn)算量很大，模型擴(kuò)展性能被抑制.Forster等人[18]提出的SVO采用直接法來(lái)跟蹤稀疏特征點(diǎn)，由于不需要計(jì)算描述子，運(yùn)行速度非常快，但SVO是一種視覺(jué)里程計(jì)，沒(méi)有回環(huán)檢測(cè)，很難消除累計(jì)誤差，建立地圖的一致性也難以保證，其主要搭載俯視相機(jī)，應(yīng)用在無(wú)人機(jī)等領(lǐng)域.Engel等人[19]提出的LSD-SLAM使用單目相機(jī)構(gòu)建了一個(gè)基于直接法的SLAM系統(tǒng)，可以在普通的CPU上進(jìn)行運(yùn)算，但當(dāng)相機(jī)快速移動(dòng)時(shí)，LSD-SLAM容易跟蹤失敗.

Klein等人[20]提出了基于關(guān)鍵幀的PTAM，該算法用多線(xiàn)程方法實(shí)現(xiàn)了并行跟蹤和建圖的過(guò)程，并在后端使用了非線(xiàn)性?xún)?yōu)化來(lái)優(yōu)化位姿.PTAM滿(mǎn)足了視覺(jué)SLAM的實(shí)時(shí)性要求，但跟蹤時(shí)容易丟失.而Artal和Tardos等人[21]提出ORB-SLAM算法，采用跟蹤線(xiàn)程、局部建圖線(xiàn)程和回環(huán)檢測(cè)線(xiàn)程并行計(jì)算和優(yōu)化位姿的方法.并且，他們又在ORB-SLAM基礎(chǔ)上，提出了ORB-SLAM2[22]算法.ORB-SLAM和ORB-SLAM2都采用了多線(xiàn)程的方案，滿(mǎn)足了視覺(jué)SLAM實(shí)時(shí)性要求.該系統(tǒng)使用ORB特征[23]算法，提取圖像的特征和描述子，無(wú)論是前端、后端或回環(huán)檢測(cè)，都是采用ORB特征進(jìn)行匹配計(jì)算，使用PnP算法求解相機(jī)位姿.ORB-SLAM2還采取多種優(yōu)化策略，使其有很高精度.但ORB-SLAM2使用的勻速模型和參考幀模型，存在精度不足的問(wèn)題，在劇烈抖動(dòng)的時(shí)候，也很容易跟蹤失敗.

本文是在ORB-SLAM2的基礎(chǔ)上開(kāi)展進(jìn)一步的研究，提出了一種定位更加準(zhǔn)確并且魯棒性能更高的視覺(jué)SLAM算法：將最小化光度誤差和ORB-SLAM2所采用的最小化重投影誤差相結(jié)合，設(shè)計(jì)一個(gè)組合優(yōu)化的視覺(jué)SLAM框架：在前端的跟蹤線(xiàn)程中，為了提高系統(tǒng)魯棒性能，首先使用雙線(xiàn)性插值法得到特征點(diǎn)的灰度值，根據(jù)兩幀之間的光度誤差來(lái)得到相機(jī)初步的位姿.但是，因?yàn)楣庹兆兓⑵毓獾仍颍烙?jì)的位姿可能并不準(zhǔn)確，為了進(jìn)一步提高當(dāng)前幀位姿的精確度，通過(guò)最小化重投影誤差對(duì)當(dāng)前幀位姿再次優(yōu)化.并且在關(guān)鍵幀選擇機(jī)制上，提出一種新的關(guān)鍵幀選擇方法：基于當(dāng)前幀與上一關(guān)鍵幀的位姿變化，度量?jī)蓭g的距離.當(dāng)其超過(guò)設(shè)定的閾值時(shí)，說(shuō)明兩幀之間的場(chǎng)景變化較大，可以將當(dāng)前幀設(shè)置為關(guān)鍵幀，參與到SLAM算法的優(yōu)化與建圖.

2 算法流程與框架

本文在ORB-SLAM2的基礎(chǔ)上展開(kāi)進(jìn)一步研究，其中在ORB-SLAM2的跟蹤線(xiàn)程中，每次在估計(jì)當(dāng)前幀的初始位姿時(shí)，采用勻速模型或者參考幀模型.與真實(shí)值相比較，一般誤差較大，需要對(duì)這個(gè)初始值進(jìn)行多次迭代優(yōu)化.

為了改進(jìn)上述缺點(diǎn)，提高相機(jī)的定位精度，本文提出在視覺(jué)SLAM算法前端中，采用雙線(xiàn)性插值的方法得到特征點(diǎn)的灰度值，提高系統(tǒng)的魯棒性，根據(jù)最小化光度誤差計(jì)算出當(dāng)前幀與參考關(guān)鍵幀之間的位姿變化.因?yàn)榛诠舛日`差計(jì)算位姿的速度較快，所以并不會(huì)影響視覺(jué)SLAM算法的實(shí)時(shí)性.但當(dāng)環(huán)境的光照變化比較劇烈時(shí)，灰度不變假設(shè)不能被滿(mǎn)足.此時(shí)通過(guò)最小化光度誤差得到的位姿計(jì)算兩幀圖片的內(nèi)點(diǎn)數(shù)量小于閾值，說(shuō)明基于光度誤差計(jì)算得到的位姿精度不足.

為了進(jìn)一步提高當(dāng)前幀位姿的精度，本文以最小化光度誤差得到的估計(jì)位姿為初始值，通過(guò)最小化重投影誤差對(duì)其再次優(yōu)化.兩次迭代優(yōu)化來(lái)降低相機(jī)定位的誤差，保證計(jì)算得到的相機(jī)位姿足夠精確，進(jìn)而求得當(dāng)前幀在世界坐標(biāo)系中的準(zhǔn)確位姿，建立統(tǒng)一的坐標(biāo)系，可以得到較高精度的環(huán)境地圖.本文改進(jìn)后的視覺(jué)SLAM算法前端跟蹤線(xiàn)程的流程圖如圖1所示.

圖1 最小化光度誤差先驗(yàn)的視覺(jué)SLAM算法跟蹤線(xiàn)程Fig.1 Tracking threads of visual SLAM algorithm for minimizing photometric error prior

由圖1可以看出，當(dāng)視覺(jué)SLAM系統(tǒng)從傳感器中獲得RGB圖像和深度圖后，首先要提取圖像的ORB特征，并根據(jù)深度圖計(jì)算出相應(yīng)特征點(diǎn)的空間坐標(biāo)并構(gòu)建出圖像幀，完成初始化.根據(jù)提取的ORB特征點(diǎn)，其灰度值要通過(guò)雙線(xiàn)性插值的方法得到，增加魯棒性.通過(guò)最小化光度誤差和最小化重投影誤差來(lái)計(jì)算并優(yōu)化當(dāng)前幀的位姿.

在關(guān)鍵幀的選擇上，根據(jù)當(dāng)前幀與上一關(guān)鍵幀的位姿變化，計(jì)算兩幀之間的運(yùn)動(dòng)距離.當(dāng)距離大于設(shè)定閾值時(shí)，就把當(dāng)前幀選為關(guān)鍵幀并加入到關(guān)鍵幀序列，參與到局部建圖和后端優(yōu)化過(guò)程.

2.1 基于光度誤差的位姿估計(jì)

在本文提出的優(yōu)化算法中，首先根據(jù)最小化光度誤差計(jì)算當(dāng)前幀的初始位姿.當(dāng)一個(gè)新的圖像幀進(jìn)入系統(tǒng)時(shí)，通過(guò)當(dāng)前幀與參考關(guān)鍵幀之間的特征點(diǎn)灰度值差異計(jì)算出兩者之間的位姿變換，即兩幀之間的旋轉(zhuǎn)矩陣R和位移向量t.為了方便計(jì)算，根據(jù)文獻(xiàn)[24,25]，相機(jī)位姿可以用李代數(shù)ξ表示，方便求導(dǎo)與優(yōu)化.

圖2 空間點(diǎn)的投影示意圖Fig.2 Projection of space points

如圖2所示，對(duì)于一個(gè)空間點(diǎn)Pc(Xc,Yc,Zc)，其在兩幀圖像I1和I2上的投影為p1和p2，根據(jù)針孔相機(jī)模型：

(1)

其中puv是空間點(diǎn)Pc在像素坐標(biāo)系下的坐標(biāo)；π(·)為變換函數(shù)，將相機(jī)坐標(biāo)系下空間點(diǎn)Pc轉(zhuǎn)為相應(yīng)的像素坐標(biāo)；fx、fy、cx、cy為相機(jī)的內(nèi)參.

由于兩幀圖像之間并沒(méi)有執(zhí)行特征匹配算法，所以并不知道哪個(gè)p2與p1對(duì)應(yīng)著同一個(gè)空間點(diǎn).但顯然的是，若當(dāng)前幀的估計(jì)位姿與真實(shí)值有較大差異時(shí)，p2與p1的灰度值就會(huì)有明顯的差異.當(dāng)有N個(gè)點(diǎn)對(duì)時(shí)，總的差異越大，說(shuō)明估計(jì)的位姿越不準(zhǔn)確，就要對(duì)估計(jì)位姿進(jìn)行調(diào)整優(yōu)化.每一次優(yōu)化的方向，都要使總的特征點(diǎn)之間灰度值差異不斷減小.定義光度誤差為：

e=I1(p1)-I2(p2)

(2)

其中I(p)為像素p在圖像I中的灰度，優(yōu)化目標(biāo)為該誤差的二范數(shù).當(dāng)系統(tǒng)在環(huán)境中檢測(cè)到N個(gè)空間點(diǎn)Pi(i=1,2…N)時(shí)，整個(gè)優(yōu)化問(wèn)題就會(huì)變?yōu)椋?/p>

(3)

這里優(yōu)化變量為相機(jī)位姿ξ，為了得到最佳的ξ，降低誤差，就需要計(jì)算出每次優(yōu)化的方向，即誤差e關(guān)于ξ的導(dǎo)數(shù),根據(jù)李代數(shù)擾動(dòng)模型：

e(ξ⊕δξ)=I1(π(Pi))-I2(π(exp(δξ∧)exp(ξ∧)Pi))

(4)

就可得到誤差e關(guān)于相機(jī)位姿ξ的雅可比矩陣J，其值可以表示為：

(5)

(6)

(7)

由此就可以根據(jù)計(jì)算出的雅可比矩陣J，得出每一步迭代優(yōu)化的方向.使用g2o[26]中的LM(列文伯格-馬夸爾特)算法計(jì)算增量，對(duì)位姿進(jìn)行迭代優(yōu)化，計(jì)算出最佳的當(dāng)前幀位姿ξ，使總的光度誤差最小.

因?yàn)閮蓭瑘D像之間有很多灰度值相同的像素點(diǎn)，容易誤匹配.為了提升算法的魯棒性能，要考慮到每個(gè)特征點(diǎn)的相鄰像素灰度值.本文在計(jì)算像素坐標(biāo)為(i,j)的特征點(diǎn)的灰度值I(i,j)時(shí)，沒(méi)有直接選擇(i,j)處的灰度值，而是取該特征點(diǎn)周?chē)囊恍┫袼兀ㄟ^(guò)雙線(xiàn)性插值的方法得到特征點(diǎn)的灰度值I(i,j)：

I(i,j)=α1×I(i,j)+α2×I(i+1,j)+α3×
I(i,j+1)+α4×I(i+1,j+1)

(8)

可以看出，本文在灰度值的取值上，考慮到了它周邊的一些像素灰度值.這樣，在計(jì)算兩幀之間一對(duì)特征點(diǎn)的灰度誤差時(shí)，把該特征點(diǎn)的周?chē)乜紤]進(jìn)去，可以增強(qiáng)系統(tǒng)的魯棒性能.

給定一個(gè)當(dāng)前幀的位姿ξ，當(dāng)絕大部分的特征點(diǎn)對(duì)都是正確匹配，說(shuō)明此時(shí)位姿已經(jīng)足夠精確.但是此時(shí)如果還有某幾個(gè)誤差項(xiàng)的數(shù)據(jù)是錯(cuò)誤的，并且這幾個(gè)特征點(diǎn)對(duì)的誤差很大.優(yōu)化算法為了降低誤差，仍會(huì)持續(xù)調(diào)整ξ的值，使這幾個(gè)點(diǎn)對(duì)的誤差減小.然而為了優(yōu)化這個(gè)錯(cuò)誤的特征點(diǎn)對(duì)，其他正確的特征點(diǎn)對(duì)會(huì)受到影響，反而降低位姿的精度.為了減少外點(diǎn)對(duì)優(yōu)化問(wèn)題的影響，可以使用一個(gè)魯棒核函數(shù)ρ(·)，增強(qiáng)本文算法的魯棒性能：

(9)

其中，

(10)

可以看到，當(dāng)每一個(gè)特征點(diǎn)對(duì)的灰度值誤差e的絕對(duì)值大于閾值δ后，函數(shù)增長(zhǎng)由平方形式變成了線(xiàn)性的.這樣做避免了當(dāng)某個(gè)錯(cuò)誤的特征點(diǎn)對(duì)的灰度值誤差很大，在對(duì)位姿進(jìn)行優(yōu)化時(shí)，對(duì)其他正確點(diǎn)對(duì)的影響過(guò)大，導(dǎo)致優(yōu)化效果不好，同時(shí)其也保留了可導(dǎo)的性質(zhì).

因?yàn)榛诠舛日`差計(jì)算位姿是快速的，所以并不會(huì)影響視覺(jué)SLAM算法的實(shí)時(shí)性.但是由此得出當(dāng)前幀的位姿，會(huì)受到環(huán)境因素干擾.在這種情況下，可能會(huì)導(dǎo)致位姿準(zhǔn)確性不佳，此時(shí)兩幀之間的內(nèi)點(diǎn)數(shù)目較少.為了進(jìn)一步提高當(dāng)前幀位姿的準(zhǔn)確性，以最小化光度誤差優(yōu)化得到的位姿為初始值，本文算法將再基于重投影誤差對(duì)當(dāng)前幀位姿進(jìn)一步優(yōu)化，提高定位精度.

2.2 基于重投影誤差的位姿估計(jì)

與最小化光度誤差相比，采用最小化重投影誤差，對(duì)環(huán)境中光照變化的敏感度較小.當(dāng)獲得來(lái)自最小化光度誤差計(jì)算的當(dāng)前幀位姿ξ時(shí)，可以把ξ作為最小化重投影誤差優(yōu)化的初值，對(duì)ξ進(jìn)行再次優(yōu)化.

圖3 視覺(jué)SLAM重投影誤差示意圖Fig.3 Reprojection error of visual SLAM

(11)

(12)

ρ(·)為魯棒核函數(shù).

針對(duì)該函數(shù)求解，初始值采用基于光度誤差估計(jì)的當(dāng)前幀位姿，利用非線(xiàn)性?xún)?yōu)化對(duì)當(dāng)前幀位姿迭代優(yōu)化，就能計(jì)算出一個(gè)比較精確的相機(jī)位姿ξ，使N對(duì)特征點(diǎn)的重投影誤差最小.

通過(guò)兩次優(yōu)化的ξ可以計(jì)算出當(dāng)前幀在世界坐標(biāo)系中的位姿Tcw，用于將當(dāng)前幀提取到的空間點(diǎn)映射到世界坐標(biāo)系中，以此構(gòu)建一個(gè)統(tǒng)一的坐標(biāo)系：

Tcw=exp(ξ∧)·Trw

(13)

其中Trw是參考關(guān)鍵幀所在的相機(jī)坐標(biāo)系與世界坐標(biāo)系的變換矩陣.得到Tcw之后，當(dāng)前幀坐標(biāo)系就可以跟世界坐標(biāo)系建立聯(lián)系，建立統(tǒng)一的全局地圖.

可以看出，本文通過(guò)兩次位姿優(yōu)化：基于光度誤差先求出初始的當(dāng)前幀位姿；采用最小化重投影誤差對(duì)求出的初始位姿再次優(yōu)化，提高了定位的精度.得到當(dāng)前幀的精確位姿之后，可以根據(jù)關(guān)鍵幀選擇機(jī)制來(lái)判斷當(dāng)前幀是否可以作為一個(gè)關(guān)鍵幀插入到關(guān)鍵幀序列.

2.3 關(guān)鍵幀選擇機(jī)制

在視覺(jué)SLAM對(duì)圖像幀的處理過(guò)程中，關(guān)鍵幀的選擇是很重要的一部分，關(guān)鍵幀選擇太密集，雖然有利于提高位姿估計(jì)的準(zhǔn)確度和建圖精度，但是會(huì)導(dǎo)致后端優(yōu)化處理花費(fèi)更多時(shí)間，導(dǎo)致視覺(jué)SLAM系統(tǒng)不能實(shí)時(shí)處理不斷生成的關(guān)鍵幀；但是關(guān)鍵幀太少則會(huì)使位姿估計(jì)的準(zhǔn)確度下降，不能建立一個(gè)精確的環(huán)境地圖.

本文提出了一個(gè)新的關(guān)鍵幀生成機(jī)制：每當(dāng)RGB-D相機(jī)采集到一幀圖像時(shí)，都將該幀圖像與其上一關(guān)鍵幀進(jìn)行比較，計(jì)算其運(yùn)動(dòng)幅度，若運(yùn)動(dòng)幅度超過(guò)閾值，則將該幀存儲(chǔ)為關(guān)鍵幀.具體來(lái)說(shuō)就是將當(dāng)前幀位姿ξ，轉(zhuǎn)化為對(duì)應(yīng)的旋轉(zhuǎn)矩陣R和位移偏量t(tx,ty,tz)，根據(jù)R和t來(lái)計(jì)算兩幀之間的運(yùn)動(dòng)幅度.但是采用旋轉(zhuǎn)矩陣作為旋轉(zhuǎn)的評(píng)價(jià)參數(shù)時(shí)，不能直觀的反應(yīng)旋轉(zhuǎn)的劇烈程度.本文采用將旋轉(zhuǎn)矩陣轉(zhuǎn)化為旋轉(zhuǎn)歐拉角的方法來(lái)表示旋轉(zhuǎn)的程度：

(14)

其中atan2()函數(shù)返回的是方位角，而R表示為：

θyaw,θpitch,θroll為歐拉角，分別表示偏航角、俯仰角和滾轉(zhuǎn)角.將旋轉(zhuǎn)矩陣轉(zhuǎn)化為歐拉角后，就可以根據(jù)歐拉角來(lái)衡量旋轉(zhuǎn)幅度的大小：

(15)

正如式(15)，計(jì)算的兩幀間運(yùn)動(dòng)幅度d，可通過(guò)計(jì)算歐拉角和位移偏量的二范數(shù)來(lái)作為運(yùn)動(dòng)幅度大小的度量值.分別賦予歐拉角和位移偏量的范數(shù)一個(gè)權(quán)值α和β，當(dāng)d的值超過(guò)閾值Threshold時(shí)，即可將當(dāng)前幀設(shè)為一個(gè)關(guān)鍵幀，加入到關(guān)鍵幀序列.當(dāng)相機(jī)在短時(shí)間內(nèi)運(yùn)動(dòng)時(shí)，若僅產(chǎn)生純平移運(yùn)動(dòng)，此時(shí)相機(jī)視野場(chǎng)景變化的范圍較小；若相機(jī)產(chǎn)生旋轉(zhuǎn)運(yùn)動(dòng)時(shí)，往往會(huì)對(duì)相機(jī)視野的變化影響巨大.因此在設(shè)置α和β的值時(shí)，β的值要遠(yuǎn)大于α，即歐拉角的權(quán)重要大于位移偏量的權(quán)重.

2.4 優(yōu)化算法

本文改進(jìn)了前端中的位姿估計(jì)方法和關(guān)鍵幀選擇機(jī)制，兩次對(duì)位姿的優(yōu)化可以使得其精度進(jìn)一步的提高，關(guān)鍵幀的選擇也更加合理.可以把這種視覺(jué)SLAM位姿優(yōu)化算法的偽代碼描述為：

算法.Visual SLAM Algorithm For Minimizing Photometric Error Prior

輸入：RGB and Depth image

輸出：Camera Pose

1.grayImg←Gray(RGB)

2.points←ExtractORB(grayImg)

3.CurFrame←InitializeFrame(grayImg,Depth)

4.whileTruedo

5.ifisTracking()then

6.foreachp∈pointsdo

7.I(p)=bilinearInterpolation(p)

8.endfor

9. MinPhotometricError(CurFrame,RefKeyFrame)

11. MinReprojectionError(CurFrame,RefKeyFrame)

13.endif

14.ifisRelocalization()then

15. Relocalization()

16.endif

17. TrackingLocalMap()

18.d←CalculatingDistance(CurFrame,LastKeyFrame)

19.ifd≥Thresholdthen

20. InsertKeyFrame(CurFrame)

21.endif

22.endwhile

改進(jìn)后的視覺(jué)SLAM算法在計(jì)算出當(dāng)前幀的位姿選擇出關(guān)鍵幀后，就可以進(jìn)入局部建圖線(xiàn)程，進(jìn)行關(guān)鍵幀的處理、地圖點(diǎn)的刪除與新建、局部?jī)?yōu)化和關(guān)鍵幀的刪除等操作，建立起局部地圖.再借助于ORB-SLAM2優(yōu)秀的后端優(yōu)化處理和回環(huán)檢測(cè)能力，可以進(jìn)一步降低定位誤差并提高建圖的精度.

3 實(shí)驗(yàn)評(píng)估

本文算法運(yùn)行的平臺(tái)配置為Intel 酷睿i5處理器，主頻為2.6GHz，8GB內(nèi)存，采用Ubuntu16.04操作系統(tǒng)，沒(méi)有使用GPU對(duì)該方法進(jìn)行加速計(jì)算，以驗(yàn)證本方法在普通CPU上的實(shí)時(shí)性能.

(16)

本文在實(shí)驗(yàn)中主要采用了TUM數(shù)據(jù)集中的fr1/360、fr1/desk、fr2/360、fr2/desk、fr3/long_office等序列進(jìn)行測(cè)試.這些序列在采集時(shí)包括不同的相機(jī)運(yùn)動(dòng)狀態(tài)，如快速移動(dòng)、緩慢移動(dòng)、強(qiáng)旋轉(zhuǎn)等.為了避免偶然的情況對(duì)實(shí)驗(yàn)結(jié)果造成影響，在實(shí)驗(yàn)中，對(duì)每個(gè)序列都進(jìn)行了多次的測(cè)量，并將絕對(duì)軌跡誤差的均方根誤差平均值用于對(duì)實(shí)驗(yàn)進(jìn)行評(píng)估，可以更好地評(píng)估系統(tǒng)性能.

3.1 關(guān)鍵幀選擇實(shí)驗(yàn)

在本文中提出的新的關(guān)鍵幀選擇機(jī)制，可以在不影響系統(tǒng)實(shí)時(shí)性能的同時(shí)，選擇更多關(guān)鍵幀，來(lái)為建圖與優(yōu)化提供更多可靠的數(shù)據(jù).圖4顯示的是改進(jìn)后的SLAM算法和ORB-SLAM2在部分序列下得到的關(guān)鍵幀數(shù)目的比較結(jié)果.

圖4 本文算法和ORB-SLAM2在TUM據(jù)集下的關(guān)鍵幀數(shù)Fig.4 Number of key frames between our algorithm and ORB-SLAM2 in TUM dataset

從圖4中可以看出，與ORB-SLAM2相比，本文提出的優(yōu)化算法在不同的序列中總是可以提供更多的關(guān)鍵幀，這樣，可以為局部或全局優(yōu)化、回環(huán)檢測(cè)等提供了更多的數(shù)據(jù)支持，以提高系統(tǒng)定位的準(zhǔn)確性.

3.2 算法定位精度評(píng)估

視覺(jué)SLAM系統(tǒng)的核心問(wèn)題在于準(zhǔn)確的相機(jī)定位和精確的環(huán)境建圖，并且建立的環(huán)境地圖精度高度依賴(lài)相機(jī)定位的準(zhǔn)確度，所以在本文中，主要評(píng)估相機(jī)定位的準(zhǔn)確度，并且驗(yàn)證優(yōu)化后的SLAM算法魯棒性能.表1顯示的是本文優(yōu)化后的SLAM算法與ORB-SLAM2的絕對(duì)軌跡誤差(ATE)的均方根誤差(RMSE)的比較結(jié)果.

表1 本文算法與ORB-SLAM2的RMSE值Table 1 RMSE values for our algorithm and ORB-SLAM2

根據(jù)表1的比較結(jié)果，可以看出，在大部分?jǐn)?shù)據(jù)集序列中，與ORB-SLAM2相比，經(jīng)過(guò)改進(jìn)的 SLAM算法可以提供更高的精度.尤其是在fr1/360、fr2/360和fr1/desk這三個(gè)序列中，絕對(duì)軌跡誤差的均方根誤差下降的非常明顯，分別下降了31.57%、16.92%和23.56%.但是在使用ORB-SLAM2對(duì)fr1/desk序列進(jìn)行多次測(cè)試時(shí)發(fā)現(xiàn)，經(jīng)常會(huì)發(fā)生跟蹤失敗，導(dǎo)致系統(tǒng)不能正常運(yùn)行的情況.所以表1中fr1/desk序列的19.83mm的誤差結(jié)果是在只計(jì)算跟蹤成功情況下得到的，經(jīng)過(guò)多次實(shí)驗(yàn)，比較在跟蹤成功條件下的均方根誤差.

圖5 ORB-SLAM2在fr1/360上的軌跡和誤差Fig.5 Trajectory and errors of ORB-SLAM2 on fr1/360

由圖5和圖6可以看出，在fr1/360序列中，ORB-SLAM2的軌跡誤差明顯大于本文優(yōu)化算法的軌跡誤差.尤其是在fr1/360序列中，優(yōu)化后的算法明顯降低了軌跡誤差.這主要是因?yàn)榕cORB-SLAM2相比，優(yōu)化后的算法不僅選擇了更加合理的關(guān)鍵幀，利用了圖像更多的信息.基于最小化光度誤差初步得到了當(dāng)前幀的相對(duì)精確的位姿，而不是簡(jiǎn)單的將當(dāng)前位姿定義為前一幀的位姿或參考幀的位姿.再經(jīng)過(guò)最小化重投影誤差的優(yōu)化，兩次優(yōu)化降低了定位的誤差，提高了精度.

圖6 本文算法在fr1/360上的軌跡和誤差Fig.6 Trajectory and errors of our algorithm on fr1/360

3.3 算法運(yùn)行時(shí)間評(píng)估

在本文的優(yōu)化算法中加入組合優(yōu)化策略和關(guān)鍵幀的選擇機(jī)制之后，不僅提高了位姿的精確度，而且這種做法并沒(méi)有影響其在普通CPU上的實(shí)時(shí)性.表2顯示的是優(yōu)化后的SLAM算法與ORB-SLAM2處理每幀所需時(shí)間的平均值比較結(jié)果.

表2 本文算法與ORB-SLAM2平均每幀運(yùn)行時(shí)間Table 2 Average operation time per frame of our algorithm and ORB-SLAM2

從表2可以看出，在大部分序列中，改進(jìn)后的算法平均每一幀處理所需時(shí)間較ORB-SLAM2僅僅只是多3ms左右，這是因?yàn)樵诟倪M(jìn)的算法中加入了更多的關(guān)鍵幀，并且實(shí)施的組合優(yōu)化策略在一定程度上需要些時(shí)間.但是在實(shí)際運(yùn)行過(guò)程中，并不會(huì)影響系統(tǒng)運(yùn)行實(shí)時(shí)性，卻可以提高相機(jī)位姿的精確度，降低系統(tǒng)定位誤差.

3.4 系統(tǒng)魯棒性能

在視覺(jué)SLAM應(yīng)用中，由于相機(jī)的快速移動(dòng)、劇烈抖動(dòng)等情況，會(huì)導(dǎo)致SLAM系統(tǒng)不能及時(shí)處理這種突發(fā)情況，導(dǎo)致前端跟蹤失敗，系統(tǒng)不能正常運(yùn)行.但是本文的算法在相機(jī)快速移動(dòng)時(shí)仍具有良好的魯棒性能，在解決兩幀圖像之間由于匹配失敗而導(dǎo)致SLAM系統(tǒng)前端跟蹤失敗方面，具有很好的定位精度.

在本文3.2中，介紹了在對(duì)fr1/desk序列進(jìn)行實(shí)驗(yàn)時(shí)發(fā)現(xiàn)，ORB-SLAM2很容易跟蹤失敗.這是因?yàn)樵谠撔蛄械?70幀到171幀運(yùn)行時(shí)存在著劇烈的抖動(dòng)，如圖7所示.兩幀間的大角度轉(zhuǎn)動(dòng)造成圖像模糊.若僅基于最小化重投影誤差計(jì)算，此時(shí)兩幀圖像間想要正確匹配特征點(diǎn)比較困難，所以視覺(jué)SLAM系統(tǒng)在前端容易進(jìn)入跟蹤失敗狀態(tài).

圖7 兩幀間的大角度轉(zhuǎn)動(dòng)Fig.7 Large angle rotation between two frames

在實(shí)驗(yàn)中，針對(duì)fr1/desk序列進(jìn)行多次測(cè)試，實(shí)驗(yàn)結(jié)果顯示，對(duì)比ORB-SLAM2和改進(jìn)后的SLAM算法，在運(yùn)行ORB-SLAM2時(shí)，有50%的概率在系統(tǒng)運(yùn)行到第171幀時(shí)前端跟蹤失敗.如圖8(a)所示，在框選的部分上，相機(jī)是沒(méi)有被跟蹤到的：在圖8(a)標(biāo)記為1的地方前端跟蹤失敗，導(dǎo)致ORB-SLAM2在接下來(lái)的運(yùn)行過(guò)程中，由于重定位一直失敗，在其中一段運(yùn)行過(guò)程中，不能計(jì)算出當(dāng)前幀的位姿和得出有效的環(huán)境地圖.直至ORB-SLAM2在圖8(a)標(biāo)記為2的地方重定位成功，系統(tǒng)才能重新成功運(yùn)行.而基于改進(jìn)后的算法進(jìn)行實(shí)驗(yàn)時(shí)，成功率從50%提高到了81.3%，在fr1/desk序列上的第170幀到171幀，前端基于光度誤差跟蹤成功，得到的相機(jī)運(yùn)動(dòng)軌跡和誤差如圖8(b)所示.可以看出，在圖8(b)中，改進(jìn)后的算法仍可以有效且正確的跟蹤，成功得到相機(jī)的位姿和軌跡.

圖8 ORB-SLAM2(a)和本文算法(b)在fr1/desk的估計(jì)軌跡Fig.8 Estimated trajectory of ORB-SLAM2 and our algorithm on fr1/desk

4 結(jié) 論

本文針對(duì)相機(jī)位姿估計(jì)和關(guān)鍵幀選擇方面做出了改進(jìn)和優(yōu)化，提高了相機(jī)位姿的準(zhǔn)確度和點(diǎn)云地圖的精度，魯棒性能也得到提高，同時(shí)實(shí)時(shí)性也可以得到保證.但是仍然存在不足之處.最小化光度誤差可以提高初始估計(jì)位姿的精度，但要求環(huán)境亮度變化幅度較小.這是因?yàn)槠淝疤崾腔叶炔蛔兗僭O(shè)，但在真實(shí)環(huán)境中的亮度可能會(huì)發(fā)生變換.并且當(dāng)相機(jī)處于低紋理的環(huán)境中，特征點(diǎn)較少，本文算法和ORB-SLAM2都不能保證定位的準(zhǔn)確性和建圖準(zhǔn)確性，甚至可能跟蹤不到相機(jī)的位置.因此在接下來(lái)的研究中，要考慮這些不足之處.可以探索在視覺(jué)SLAM中加入線(xiàn)特征，線(xiàn)特征有助于在點(diǎn)特征較少的情況下，使用線(xiàn)特征匹配，在前端進(jìn)行更精確的位姿估計(jì)，提高位姿精度和建圖的準(zhǔn)確性.