基于高斯金字塔的視覺(jué)里程計(jì)算法研究

劉 瑞，徐雪松，曾 昱

（華東交通大學(xué) 電氣與自動(dòng)化工程學(xué)院，江西 南昌330013）

視覺(jué)里程計(jì)算法[1-2]是視覺(jué)定位的方法之一，通過(guò)在車(chē)輛、機(jī)器人等載體上裝備的單個(gè)或多個(gè)相機(jī)獲取視頻信息，然后通過(guò)對(duì)連續(xù)幀圖像分析計(jì)算運(yùn)動(dòng)情況[3]。 相比全球定位系統(tǒng)或慣性導(dǎo)航等定位方式，視覺(jué)里程計(jì)[4-5]成本更低、累積誤差更小，因而在室內(nèi)和航空航天等領(lǐng)域發(fā)揮著重要的作用[6]，是對(duì)傳統(tǒng)定位方式的有效補(bǔ)充[7]。

視覺(jué)里程計(jì)的主要環(huán)節(jié)包括圖片獲取、特征提取與匹配追蹤、運(yùn)動(dòng)估計(jì)等[8]。圖片獲取可通過(guò)相機(jī)獲得。特征提取與追蹤[9-10]是匹配或追蹤前后幀的同一特征點(diǎn)。 運(yùn)動(dòng)估計(jì)則是根據(jù)特征點(diǎn)變換信息并結(jié)合相機(jī)內(nèi)外參數(shù)恢復(fù)其運(yùn)動(dòng)信息。在特征提取環(huán)節(jié)中，由于角點(diǎn)檢測(cè)算法簡(jiǎn)單、實(shí)時(shí)性好，為目前常用的提取算法，主要分為基于圖像邊緣和基于圖像灰度的方法兩大類(lèi)[11]。 基于灰度的方法運(yùn)行速度快、精度高，為主流算法。代表性算法有Moravec、Fast、Harris、Shi-Tomasi 等。但這些算法存在一個(gè)共同問(wèn)題：容易出現(xiàn)角點(diǎn)聚簇現(xiàn)象。

為解決特征分布不均勻的問(wèn)題，一些學(xué)者進(jìn)行了研究。 如：文獻(xiàn)[12]利用相鄰匹配特征點(diǎn)之間的空間距離不變性，將特征點(diǎn)分為靜態(tài)與動(dòng)態(tài)區(qū)域，最終只提取靜態(tài)背景區(qū)域的特征點(diǎn)。 文獻(xiàn)[13]將圖片均勻等分成幾個(gè)區(qū)域，然后對(duì)每個(gè)區(qū)域釆用不同的閾值進(jìn)行特征提取。這些方法的基本思路都是先將圖像分塊，然后針對(duì)不同塊選擇不同閾值，以保證每個(gè)部分都保留基本的角點(diǎn)信息。對(duì)于這類(lèi)方法，合理分塊和選擇角點(diǎn)閾值都是很重要的問(wèn)題。

針對(duì)角點(diǎn)分布聚集問(wèn)題，本文將高斯金字塔[14-15]方法應(yīng)用到視覺(jué)里程計(jì)中的角點(diǎn)特征提取過(guò)程中，提出了一種新的角點(diǎn)提取方法。該方法通過(guò)對(duì)金字塔頂層圖像進(jìn)行角點(diǎn)提取，對(duì)特征位置進(jìn)行粗定位；然后映射到金字塔底層圖像進(jìn)行精確定位，從而獲取均勻的角點(diǎn)信息。 文章采用KITTI 數(shù)據(jù)集進(jìn)行了算法驗(yàn)證，結(jié)果表明該方法能夠有效改善所提取的角點(diǎn)分布均勻性和所恢復(fù)得運(yùn)動(dòng)信息的精確度。

1 基于高斯金字塔的角點(diǎn)提取算法

1.1 促進(jìn)角點(diǎn)信息均勻化的思路

