基于融合Shi-Tomasi 算法的單目視覺里程計(jì)研究

孫蔚

（200093 上海市 上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院）

0 引言

近年來，隨著人工智能領(lǐng)域的高速發(fā)展，融合SLAM（時定位與建圖）技術(shù)的自動駕駛輔助系統(tǒng)成為熱門研究方向之一。經(jīng)典的視覺SLAM系統(tǒng)會包含5 個模塊，分別是傳感器數(shù)據(jù)提取、前端視覺里程計(jì)、后端非線性優(yōu)化、建圖以及貫穿其中的回環(huán)檢測，如圖1 所示。

圖1 視覺SLAM 結(jié)構(gòu)組成Fig.1 Visual SLAM structure composition

其中，最為關(guān)鍵的視覺里程計(jì)部分，通過相機(jī)傳感器獲取周圍環(huán)境的圖像數(shù)據(jù)，提取圖像間的共同特征信息并進(jìn)行運(yùn)動估計(jì)[1]。視覺里程計(jì)不同于人類的感知系統(tǒng)，能對周遭環(huán)境進(jìn)行具象描述，而是通過對傳入圖像進(jìn)行特征標(biāo)注的操作以獲得供后續(xù)處理的信息攝入。為了得到更為精確的位姿估算結(jié)果，特征提取與匹配的重要性自然不言而喻。國內(nèi)外眾多學(xué)者都對其進(jìn)行了大量研究，提出了多種算法。Moravec[2]通過計(jì)算各個像素沿著水平、垂直、對角線以及反對角線的灰度變化量來提取特征點(diǎn)，進(jìn)而提出了最早的角點(diǎn)檢測算法之一：Moravec 算子；Harris 和Stephens[3]在灰度變化的基礎(chǔ)上加入微分思想并定義了角點(diǎn)響應(yīng)函數(shù)的數(shù)學(xué)公式，從而得出了不易受旋轉(zhuǎn)、光照等影響且更為優(yōu)異的Harris 算子；Shi 和Tomasi[4]在前者的基礎(chǔ)上去掉了影響檢測穩(wěn)定性的經(jīng)驗(yàn)常數(shù)，提出了提取效果更好的Shi-Tomasi 算子；David Lowe[5]提出并完善了尺度不變特征轉(zhuǎn)換SIFT 算法，建立尺度空間利用高斯差分函數(shù)進(jìn)行查找，檢測到的特征點(diǎn)在面對光照變化、圖像旋轉(zhuǎn)、尺度變化等情況下仍然有較好的提取效果。

在SIFT 算法的基礎(chǔ)上，為了解決其計(jì)算復(fù)雜度較高的問題，Bay[6]等提出了SUFR 算法，引入積分圖像和盒子濾波器，對前者進(jìn)行優(yōu)化改進(jìn)使計(jì)算速度提高了近3 倍；Rosten 和Drummond[7]提出了ORB 算法，根據(jù)周圍連續(xù)多個像素點(diǎn)的灰度差判定進(jìn)行特征點(diǎn)檢測，是速度最快的特征點(diǎn)檢測算法之一。此外，隨著深度學(xué)習(xí)的快速發(fā)展，也出現(xiàn)了不少融合神經(jīng)網(wǎng)絡(luò)的特征檢測算法。例如，在SIFT 基礎(chǔ)上對關(guān)鍵點(diǎn)檢測、方向賦值、描述子向量進(jìn)行網(wǎng)絡(luò)訓(xùn)練整合的LIFT 算法，但由于實(shí)時性和CPU 占用率的考究，本文暫不做展開討論研究。

在各類特征點(diǎn)提取算法中，Shi-Tomasi 算法既擁有前者所訴算法的優(yōu)良特性，如運(yùn)行穩(wěn)定性好、不易受到運(yùn)動變化和外界環(huán)境噪聲的影響，又能使特征點(diǎn)的分布更加均勻而不是聚簇在某定區(qū)域，適合應(yīng)用于前端圖像提取與匹配過程中。本文在經(jīng)典特征提取算法ORB 上進(jìn)行修改，將Shi-tomaisi 算子與其ORB 算子結(jié)合，在不過大影響運(yùn)行效率的前提下，改善其特征點(diǎn)分布容易聚簇影響后續(xù)運(yùn)動估計(jì)精確度的問題。主體結(jié)構(gòu)如圖2 所示。

