張超，曹雄，徐春鳳，韓成，白燁

（1.長春理工大學(xué) 計算機科學(xué)技術(shù)學(xué)院，長春 130022；2.長春理工大學(xué)光電信息學(xué)院，長春 130022）

SLAM（Simultaneous Localization and Mapping，同時定位與建圖）的主要功能是指搭載特定傳感器的主體，在沒有環(huán)境先驗信息的情況下，在運動過程中估計自己的位置和狀態(tài)信息，同時建立環(huán)境模型[1]。2007年，Andrew Davison首次提出了基于視覺的MonoSLAM[2]，但是MonoSLAM只適合小場景。Klein和Murray在MonoSLAM基礎(chǔ)上提出PTAM[3]，將視覺SLAM分為跟蹤和建圖線程，但PTAM具有跟蹤易丟失，適合小場景等缺點。2014年Jakob Engel等人提出基于直接法的LSD-SLAM方法[4]。LSD-SLAM使用直接法進(jìn)行半稠密地圖的構(gòu)建，并且算法的魯棒性也會增加不少，而缺點是計算量大，精度相對較低。2016年Mur-Artal提出了ORB-SLAM2[5]，這種方法具有優(yōu)秀的特征點和回環(huán)算法，但同時也存在著依賴特征點、計算量大等缺點。為此文獻(xiàn)[6]使用平方根濾波方法進(jìn)行SLAM的改進(jìn)。

為了提高視覺SLAM算法構(gòu)建地圖的精度，通過分析對比目前的直接法和特征點法SLAM算法中的優(yōu)缺點，提出了基于稀疏直接法的SLAM算法（簡稱SD-SLAM，sparse direct SLAM），最后通過實驗驗證了算法能夠有效提高相機軌跡的精度。

1 基于稀疏直接法的SLAM

SD-SLAM流程如圖1所示，在前端中使用改進(jìn)的Shi-Tomasi特征點檢測算法進(jìn)行稀疏特征點的提取，然后基于稀疏特征點采用直接法進(jìn)行相機位姿估計，同時利用構(gòu)建的地圖進(jìn)行位姿優(yōu)化，以提高相機軌跡精度。在后端中加入文獻(xiàn)[5]中的回環(huán)檢測模塊以保證算法的完整性。

圖1 算法流程圖

1.1 改進(jìn)的Shi-Tomasi特征提取算法

文獻(xiàn)[7，8]使用SURF和SIFT特征提取算法進(jìn)行目標(biāo)跟蹤，但SURF和SIFT特征提取算法并不能滿足SLAM的實時性。Shi-Tomasi角點檢測法[9]具有計算量大和不具備尺度不變性的缺點，而FAST角點檢測算法[10]僅僅通過檢測亮度差來確定角點，具有較快的速度。因此結(jié)合Shi-Tomasi和FAST角點檢測算法，并對融合后的算法進(jìn)行改進(jìn)。使用改進(jìn)后的算法提取特征點，然后基于稀疏特征點進(jìn)行后續(xù)的相機位姿估計。特征點提取的步驟如下：

（1）對圖像構(gòu)建高斯金字塔；

（2）在圖像金字塔每層上檢測FAST特征點作為候選特征點；

（3）對每個候選特征點進(jìn)行Shi-Tomasi角點檢測并排序，選取前N個作為特征點；

（4）使用灰度質(zhì)心法[11]為選擇出來的特征點添加方向信息。

1.2 稀疏直接法位姿估計

如圖2所示，在追蹤線程中，相機運動到當(dāng)前幀Im時，已知上一幀Im-1的位姿Tm-1、幀上提取的特征點u以及u對應(yīng)深度du。需要求得當(dāng)前幀Im和上一幀Im-1的位姿變換Tm,m-1。

圖2 幀間灰度誤差

因為單個特征點的灰度沒有區(qū)分性，所以使用特征塊（特征點的4*4領(lǐng)域）的灰度值。將Im-1幀上特征塊u根據(jù)其深度du逆投影（π為投影方程）到Im-1幀相機坐標(biāo)系中的三維空間點P，P經(jīng)過幀間位姿Tm,m-1變換到Im幀相機坐標(biāo)系中，再投影到Im幀上形成u′。

由于噪聲導(dǎo)致位姿估計存在誤差，所以u′與P的真實投影（圖2中虛線框所示）存在誤差e。

采用非線性優(yōu)化方法，將誤差e定義為誤差項，將相機位姿Tm,m-1定義為優(yōu)化變量，不斷調(diào)整位姿使誤差最小，構(gòu)建最小二乘問題求解位姿Tm,m-1。

公式（3）為幀與幀之間的相機位姿估計，但不可避免的累計誤差會導(dǎo)致相機軌跡精度下降，所以需要對相機位姿Tm,m-1進(jìn)行優(yōu)化。

文獻(xiàn)[12]中提出，當(dāng)前幀上的部分特征點對應(yīng)的空間點可以被局部地圖中的某些關(guān)鍵幀觀測到，利用這個數(shù)據(jù)關(guān)聯(lián)可以優(yōu)化當(dāng)前幀特征u′i的位置。如公式（4）所示，其中Ai為仿射變換函數(shù)[13]，Im為當(dāng)前幀，In為ui所在關(guān)鍵幀。