造成角點(diǎn)信息分布不均勻的根本問(wèn)題在于角點(diǎn)特征反差太大。 對(duì)于紋理特征分布不均衡的圖像，角點(diǎn)特征都聚集在紋理豐富區(qū)域，而紋理稀疏區(qū)域則數(shù)量很少。 針對(duì)該問(wèn)題，應(yīng)該對(duì)各個(gè)區(qū)域精準(zhǔn)定位，單獨(dú)提取特征。 基本思路是通過(guò)高斯金字塔縮小原圖尺度，壓縮原紋理豐富區(qū)域細(xì)節(jié)，增加紋理稀疏區(qū)域紋理。 通過(guò)尺度壓縮可以自適應(yīng)的促使整個(gè)圖像特征分布均勻化，通過(guò)控制每個(gè)區(qū)域的特征數(shù)量來(lái)促使不同的圖像特征信息保持在一個(gè)穩(wěn)定的范圍內(nèi)。

本文先采用Shi-Tomasi 方法提取小尺度圖片角點(diǎn)， 這些角點(diǎn)信息可以提示原圖像對(duì)應(yīng)位置特征信息。不過(guò)，由于高斯金字塔變換會(huì)模糊圖像細(xì)節(jié)，故小尺度圖像的角點(diǎn)信息精確度不夠。 因此，可將該角點(diǎn)位置映射回原圖，在原圖相對(duì)應(yīng)位置的周?chē)鷧^(qū)域重新分塊搜索精確的角點(diǎn)位置，這樣一來(lái)，既在宏觀上保持了角點(diǎn)分布的均勻性，在微觀上也能獲得更精確的角點(diǎn)位置信息。

1.2 角點(diǎn)提取算法流程

基于高斯金字塔的角點(diǎn)提取算法主要有以下幾個(gè)步驟：

Step1：用高斯金字塔對(duì)原圖像進(jìn)行尺度壓縮。

設(shè)原圖為I，高斯金字塔有N 層，從塔底到塔頂圖像為I1，I2，…，IN，則高斯金字塔可以用公式（1）生成

其中：l 為第l 個(gè)圖層；w（m，n）為高斯濾波窗口函數(shù)；1≤l≤N，0≤i≤r，0≤j≤c；r，c 分別是第l 圖層的高和寬。

Step2：提取高斯金字塔頂層圖像角點(diǎn)信息。

角點(diǎn)提取算法很多，如Sift、Fast、Shi-Tomasi 等。 考慮到Shi-Tomasi 方法穩(wěn)定性好、自適應(yīng)能力強(qiáng)、實(shí)時(shí)性好，本文選用Shi-tomasi 方法對(duì)金字塔頂層圖像進(jìn)行角點(diǎn)提取。 具體計(jì)算方法如下：

Step2.1：逐個(gè)像素點(diǎn)計(jì)算金字塔頂層圖像IN的橫向梯度Ix和縱向梯度Iy。

Step2.2：按公式（2）計(jì)算每個(gè)像素點(diǎn)的M 矩陣

其中：w（x，y）是窗口濾波函數(shù)，通常選擇為高斯函數(shù)；x 和y 是所選擇的平滑窗口內(nèi)的像素點(diǎn)坐標(biāo)索引。

Step2.3：逐個(gè)像素點(diǎn)計(jì)算相應(yīng)像素點(diǎn)M 矩陣的特征值λ1、λ2，若兩個(gè)特征值中較小的一個(gè)大于最小閾值，則會(huì)得到角點(diǎn)，并標(biāo)記相應(yīng)位置。 所得角點(diǎn)位置數(shù)據(jù)集為corners1

其中：i、j 是當(dāng)前像素點(diǎn)位置索引；n1為角點(diǎn)數(shù)閾值。

Step3：將頂層角點(diǎn)位置信息映射回原圖，確定原圖的角點(diǎn)搜索區(qū)域。 該過(guò)程分成如下幾步：

Step3.1：按公式（4）計(jì)算corners1 對(duì)應(yīng)原圖像的位置corners2