圖2 本文算法結(jié)構(gòu)Fig.2 Algorithm structure of this article

1 融合Shi-Tomasi 算法的特征提取與匹配

1.1 ORB 特征提取——FAST 算法

ORB 算法是在FAST 算法的基礎(chǔ)上進(jìn)行改進(jìn)所得來的，它包含F(xiàn)AST 角點(diǎn)法和BRIEF 描述子兩部分，后者在匹配過程中提供關(guān)鍵點(diǎn)附近的像素信息，而其中的FAST 算法作為最快速的特征提取算法之一，創(chuàng)造性地采用機(jī)器學(xué)習(xí)的方法對角點(diǎn)檢測過程進(jìn)行加速。檢測示意圖如圖3 所示，具體實(shí)現(xiàn)原理為：首先，在圖像中選擇一點(diǎn)p，以該點(diǎn)為圓心構(gòu)造一個半徑為3 像素值的圓形區(qū)域。再給定一灰度經(jīng)驗(yàn)閾值t，如果在圓形區(qū)域的邊界上有連續(xù)12 個像素點(diǎn)與p 點(diǎn)的灰度差絕對值大于或小于閾值t，則認(rèn)為p 點(diǎn)為特征點(diǎn)。為加快提取速度，可先與1，5，9，19 位置上的像素點(diǎn)進(jìn)行灰度比對。若這4 個位置存在2 個及以上不滿足條件的像素點(diǎn)，則可直接排除p 為特征點(diǎn)的可能；接著，將上述粗提取特征點(diǎn)周圍的16 個像素點(diǎn)輸入到經(jīng)過機(jī)器學(xué)習(xí)訓(xùn)練的決策樹中，進(jìn)而篩選出最優(yōu)ORB 特征點(diǎn)。

圖3 FAST 算法檢測示意圖Fig.3 Schematic diagram of FAST algorithm detection

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

FAST 算法雖然運(yùn)行速度十分高效，但只能檢測單一類型的角點(diǎn)，容易使得提取出來的特征點(diǎn)分布較為集中而忽略了其他區(qū)域關(guān)鍵點(diǎn)。為了避免產(chǎn)生特征點(diǎn)聚簇的問題影響后續(xù)視覺里程計(jì)的信息攝取，可以在FAST 算法進(jìn)行角點(diǎn)提取之后，將提取到的特征點(diǎn)用Shi-Tomasi 算法處理。

Shi-Tomasi 算法主要根據(jù)滑動窗口中像素點(diǎn)的灰度變化趨勢來進(jìn)行角點(diǎn)判斷。理論上，作為兩條邊緣線的交叉點(diǎn)即角點(diǎn)，無論在哪個方向上進(jìn)行移動，都會使得該點(diǎn)區(qū)域的梯度方向和大小發(fā)生明顯改變。于是，我們定義滑動窗口在圖像上平移[u,v]單位時，窗口在各個方向所產(chǎn)生的灰度變化為量E(u,v)，則如式（1）所描述

式中：（x，y）——窗口中心位置的坐標(biāo)；ω（x，y）——窗口函數(shù)代表像素的權(quán)重大小，當(dāng)中心位置是角點(diǎn)時，移動窗口會造成較為劇烈的灰度變化，此時窗口函數(shù)會設(shè)置較大一點(diǎn)，而離中心位置較遠(yuǎn)的像素點(diǎn)，由于對灰度變化的影響不那么大，所以此時的窗口權(quán)重會設(shè)置得較小；I（x，y）——該點(diǎn)的像素灰度值，所以，I（x+u，y+v）-I（x，y）就是窗口移動所產(chǎn)生的灰度差值。對任意一個角點(diǎn)而言，所計(jì)算出來的E（u，v）值會非常大。為了提升計(jì)算效率，采用泰勒展開式對其灰度插值進(jìn)行變化，如式（2）所示：

式中：Ix，Iy——I 的偏微分，顯示在圖像中就表示x 和y 方向上的梯度值。將偏微分帶入式（1）可得式（3）。

將u 和v 拿出，可化簡為式（4）

式中矩陣M 如式（5）所示展開

將式（5）實(shí)對稱矩陣進(jìn)行對角化處理，將2 個正交方向的變化分量提取出來，可得式（6），其中λ1、λ2為矩陣M 的特征值，決定了角點(diǎn)響應(yīng)函數(shù)R 的大小。