由于關(guān)鍵幀和當(dāng)前幀的環(huán)境光源和相機曝光可能發(fā)生變化，造成P點投影在幀Im和In中的點的灰度發(fā)生變化，不符合直接法的灰度不變性假設(shè)。所以對公式（4）進(jìn)行改進(jìn)以修正環(huán)境造成的偏差，得到精確的特征位置u′i。

其中，b為灰度補償參數(shù)（下標(biāo)m，n分別代表當(dāng)前幀和關(guān)鍵幀）。在求得優(yōu)化后的特征位置u′i后，以當(dāng)前幀相對世界坐標(biāo)系的位姿Tm,w為優(yōu)化變量，將世界坐標(biāo)系下空間點Pi的投影誤差作為誤差項，通過最小化誤差項，優(yōu)化相機位姿Tm,w。

幀序列相對于世界坐標(biāo)系的相機位姿Tm,w構(gòu)成相機軌跡。因此在前端中使用稀疏特征點和直接法進(jìn)行相機位姿估計，同時使用構(gòu)建的地圖優(yōu)化位姿，從而得到精度更高的相機位姿估計。

1.3 后端

前端的作用為追蹤相機運動，后端需要對數(shù)據(jù)進(jìn)行優(yōu)化。首先在圖像幀序列中生成關(guān)鍵幀，關(guān)鍵幀的選取關(guān)系到算法的運行效率。算法中關(guān)鍵幀的判斷標(biāo)準(zhǔn)為：

（1）與上一關(guān)鍵幀間隔最少20幀；

（2）跟蹤到的地圖點的數(shù)量不少于50；

（3）與上一關(guān)鍵幀觀測到地圖點的重合率小于10%。

同時滿足以上三個條件則創(chuàng)建關(guān)鍵幀，并對關(guān)鍵幀上的特征點進(jìn)行Brief描述子[13]的計算，以便進(jìn)行回環(huán)檢測。關(guān)鍵幀中存儲著幀位姿、特征點以及對應(yīng)的地圖點信息。

在局部優(yōu)化中刪除冗余的地圖點。如果觀測到該地圖點的關(guān)鍵幀數(shù)量小于3，則認(rèn)為該地圖點冗余并刪除該點。

最后添加了文獻(xiàn)[5]的回環(huán)檢測模塊，使算法擁有回環(huán)的能力，提高算法的魯棒性。

2 算法實現(xiàn)與結(jié)果分析

算法運行平臺采用Intel-E3處理器，NVIDIA Quadra k620顯卡，內(nèi)存8Gb，操作系統(tǒng)為Ubuntu14.04。

采用TUM標(biāo)準(zhǔn)數(shù)據(jù)集作為實驗使用的圖像序列。TUM數(shù)據(jù)集是一個用于估計攝像頭定位精度的優(yōu)秀數(shù)據(jù)庫，提供了許多圖像序列，包括對應(yīng)的軌跡真值。為了比較ORB-SLAM2和SD-SLAM算法的軌跡精度，使用TUM提供的檢測工具檢測軌跡的絕對誤差。表1記錄了ORB-SLAM2算法和SD-SLAM算法運行不同圖像序列的估計軌跡和真實軌跡的均方根誤差對比。圖3展示了SD-SLAM算法運行不同圖像序列的估計軌跡和真實軌跡的對比。

表1中可以看出在fr1/room、fr2/desk和fr2/360h圖像序列中，SD-SLAM算法的軌跡精度相對ORB-SLAM2算法有所提高。圖3展示了SD-SLAM算法運行4個圖像序列的相機估計軌跡在xy軸上與真實軌跡的對比圖，圖中實線為真實軌跡，虛線軌跡為估計軌跡。fr2/360h圖像序列相比其它圖像序列場景中的紋理較少，而SD-SLAM算法需要提取特征進(jìn)行位姿估計，所以SD-SLAM算法在運行fr2/360h圖像序列時誤差相對較大。

總體來說SD-SLAM能完整運行4個圖像序列，在部分圖像序列中相對ORB-SLAM2算法提高了軌跡精度。

表1 4個圖像序列下兩種算法的位姿軌跡均方根誤差（m）對比

圖3 SD-SLAM在4個圖像序列中的估計軌跡與真實軌跡對比

3 結(jié)論

通過在前端中使用改進(jìn)Shi-Tomasi特征點檢測算法進(jìn)行特征點提取，然后基于提取的稀疏特征點采用直接法進(jìn)行相機位姿估計，同時利用構(gòu)建的地圖進(jìn)行位姿優(yōu)化。在后端中加入回環(huán)檢測模塊，保證算法的完整性。實驗驗證了SD-SLAM算法在適合特征提取的場景下相對ORB-SLAM2算法能有效地提高相機軌跡精度。

SD-SLAM算法的改進(jìn)之處還有很多，例如算法的效率還需要進(jìn)一步提高；在環(huán)境缺少紋理、相機強旋轉(zhuǎn)運動時，跟蹤容易丟失的問題依舊存在。