其中：N 為金字塔層數(shù)。

Step3.2：以corners2 中各點(diǎn)為中心，重新確定角點(diǎn)搜索窗口。 由第k 個(gè)點(diǎn)構(gòu)成的窗口范圍為

其中：角點(diǎn)搜索窗口的寬度為W+1，高度為H+1。

Step3.3：針對(duì)每個(gè)搜索窗口，按特定數(shù)量閾值Q，采用Shi-Tomasi 方法搜索角點(diǎn)信息，并記錄下來(lái)。具體計(jì)算方法見(jiàn)Step2.1-Step2.3。 第k 個(gè)窗口搜索到的角點(diǎn)坐標(biāo)信息構(gòu)成角點(diǎn)集corners3k。

Step3.4：將所有窗口搜索結(jié)果合成角點(diǎn)集corners，也就是最終結(jié)果

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

為了驗(yàn)證算法的有效性，本文進(jìn)行了相關(guān)實(shí)驗(yàn)。 實(shí)驗(yàn)主要分兩步進(jìn)行：第1 步是角點(diǎn)提取算法的比較。分別采用Shi-Tomasi、Fast、Harris 算法和本文算法對(duì)相同圖像進(jìn)行角點(diǎn)提取，用來(lái)反映四種算法在保證提取信息均勻化方面的性能；第2 步是采用不同的角點(diǎn)提取方法計(jì)算載體運(yùn)動(dòng)信息精確度的對(duì)比，主要是將幾種算法應(yīng)用于視覺(jué)里程計(jì)，計(jì)算載體運(yùn)動(dòng)信息，用來(lái)觀察本文角點(diǎn)提取方法對(duì)載體運(yùn)動(dòng)信息計(jì)算精確度的影響。

采用的硬件平臺(tái)是Windows10 64 位NVIDIA GeForce MX150，軟件平臺(tái)是vs2017+opencv3.4，用做實(shí)驗(yàn)的數(shù)據(jù)集采用通用的KITTI[16]數(shù)據(jù)集。

2.1 角點(diǎn)提取算法比較

圖片選取的是KITTI 數(shù)據(jù)集中一張典型的路面信息圖，環(huán)境單一，路面紋理較弱，圖片中一般只有天空、草叢和樹(shù)葉等信息。因?yàn)槁放詷?shù)木繁茂，紋理豐富；路面光滑，紋理較少，兩者反差較大，再加上光線(xiàn)的不確定因素，特征很難提取均勻。在高速公路這種典型的紋理分布不均勻場(chǎng)景中行駛，樹(shù)葉和天空形成的斑駁印記一般都是角點(diǎn)聚簇的區(qū)域，存在大量相似、重復(fù)、聚集的角點(diǎn)，這種類(lèi)型的角點(diǎn)在前后幀追蹤時(shí)難度較大。如遇轉(zhuǎn)彎或角點(diǎn)聚集在圖片邊緣等情形，前后幀畫(huà)面差別較大，大量聚集在邊緣的角點(diǎn)易大量瞬間滑出畫(huà)面，無(wú)法保證圖片角點(diǎn)個(gè)數(shù)穩(wěn)定在一個(gè)常數(shù)周?chē)?而且，圖像中的角點(diǎn)相對(duì)集中，利用不夠離散的角點(diǎn)求解位姿不能夠代表相鄰幀之間的位姿變化。

本文分別采用Shi-Tomasi、Harris、Fast 以及本文算法對(duì)圖片進(jìn)行角點(diǎn)提取， 每種方法均提取300 個(gè)角點(diǎn)，其中本文算法參數(shù)為N=3，W=H=30，n1=147，Q=2。 4 種算法角點(diǎn)分布情況見(jiàn)圖1～圖4。

圖1 Shi-Tomasi 算法提取結(jié)果Fig.1 Extraction result of Shi-Tomasi algorithm

圖2 Fast 算法提取結(jié)果Fig.2 Extraction results of Fast algorithm

圖3 Harris 算法提取結(jié)果Fig.3 Extraction results of Harris algorithm