由于角點(diǎn)穩(wěn)定性受矩陣M 的較小特征值影響更大，于是我們定義角點(diǎn)響應(yīng)函數(shù)的分?jǐn)?shù)公式為R=min（λ1，λ2）diag，如果某點(diǎn)的分?jǐn)?shù)大于設(shè)定的閾值，則認(rèn)定為一個角點(diǎn)。

將ORB-ORB 與Shi-Tomasi 進(jìn)行優(yōu)化融合的特征提取算法包括如下幾個步驟：首先，采用ORB-9 算法（周圍至少有9 個點(diǎn)與中心點(diǎn)的像素差超過閾值）對整幅圖像進(jìn)行初步的特征提取操作；接著，對所得到的ORB 角點(diǎn)領(lǐng)域內(nèi)的像素點(diǎn)灰度值進(jìn)行處理進(jìn)而求得對應(yīng)的特征矩陣M；然后，根據(jù)矩陣M 中的特征值求得角點(diǎn)響應(yīng)函數(shù)R 的值，選取R 值大于設(shè)定閾值且為局部極大值的點(diǎn)作為我們最終選取進(jìn)行后續(xù)匹配的特征點(diǎn)。

1.3 特征描述與匹配

在特征點(diǎn)確定之后，需要進(jìn)一步計(jì)算這些特征點(diǎn)屬性的描述子以便于完成后續(xù)的匹配工作。ORB 算法采用BRIEF 特征向量描述器，用二進(jìn)制串來描述特征，無論是在描述的建立還是后續(xù)的匹配過程中，運(yùn)行速度都十分優(yōu)異。BRIEF 特征描述子是對圖像塊二值字符串的描述，圖像塊由二值灰度比較的結(jié)果構(gòu)成[8]。在特征點(diǎn)附近隨機(jī)選取若干點(diǎn)對，將這些點(diǎn)對的灰度值大小組合成一個長為256 的二進(jìn)制字串，并將這個二進(jìn)制字串作為該特征點(diǎn)的特征描述子。其二值判斷方法τ如式（7）所示，p（x）表示圖像塊在點(diǎn)x 處的灰度值。

本文采用暴力算法進(jìn)行圖像幀間的匹配，也就是當(dāng)特征點(diǎn)的描述子的相似度大于一定閾值時，認(rèn)為是相同特征點(diǎn)，也就是匹配點(diǎn)對成立。ORB 算法通過漢明距離的大小來表示特征點(diǎn)之間的相似度。漢明距離越小表示特征點(diǎn)之間的相似度越高。

圖像匹配過程中，不論是前期特征點(diǎn)的檢測還是之后的描述過程中，都會有誤差存在，同時匹配結(jié)果也會由于設(shè)置的閾值不同而產(chǎn)生變化，再加上圖片復(fù)雜程度和噪聲等因素的影響，會存在一定的錯誤匹配，因此本文算法在融合Shi-Tomasi 的特征提取與匹配過程后加入隨機(jī)采樣一致性算法作進(jìn)一步誤差剔除。

2 單目里程計(jì)運(yùn)動估計(jì)

以前后時刻兩幀圖像為基礎(chǔ)，采取融合Shi-Tomasi 角點(diǎn)響應(yīng)函數(shù)的ORB 算法獲得特征匹配點(diǎn)后，對其運(yùn)動展開合理的預(yù)估，其幾何模型如圖4 所示。由于前一幀圖像I1到后一幀圖像I2是剛體運(yùn)動，那么觀測點(diǎn)P 在圖像I1坐標(biāo)系的坐標(biāo)就可以通過剛體轉(zhuǎn)換變成圖像I2坐標(biāo)系下，如果匹配正確，p1，p2將會是同一個空間點(diǎn)在2 個成像平面上的投影。

圖4 對極幾何示意圖Fig.4 Schematic diagram of epipolar geometry

假設(shè)空間點(diǎn)P 的坐標(biāo)為[x，y，z]，則兩個像素點(diǎn)p1和p2的像素位置表達(dá)式分別為P1=KP，P2=K（RP+t）。其中，K 為相機(jī)的內(nèi)參矩陣；R和t 分別表示旋轉(zhuǎn)參量和平移參量用來描述不同圖像坐標(biāo)系下的相機(jī)運(yùn)動。現(xiàn)取u1，v1為兩像素點(diǎn)的歸一化平面坐標(biāo)，可以看成是一個二維的齊次坐標(biāo)[x/z，y/z，1]；則