圖4 本文算法提取結(jié)果Fig.4 Extraction results of the text algorithm

從圖1 與圖4 可以看出，Harris 與Shi-Tomasi 算法角點(diǎn)聚簇現(xiàn)象比較明顯， 主要聚集在光線(xiàn)明亮、樹(shù)葉、草叢等強(qiáng)紋理區(qū)域。 Fast 算法則強(qiáng)烈依賴(lài)閾值，角點(diǎn)聚集程度很高，而本文算法檢測(cè)到的角點(diǎn)，原本聚簇、相似的角點(diǎn)被分隔開(kāi)來(lái)，原本光線(xiàn)較暗區(qū)域也有角點(diǎn)被提取出來(lái)，整張圖片各個(gè)角落、各個(gè)區(qū)域都有角點(diǎn)分布，分布均勻、離散。

為驗(yàn)證本文算法可滿(mǎn)足角點(diǎn)提取實(shí)時(shí)性要求， 采用本文算法與Shi-Tomasi 算法、Fast 算法、Harris 算法分別對(duì)2 張圖片進(jìn)行不同角點(diǎn)閾值的特征提取，角點(diǎn)檢測(cè)時(shí)間如下表1 所示。 計(jì)算中，感興趣區(qū)域的獲取以及每個(gè)區(qū)域進(jìn)行二次角點(diǎn)提取是導(dǎo)致運(yùn)行速度慢的的主要原因。 但是尺度化處理縮小了需處理圖片的大小，這在一定程度上彌補(bǔ)了感興趣區(qū)域耗時(shí)的缺點(diǎn)，滿(mǎn)足實(shí)時(shí)性要求。

表1 計(jì)算時(shí)間比較Tab.1 Comparison of calculation times

為了檢驗(yàn)本文算法對(duì)追蹤信息的保持能力，采用KITTI 數(shù)據(jù)集01 序列與10 序列，用金字塔LK 光流法追蹤前20 幀圖片，并與綜合性能表現(xiàn)最好的Shi-Tomasi 方法進(jìn)行對(duì)比。 金字塔LK 光流法是在原光流法的基礎(chǔ)上加入了圖像金字塔算法，用于解決單純光流法只適用于相機(jī)的微小運(yùn)動(dòng)，而在實(shí)際情況中經(jīng)常出現(xiàn)較大的運(yùn)動(dòng)幅度的問(wèn)題。 金字塔LK 光流法為常用的角點(diǎn)追蹤算法。

01 序列與10 序列采集的是轉(zhuǎn)彎處數(shù)據(jù)集，前后幀畫(huà)面差別較大。 采用召回率來(lái)反應(yīng)光流追蹤性能，召回率＝追蹤角點(diǎn)個(gè)數(shù)／角點(diǎn)個(gè)數(shù)，角點(diǎn)個(gè)數(shù)即第一幀圖片檢測(cè)到的總角點(diǎn)個(gè)數(shù)，追蹤角點(diǎn)個(gè)數(shù)為之后每一幀追蹤到的角點(diǎn)個(gè)數(shù)。為反映召回率，沒(méi)有設(shè)置追蹤角點(diǎn)個(gè)數(shù)低于某個(gè)閾值即重新檢測(cè)。召回率反映了角點(diǎn)的可區(qū)分性，召回率高說(shuō)明角點(diǎn)可區(qū)分性好。

具體情況如圖5 所示，隨著物體的運(yùn)動(dòng)，一些角點(diǎn)會(huì)逐漸跑出畫(huà)面外，所以召回率是逐漸降低的。 從圖中可看出，均勻化后的本文算法召回率優(yōu)于原Shi-Tomasi 算法，更有利于維持不同圖像特征點(diǎn)數(shù)量的穩(wěn)定性。

圖5 01 序列與10 序列召回率計(jì)算結(jié)果Fig.5 Recall rate calculation results of Sequence 01 and Sequence 10

2.2 載體運(yùn)動(dòng)信息計(jì)算精確度對(duì)比