代入P1，P2表達(dá)式可得

將P1，P2代入式（10）得

式（11）便被稱為對極約束，它表示O1，P，O2三者共面。

將中間部分記作本質(zhì)矩陣E，可進(jìn)一步簡化對極約束

至此，相機(jī)位姿估計(jì)問題可以分解為如下2個步驟，首先根據(jù)前后幀圖像上匹配點(diǎn)的像素位置求出本質(zhì)矩陣E，接著采用經(jīng)典的八點(diǎn)法[9]將本質(zhì)矩陣進(jìn)行奇異值分解，求出相機(jī)運(yùn)動的旋轉(zhuǎn)矩陣R 和平移矩陣T，這樣就初步完成了視覺里程計(jì)的兩幀之間的運(yùn)動估計(jì)推算。

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

為了評估融合Shi-Tomasi 特征檢測算法的視覺里程計(jì)的匹配效果和軌跡精度結(jié)果，本文對室外環(huán)境做圖片數(shù)據(jù)采集。相機(jī)移動過程中拍攝視頻的圖像序列采集幀率為10 fps，圖像大小為1 024×720。軌跡比對真值為由RTK 器件采集的GPS 數(shù)據(jù)作為參考。算法運(yùn)行的PC 平臺配置為Intel Core i5-9300H CPU @2.40GHz，內(nèi)存為8 GB，系統(tǒng)為 Ubuntu16.04。實(shí)驗(yàn)發(fā)現(xiàn)，本文算法與ORB 特征提取在簡單外物場景下有著較為相似的特征提取及匹配效果，但當(dāng)環(huán)境變量開始多樣化時，本文算法有著明顯更為優(yōu)異的提取效果，而ORB 特征提取容易發(fā)生特征擁簇現(xiàn)象，特別是當(dāng)存在一些紋理性較強(qiáng)的物體時，特征點(diǎn)都容易聚集在此處，例如下述場景中的樹木區(qū)域總是能吸引大量特征點(diǎn)的注入，導(dǎo)致匹配效果也不夠完善。具體實(shí)驗(yàn)結(jié)果分別如圖5、圖6 所示。

圖5 場景1 的匹配效果對比Fig.5 Matching effect comparison of scenario 1

圖6 場景2 的匹配效果對比Fig.6 Comparison of matching effects in scenario 2

結(jié)合表1，再從匹配數(shù)量和平均每幀匹配耗時兩方面進(jìn)行評估。可以看出，在同樣提取250個特征點(diǎn)的情況下，本文算法有著更高成功率的匹配點(diǎn)數(shù)。雖然在時效性上舍棄了一些速度，但匹配耗時仍滿足在相機(jī)提取數(shù)據(jù)幀率之下。

表1 特征匹配算法對比Tab.1 Comparison of feature matching algorithms

在兩者算法運(yùn)行過后，截取其中一段相同路徑的位姿估計(jì)值，與RTK 器件所提取的GPS數(shù)據(jù)進(jìn)行絕對軌跡誤差計(jì)算。其均值、中值及標(biāo)準(zhǔn)差等參考指標(biāo)如圖7 所示。可知，在復(fù)雜環(huán)境路段所帶來的不適應(yīng)性致使ORB 算法作出的運(yùn)動估計(jì)有著很大的偏差，而本文算法總體更為穩(wěn)定。

圖7 絕對軌跡誤差Fig.7 Absolute trajectory error

4 結(jié)論

針對ORB-SLAM 算法特征檢測模塊容易發(fā)生特征點(diǎn)簇?fù)韥G失圖像信息的問題，本文在其原先基礎(chǔ)上融合了Shi-Tomasi 檢測算子對由ORB檢測到的角點(diǎn)做二次篩選，在經(jīng)過隨機(jī)一致性算法進(jìn)行誤匹配刪除后，達(dá)成了精確度更高的匹配點(diǎn)。由仿真實(shí)驗(yàn)可知，本文算法在特征匹配比例和后續(xù)的軌跡推算上都有著更為優(yōu)秀的準(zhǔn)確率展示。