為了比較角點(diǎn)檢測(cè)結(jié)果對(duì)載體運(yùn)動(dòng)信息計(jì)算的影響，將本文方法用于視覺(jué)里程計(jì)計(jì)算。 角點(diǎn)提取環(huán)節(jié)用本文算法與Shi-Tomasi 算法、Harris 算法與Fast 算法對(duì)比，然后用金字塔LK 光流法追蹤角點(diǎn)，最后根據(jù)相機(jī)對(duì)極幾何約束模型恢復(fù)運(yùn)動(dòng)軌跡。 求出前兩幀圖片之間的運(yùn)動(dòng)尺度k 與相對(duì)位姿態(tài)信息（Rt，Tt）， 后續(xù)位姿信息通過(guò)Pt=Rt-1Pt-1+Tt-1k計(jì)算得到。

數(shù)據(jù)集采用KITTI 數(shù)據(jù)集02 左序列前500張。 其真實(shí)軌跡由KITTI 數(shù)據(jù)集groundtruth 提供。 圖6 所示為分別采用Shi-Tomasi 算法、本文算法、Harris 算法、Fast 算法對(duì)比所得到的角點(diǎn)信息恢復(fù)的運(yùn)動(dòng)信息。 從實(shí)驗(yàn)結(jié)果中可看出，四種算法隨著時(shí)間的推移軌跡都有一定程度的發(fā)散現(xiàn)象，這是由于算法本身只計(jì)算兩幀圖片之間的運(yùn)動(dòng)信息導(dǎo)致的誤差累計(jì)。 而本文算法計(jì)算得到的運(yùn)動(dòng)信息的精度優(yōu)于另外三種算法。

圖6 數(shù)據(jù)集02 運(yùn)動(dòng)信息計(jì)算結(jié)果Fig.6 Calculation results of motion information in Dataset 02

為了更好的評(píng)測(cè)視覺(jué)里程計(jì)的效果，本文沒(méi)有加入回環(huán)檢測(cè)環(huán)節(jié)。 如加入回環(huán)檢測(cè)累計(jì)誤差會(huì)小很多。 同時(shí)，本文進(jìn)一步計(jì)算了運(yùn)動(dòng)信息的誤差曲線(xiàn)。 如圖7 所示，誤差曲線(xiàn)是計(jì)算實(shí)驗(yàn)值與真實(shí)值之間的歐氏距離. 橫坐標(biāo)為圖像采樣幀號(hào)，縱坐標(biāo)為歐式距離。 從圖中可以看出本文算法恢復(fù)的運(yùn)動(dòng)信息比Shi-Tomasi 算法誤差要小，恢復(fù)的位姿態(tài)信息準(zhǔn)確度更高。 而且隨著圖像采樣幀號(hào)的增加，因?yàn)楸疚乃惴ǖ恼倩芈时憩F(xiàn)更優(yōu)良，故角點(diǎn)流失也保持在一個(gè)穩(wěn)定的狀態(tài)，因此誤差曲線(xiàn)逐漸趨于穩(wěn)定，恢復(fù)的位姿態(tài)信息準(zhǔn)確度更高。

圖7 02 序列誤差曲線(xiàn)Fig.7 Error curve of Sequence 02

3 結(jié)論

針對(duì)視覺(jué)里程計(jì)算法中因角點(diǎn)提取不均勻?qū)е禄謴?fù)的運(yùn)動(dòng)信息誤差較大問(wèn)題，本文提出了一種基于高斯金字塔的角點(diǎn)提取算法。實(shí)驗(yàn)表明，本文算法能夠準(zhǔn)確提取圖片中的特征信息，有效解決重復(fù)紋理與豐富紋理特征聚簇問(wèn)題，角點(diǎn)分布均勻、離散，提高了算法的魯棒性。在同等條件下運(yùn)行本文算法，在視覺(jué)里程計(jì)中可以得到更加準(zhǔn)確的運(yùn)動(dòng)信息。