一種結(jié)合光流追蹤的雙目視覺里程計(jì)算法

李偉東，朱旭浩

(大連理工大學(xué) 汽車工程學(xué)院，遼寧 大連 116024)

0 引言

里程計(jì)技術(shù)最早出現(xiàn)于機(jī)器人領(lǐng)域，它是指機(jī)器人在未知環(huán)境中通過傳感器捕捉環(huán)境特征來確定自身位置和姿態(tài)，同時(shí)構(gòu)建一個(gè)增量地圖。使用相機(jī)作為傳感器的里程計(jì)稱為視覺里程計(jì)。相較于激光雷達(dá)傳感器，相機(jī)部署方便，價(jià)格低廉，并且可以獲取更為豐富的視覺信息。隨著自動(dòng)駕駛技術(shù)的發(fā)展，視覺里程計(jì)也越來越多地被用于對載具進(jìn)行定位和地圖構(gòu)建，因此基于視覺的里程計(jì)受到了廣泛關(guān)注。基于特征匹配的特征點(diǎn)法通常需要提取特征描述子并對其進(jìn)行匹配，這個(gè)過程會(huì)消耗較多的資源和時(shí)間，在一些無法搭載高算力設(shè)備的自動(dòng)駕駛場景中實(shí)時(shí)性較差；直接法則不需要耗費(fèi)時(shí)間計(jì)算提取描述子，在只計(jì)算稀疏像素點(diǎn)的光度誤差時(shí)速度較快，但是對于光照變化非常敏感，精度低于特征點(diǎn)法視覺里程計(jì)，并且缺乏描述子用于系統(tǒng)追蹤失敗時(shí)進(jìn)行重定位。因此一個(gè)輕量快速的高精度視覺里程計(jì)仍然是一個(gè)關(guān)鍵的研究問題。

根據(jù)數(shù)據(jù)關(guān)聯(lián)方式的不同，視覺里程計(jì)可以分為基于光度誤差的直接法和基于特征匹配的特征點(diǎn)法[1]。直接法視覺里程計(jì)通過最小化不同圖像之間的光度誤差來恢復(fù)相機(jī)運(yùn)動(dòng)。LSD-SLAM(Large Scale Direct Monocular SLAM)[2]是Engel等人提出的一種直接法視覺SLAM，它基于單目相機(jī)運(yùn)行，并且可以在CPU上進(jìn)行半稠密建圖。為了提高全局精度，系統(tǒng)還加入了回環(huán)檢測模塊。相較于傳統(tǒng)直接法，LSD-SLAM將光度誤差以及深度噪聲一起進(jìn)行優(yōu)化從而實(shí)現(xiàn)了較好的效果，但是由于采用了單目相機(jī)，所以無法恢復(fù)絕對尺度信息。Stereo LSD-SLAM[3]則是將LSD-SLAM拓展到雙目相機(jī)上，解決了單目相機(jī)無法恢復(fù)絕對尺度信息的問題。DSO(Direct Sparse Odometry)[4]是一種純直接法視覺里程計(jì)，它將數(shù)據(jù)關(guān)聯(lián)以及位姿估計(jì)統(tǒng)一成一個(gè)非線性優(yōu)化問題，并且使用了滑動(dòng)窗口算法，既控制了優(yōu)化規(guī)模，提高優(yōu)化速度，同時(shí)又保證了一定的系統(tǒng)精度。其缺點(diǎn)是由于圖像之間的光度誤差函數(shù)是一個(gè)非凸函數(shù)，因而優(yōu)化時(shí)非常容易陷入局部最優(yōu)值，只適合于移動(dòng)較小的場景中。SVO(Semi Direct Monocular Visual Odometry)[5]是Forster等提出的一個(gè)結(jié)合了特征點(diǎn)法和直接法的半直接法視覺里程計(jì)。其首先提取稀疏特征點(diǎn)，使用直接法進(jìn)行不同圖像之間對齊從而計(jì)算位姿；同時(shí)使用深度濾波器來估計(jì)深度信息。優(yōu)點(diǎn)是運(yùn)行速度非?？欤诘凸脑O(shè)備上也有較高的幀率；缺點(diǎn)是其對光照情況非常敏感，在光照變化大的情況下非常容易追蹤失敗。

特征點(diǎn)法視覺里程計(jì)通過提取視覺特征，獲取匹配關(guān)系后使用光束法平差[6]計(jì)算和優(yōu)化相機(jī)的位姿與地圖點(diǎn)位置。PTAM[7]是Klein等人提出的一個(gè)視覺里程計(jì)系統(tǒng)，其包含了追蹤和建圖兩個(gè)線程來同時(shí)恢復(fù)相機(jī)運(yùn)動(dòng)并優(yōu)化地圖點(diǎn)和位姿，首次將多線程用于視覺里程計(jì)算法中。Raulmur等提出的ORB-SLAM[8]則在PTAM兩個(gè)線程基礎(chǔ)上增加了回環(huán)檢測線程，其利用詞袋模型[9]進(jìn)行相似度檢測，在檢測到發(fā)生回環(huán)后進(jìn)行回環(huán)矯正來全局優(yōu)化。ORB-SLAM采用的傳感器為單目相機(jī)，無法恢復(fù)絕對尺度，并且非常容易出現(xiàn)尺度漂移的現(xiàn)象。在此基礎(chǔ)之上ORB-SLAM2[10]加入了對雙目相機(jī)以及RGBD相機(jī)的支持，不僅可以恢復(fù)絕對尺度，并且在RGBD模式下可以實(shí)現(xiàn)稠密建圖。隨后的ORB-SLAM3[11]則加入了魚眼相機(jī)模型，并且可以融合IMU傳感器，精度有了很大的提高。除了點(diǎn)特征外，線特征也可以用于位姿估計(jì)，在特征點(diǎn)缺失的場景下會(huì)有更好的魯棒性。PL-SLAM[12]是Pumarola等提出的一個(gè)將線特征用于視覺里程計(jì)上的視覺SLAM算法，該算法結(jié)合了線特征和點(diǎn)特征，在一些特征點(diǎn)缺失的場景中更加魯棒，精度更高，但是同時(shí)也提高了計(jì)算復(fù)雜度，導(dǎo)致系統(tǒng)計(jì)算資源消耗變大。Gomez等則將PL_SLAM擴(kuò)展到雙目相機(jī)上，不僅提高了系統(tǒng)精度，同時(shí)還可以恢復(fù)絕對尺度[13]。

精度與運(yùn)行速度是評價(jià)視覺里程計(jì)的兩個(gè)重要指標(biāo)。特征點(diǎn)法視覺里程計(jì)會(huì)消耗大量時(shí)間去計(jì)算每一幀圖像中關(guān)鍵點(diǎn)的描述子，對實(shí)時(shí)性有較大影響；直接法視覺里程計(jì)省去了描述子計(jì)算過程，但是對于光照極為敏感，相較于特征點(diǎn)法精度有所降低，同時(shí)缺乏描述子用于在系統(tǒng)追蹤失敗時(shí)進(jìn)行重定位。為了實(shí)現(xiàn)保持較高精度的同時(shí)有更好的實(shí)時(shí)性能，提出了一種結(jié)合了光流追蹤的視覺里程計(jì)算法。該算法基于特征點(diǎn)法，在追蹤階段主要通過光流追蹤來獲得匹配關(guān)系；生成關(guān)鍵幀時(shí)使用描述子來獲取匹配關(guān)系，并且在追蹤失敗時(shí)會(huì)使用描述子匹配進(jìn)行重定位。由于只在提取關(guān)鍵幀和重定位時(shí)計(jì)算和匹配描述子，算法的運(yùn)行速度有了大幅提升，同時(shí)保持了較高的精度。

1 視覺里程計(jì)算法

視覺里程計(jì)選取的傳感器為雙目相機(jī)。相較于單目相機(jī)，雙目相機(jī)可以計(jì)算得到像素點(diǎn)深度信息，解決了尺度不確定的問題，并且在追蹤過程中有著更好的魯棒性。算法包括追蹤線程和滑動(dòng)窗口優(yōu)化線程兩大部分。追蹤線程主要用于計(jì)算相機(jī)當(dāng)前位姿，滑動(dòng)窗口優(yōu)化線程則會(huì)對關(guān)鍵幀及地圖點(diǎn)進(jìn)行優(yōu)化，保證較高的計(jì)算精度。相機(jī)捕捉到的圖像作為輸入，輸出內(nèi)容包括關(guān)鍵幀和地圖點(diǎn)，兩者共同構(gòu)成了地圖。每個(gè)關(guān)鍵幀也保留了以其為參考的普通幀相對位姿。

視覺里程計(jì)在運(yùn)行時(shí)首先會(huì)進(jìn)行初始化操作，通過提取當(dāng)前圖像中的ORB特征點(diǎn)進(jìn)行匹配。由于初始化生成的地圖和位姿對整個(gè)系統(tǒng)運(yùn)行的精度有著較大影響，因此初始化過程所提取特征點(diǎn)數(shù)量較多。當(dāng)匹配成功的特征點(diǎn)對數(shù)量滿足要求后會(huì)三角化生成新的地圖點(diǎn)，并將對應(yīng)的圖像設(shè)置為關(guān)鍵幀。在生成初始地圖后系統(tǒng)會(huì)對其中的關(guān)鍵幀和地圖點(diǎn)位姿進(jìn)行一次全體優(yōu)化，保證系統(tǒng)的初始精度，同時(shí)會(huì)根據(jù)關(guān)鍵幀的位姿來計(jì)算并初始化運(yùn)動(dòng)模型。當(dāng)成功完成初始化工作后，系統(tǒng)會(huì)使用運(yùn)動(dòng)模型進(jìn)行追蹤，若系統(tǒng)追蹤失敗則會(huì)切換到參考關(guān)鍵幀追蹤進(jìn)行重定位來恢復(fù)相機(jī)當(dāng)前的位姿和運(yùn)動(dòng)模型。在運(yùn)行過程中系統(tǒng)會(huì)判斷當(dāng)前是否需要生成新的關(guān)鍵幀。當(dāng)滿足生成關(guān)鍵幀的條件時(shí)，系統(tǒng)會(huì)對當(dāng)前幀進(jìn)行ORB特征點(diǎn)提取操作，同時(shí)將其與上一關(guān)鍵幀中所提取的ORB特征點(diǎn)進(jìn)行匹配，并通過三角化生成新的地圖點(diǎn)。同時(shí)將新的關(guān)鍵幀和地圖點(diǎn)插入到滑動(dòng)窗口中進(jìn)行優(yōu)化，剔除冗余關(guān)鍵幀和地圖點(diǎn)。該算法框架如圖1所示。

圖1 算法框架

1.1 特征提取

常見的視覺特征點(diǎn)有SIFT特征點(diǎn)、SURF特征點(diǎn)以及ORB特征點(diǎn)。相較于SIFT和SURF特征點(diǎn)，ORB特征點(diǎn)在保持旋轉(zhuǎn)、尺度不變性的前提下有更高的計(jì)算速度，因此系統(tǒng)選取ORB特征點(diǎn)來進(jìn)行數(shù)據(jù)關(guān)聯(lián)。

ORB[14]特征點(diǎn)包含具有方向信息的FAST角點(diǎn)和BRIEF描述子兩部分。FAST角點(diǎn)通過檢測像素灰度變化來確定，其原理如圖2所示。FAST角點(diǎn)通過以下幾個(gè)步驟進(jìn)行提取：

圖2 FAST角點(diǎn)

1)選取一個(gè)像素p，記錄它的亮度I；

2)設(shè)定閾值T，并以像素p為中心，選取半徑為3的圓周上的像素點(diǎn)；

3)若有連續(xù)N個(gè)點(diǎn)的亮度大于I+T或者小于I-T，則將像素點(diǎn)p選取為FAST角點(diǎn)。

傳統(tǒng)的提取方法閾值亮度閾值T固定不變，容易導(dǎo)致在整張圖像中出現(xiàn)FAST角點(diǎn)過于集中和分布不均勻的現(xiàn)象，使得系統(tǒng)計(jì)算精度降低。為了緩解該現(xiàn)象，系統(tǒng)在提取FAST角點(diǎn)時(shí)會(huì)將圖像分割成多個(gè)區(qū)塊，對每個(gè)區(qū)塊進(jìn)行FAST角點(diǎn)提取。若某個(gè)區(qū)域提取到的FAST角點(diǎn)數(shù)量過少，系統(tǒng)會(huì)將該區(qū)域的亮度閾值T減小，從而提取到更多的角點(diǎn)[15]。當(dāng)FAST角點(diǎn)提取完畢后，系統(tǒng)會(huì)使用四叉樹均分策略剔除掉冗余的角點(diǎn)，使分布更加均勻。

FAST角點(diǎn)方向通過灰度質(zhì)心法來確定，步驟如下：

1)在一個(gè)圖像塊B中定義它的矩為：

mpq=∑x，y∈BxpyqI(x，y)

(1)

其中：p，q={0，1}。

2)圖像塊的質(zhì)心C為：

(2)

3)設(shè)O為圖像塊的幾何中心，通過向量將FAST角點(diǎn)方向定義為：

θ=arctan(m01/m10)

(3)

通過灰度質(zhì)心法計(jì)算得到FAST角點(diǎn)方向可以保證特征點(diǎn)的旋轉(zhuǎn)不變性。

BRIEF[16]描述子是一種二進(jìn)制描述子，通過比較FAST角點(diǎn)附近的像素亮度關(guān)系可以獲得一串由0和1構(gòu)成的向量。由于其使用了二進(jìn)制表達(dá)，因此相較于其他描述子有著非?？斓挠?jì)算速度。旋轉(zhuǎn)不變性通過FAST角點(diǎn)的方向來確定，尺度不變性則引入圖像金字塔來實(shí)現(xiàn)。如圖3所示，通過將圖像按照一定比例縮放形成圖像金字塔，在每一層進(jìn)行特征點(diǎn)提取，從而保證在特征匹配時(shí)的尺度不變性。

圖3 圖像金字塔

1.2 特征匹配

特征匹配用于獲取同一特征點(diǎn)在不同圖像中的像素位置。獲取特征點(diǎn)匹配關(guān)系后，系統(tǒng)會(huì)通過最小化重投影誤差來得到相機(jī)位姿。為了提高系統(tǒng)的運(yùn)行速度，同時(shí)保證計(jì)算精度，系統(tǒng)采用了光流追蹤和描述子匹配兩種匹配方法。

運(yùn)動(dòng)模型追蹤通過光流追蹤特征點(diǎn)來獲得匹配關(guān)系。光流用于描述圖像之間像素的運(yùn)動(dòng)，通過光流可以追蹤特征點(diǎn)在不同幀之間的位置從而獲取匹配關(guān)系。相較于描述子匹配，光流追蹤可以省去提取描述子的過程，速度更快。算法采用Luckas-Kanade[17]光流，簡稱LK光流，其工作原理如圖4所示。LK光流是一種稀疏光流，用于追蹤稀疏像素，其基于兩個(gè)假設(shè)：

圖4 LK光流法示意圖

1)灰度不變假設(shè)，即空間中任意一點(diǎn)的灰度值在不同圖像中是相同的。圖像中(x，y)處的像素從t時(shí)刻運(yùn)動(dòng)到t+dt時(shí)刻，坐標(biāo)變?yōu)?x+dx，y+dy)，其灰度值不變，用公式表示為I(x+dx，y+dy，t+dt)=I(x，y，t)。對其進(jìn)行泰勒展開，整理可得：

(4)

2)在同一個(gè)窗口內(nèi)的所有像素會(huì)做相同的運(yùn)動(dòng)。假設(shè)在同一窗口中選取了n個(gè)像素點(diǎn)，那么則有：

(5)

當(dāng)相機(jī)快速移動(dòng)時(shí)，光流追蹤特征點(diǎn)容易失敗。為了緩解該現(xiàn)象，系統(tǒng)使用了圖像金字塔來進(jìn)行光流追蹤。圖像金字塔首先對圖像進(jìn)行由精至粗進(jìn)行降采樣處理，同時(shí)上層的光流追蹤結(jié)果作為下一層的計(jì)算初始值，實(shí)現(xiàn)由粗至精追蹤光流，從而緩解快速運(yùn)動(dòng)帶來的影響。

通過光流追蹤獲取特征點(diǎn)會(huì)出現(xiàn)一些誤匹配現(xiàn)象，導(dǎo)致系統(tǒng)計(jì)算精度降低，因此系統(tǒng)會(huì)進(jìn)一步使用隨機(jī)抽樣一致性(RANSAC，random sample consensus)[18]算法進(jìn)行過濾，剔除錯(cuò)誤的匹配關(guān)系。通過光流追蹤來獲取特征點(diǎn)匹配關(guān)系不需要計(jì)算和匹配描述子，因此運(yùn)行速度有了較大提升。

生成關(guān)鍵幀和參考關(guān)鍵幀追蹤會(huì)使用描述子匹配來獲取特征點(diǎn)匹配關(guān)系。描述子用于描述關(guān)鍵點(diǎn)周圍的信息，通過對比特征點(diǎn)的描述子可以判斷匹配關(guān)系。系統(tǒng)采用的描述子為BRIEF描述子。它是一種二進(jìn)制的描述子，通過計(jì)其漢明距離可以確定特征點(diǎn)之間的匹配關(guān)系。傳統(tǒng)的匹配方法在遇到大量特征點(diǎn)時(shí)耗時(shí)非常大，并且會(huì)出現(xiàn)大量誤匹配現(xiàn)象，降低系統(tǒng)的實(shí)時(shí)性能和計(jì)算精度。因此系統(tǒng)采用改進(jìn)后的匹配方法，通過以下步驟進(jìn)行特征匹配：

1)通過運(yùn)動(dòng)模型提供的初始位姿計(jì)算得出特征點(diǎn)在下一幀圖像中的粗略位置，并與位于該位置半徑8個(gè)像素以內(nèi)提取到的特征點(diǎn)進(jìn)行匹配，選出漢明距離最短且滿足閾值要求的特征點(diǎn)作為匹配點(diǎn)。若未成功匹配，系統(tǒng)則會(huì)將半徑提高至14個(gè)像素，進(jìn)一步匹配。

2)根據(jù)灰度質(zhì)心法計(jì)算出的特征點(diǎn)方向來計(jì)算其在兩幀之間的旋轉(zhuǎn)角度，并將所有匹配成功的特征點(diǎn)旋轉(zhuǎn)角度進(jìn)行統(tǒng)計(jì)，將旋轉(zhuǎn)角度偏離較大的特征點(diǎn)剔除，最后使用RANSAC算法進(jìn)一步剔除外點(diǎn)，保證系統(tǒng)的整體精度。

1.3 關(guān)鍵幀和地圖點(diǎn)

關(guān)鍵幀[19]是指一系列連續(xù)圖像中最有代表性的一幀，其包含的信息質(zhì)量很高，相當(dāng)于普通幀進(jìn)行過濾和優(yōu)化的結(jié)果。采取關(guān)鍵幀策略可以有效降低信息的冗余度，控制系統(tǒng)優(yōu)化的規(guī)模，防止無效或錯(cuò)誤信息對系統(tǒng)的干擾。系統(tǒng)會(huì)提取關(guān)鍵幀的ORB特征點(diǎn)，并通過提取到的特征點(diǎn)三角化生成地圖點(diǎn)，將其插入到滑動(dòng)窗口中進(jìn)行優(yōu)化。為了確保系統(tǒng)的穩(wěn)定性，降低追蹤失敗的機(jī)率，關(guān)鍵幀的選取采用比較寬松的策略。系統(tǒng)初始化時(shí)會(huì)將第一幀設(shè)置為關(guān)鍵幀，三角化生成初始地圖點(diǎn)，并將其位姿設(shè)置為坐標(biāo)系原點(diǎn)。初始化后系統(tǒng)在運(yùn)行過程中若滿足以下條件之一則會(huì)生成新的關(guān)鍵幀：

1)追蹤到的特征點(diǎn)數(shù)量小于設(shè)定閾值；

2)距離上次生成關(guān)鍵幀超過10幀；

3)當(dāng)前圖像與上次生成的關(guān)鍵幀旋轉(zhuǎn)角度或者平移距離超過設(shè)定閾值。

關(guān)鍵幀之間提取并成功匹配的ORB特征點(diǎn)會(huì)用于三角化生成地圖點(diǎn)。由于只采用提取到的ORB特征點(diǎn)來三角化生成地圖點(diǎn)而并非所有的像素點(diǎn)，因此系統(tǒng)生成的是一個(gè)稀疏地圖。相較于稠密地圖，建立稀疏地圖可以大大降低資源消耗，提高運(yùn)行速度，使視覺里程計(jì)可以運(yùn)行在低算力平臺上，同時(shí)滿足較高精度的定位需求。

1.4 運(yùn)動(dòng)模型追蹤與參考關(guān)鍵幀追蹤

系統(tǒng)追蹤過程中主要采取運(yùn)動(dòng)模型追蹤，其通過光流追蹤來獲取特征點(diǎn)匹配關(guān)系，不需計(jì)算和匹配描述子，因此速度較快。運(yùn)動(dòng)模型會(huì)根據(jù)式(6)使用上一幀的初始位姿T1和速度δT來計(jì)算當(dāng)前幀的初始位姿T2，并以T2作為初始值將地圖點(diǎn)投影的位置與相機(jī)觀測到的位置做差，構(gòu)建出一個(gè)非線性優(yōu)化問題，并使用列文伯格-馬夸爾特算法進(jìn)行優(yōu)化求解。如式(7)所示，其中K為相機(jī)內(nèi)參，μ為特征點(diǎn)的像素坐標(biāo)，P為地圖點(diǎn)坐標(biāo)，T為相機(jī)位姿。

T2=δT*T1

(6)

(7)

系統(tǒng)初始化或者追蹤失敗時(shí)會(huì)采取關(guān)鍵幀追蹤，其通過描述子來獲取匹配關(guān)系。當(dāng)系統(tǒng)初始時(shí)運(yùn)動(dòng)模型為空，此時(shí)會(huì)進(jìn)行關(guān)鍵幀生成步驟，通過描述子匹配來計(jì)算相機(jī)位姿，初始化運(yùn)動(dòng)模型，隨后使用運(yùn)動(dòng)模型追蹤。當(dāng)運(yùn)動(dòng)模型追蹤失敗時(shí)，系統(tǒng)會(huì)提取當(dāng)前幀的ORB特征點(diǎn)，并與滑動(dòng)窗口內(nèi)的關(guān)鍵幀進(jìn)行匹配，成功匹配后會(huì)生成新的關(guān)鍵幀并恢復(fù)運(yùn)動(dòng)模型，繼續(xù)使用運(yùn)動(dòng)模型追蹤。

1.5 滑動(dòng)窗口優(yōu)化

系統(tǒng)生成關(guān)鍵幀后，系統(tǒng)在后端會(huì)對關(guān)鍵幀和地圖點(diǎn)進(jìn)行優(yōu)化，其本質(zhì)是一個(gè)非線性優(yōu)化問題。設(shè)zij為相機(jī)在Ti處觀察到的地圖點(diǎn)pj產(chǎn)生的數(shù)據(jù)，那么觀測誤差為：

eij=zij-h(Ti，pj)

(8)

系統(tǒng)所優(yōu)化的整體代價(jià)函數(shù)為：

(9)

為了保證系統(tǒng)的運(yùn)行速度，系統(tǒng)采用滑動(dòng)窗口算法[20]來進(jìn)行優(yōu)化?；瑒?dòng)窗口算法是指當(dāng)系統(tǒng)生成新的關(guān)鍵幀后，為了維持一定的計(jì)算量，防止優(yōu)化規(guī)模不斷擴(kuò)大，會(huì)設(shè)置一個(gè)窗口大小，將新的關(guān)鍵幀插入，同時(shí)將冗余的關(guān)鍵幀移除。若滑動(dòng)窗口內(nèi)關(guān)鍵幀數(shù)量沒有達(dá)到閾值，系統(tǒng)會(huì)對窗口內(nèi)所有關(guān)鍵幀和地圖點(diǎn)進(jìn)行優(yōu)化。當(dāng)關(guān)鍵幀數(shù)量超過設(shè)定的閾值時(shí)，系統(tǒng)會(huì)對窗口內(nèi)的關(guān)鍵幀判斷，將共視程度最小的關(guān)鍵幀設(shè)置為冗余關(guān)鍵幀，并使用舒爾補(bǔ)將其從滑動(dòng)窗口中邊緣化；對于沒有追蹤到的地圖點(diǎn)則直接將其從滑動(dòng)窗口中剔除。相較于直接將關(guān)鍵幀位姿固定不進(jìn)行迭代優(yōu)化或者直接將關(guān)鍵幀約束信息刪除的做法，該方法可以有效保留冗余關(guān)鍵幀的約束關(guān)系，在保證有較高的計(jì)算精度的同時(shí)也控制了用于優(yōu)化的地圖規(guī)模，提高了系統(tǒng)的實(shí)時(shí)性能。

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

實(shí)驗(yàn)首先會(huì)驗(yàn)證特征點(diǎn)提取、光流追蹤特征點(diǎn)以及描述子匹配特征點(diǎn)的效果，進(jìn)行定性分析。最后在數(shù)據(jù)集上對整個(gè)視覺里程計(jì)算法進(jìn)行實(shí)驗(yàn)，作為對比，會(huì)同時(shí)對雙目模式下的ORB-SLAM3算法進(jìn)行實(shí)驗(yàn)，評估兩者軌跡誤差以及實(shí)時(shí)性能，并對實(shí)驗(yàn)結(jié)果進(jìn)行分析。操作系統(tǒng)采用的是Ubuntu18，CPU型號為AMD Ryzen 7 4800U，主頻1.80 GHz，內(nèi)存大小為16 GB，不使用GPU加速，特征點(diǎn)提取數(shù)量預(yù)設(shè)為1 000。數(shù)據(jù)集采用的是KITTI[21]數(shù)據(jù)集00～10序列，其包含了城市道路、高速公路等多種場景，灰度圖由雙目相機(jī)采集并進(jìn)行了矯正，同時(shí)提供了位姿真實(shí)數(shù)據(jù)。

2.1 特征點(diǎn)提取實(shí)驗(yàn)

系統(tǒng)會(huì)在初始化過程和生成關(guān)鍵幀時(shí)會(huì)提取圖像中的ORB特征點(diǎn)。傳統(tǒng)的ORB特征點(diǎn)提取方法會(huì)出現(xiàn)特征點(diǎn)過于集中的現(xiàn)象，導(dǎo)致計(jì)算數(shù)值上精度下降。系統(tǒng)所采用的優(yōu)化后的提取方法可以使ORB特征點(diǎn)均勻分布在圖像中，并且可以將冗余的特征點(diǎn)剔除，避免出現(xiàn)特征點(diǎn)過于集中的現(xiàn)象，從而提升系統(tǒng)高數(shù)值計(jì)算精度和穩(wěn)定性。

在KITTI數(shù)據(jù)集上分別使用傳統(tǒng)ORB特征點(diǎn)提取方法和改進(jìn)后的方法進(jìn)行特征點(diǎn)提取，并將結(jié)果輸出為圖片進(jìn)行對比。實(shí)驗(yàn)的結(jié)果如圖5所示。

圖5 特征點(diǎn)提取實(shí)驗(yàn)

從實(shí)驗(yàn)結(jié)果可以看出，相較于傳統(tǒng)的ORB特征點(diǎn)提取方法，改進(jìn)后的方法可以有效地將特征點(diǎn)均勻分布在圖像中，避免了特征點(diǎn)集中的現(xiàn)象。

2.2 光流追蹤特征點(diǎn)實(shí)驗(yàn)

追蹤線程主要采用光流追蹤特征點(diǎn)獲取匹配關(guān)系，同時(shí)以上一幀圖像的位姿為初始值，通過最小化重投影誤差來計(jì)算和優(yōu)化得到當(dāng)前位姿。光流追蹤可以避免描述子的計(jì)算和匹配過程，使得系統(tǒng)有著較高的運(yùn)行速度。算法采用的是多層光流，相較于單層光流，在高速運(yùn)動(dòng)時(shí)仍能較好地追蹤特征點(diǎn)。追蹤成功后系統(tǒng)會(huì)使用RANSAC算法過濾掉一些錯(cuò)誤的追蹤信息，提高系統(tǒng)的精度。在KITTI數(shù)據(jù)集上，分別使用單層光流和系統(tǒng)所采用的多層光流對特征點(diǎn)進(jìn)行追蹤，并將追蹤結(jié)果繪制輸出進(jìn)行對比。實(shí)驗(yàn)結(jié)果如圖6所示。

圖6 光流追蹤實(shí)驗(yàn)

從實(shí)驗(yàn)結(jié)果可以看出，優(yōu)化后的多層光流可以追蹤到更多的特征點(diǎn)，同時(shí)錯(cuò)誤的結(jié)果也大大減少。

2.3 描述子匹配特征點(diǎn)實(shí)驗(yàn)

生成關(guān)鍵幀時(shí)系統(tǒng)會(huì)提取當(dāng)前圖像中的ORB特征點(diǎn)，并使用描述子進(jìn)行特征匹配。使用描述子漢明距離的傳統(tǒng)匹配方法會(huì)出現(xiàn)大量錯(cuò)誤匹配，在遇到大量特征點(diǎn)時(shí)還會(huì)嚴(yán)重消耗資源，導(dǎo)致最后的運(yùn)行速度和計(jì)算精度降低。系統(tǒng)采取的優(yōu)化后的匹配方法，可以有效降低誤匹配現(xiàn)象，有著更高的計(jì)算精度。在KITTI數(shù)據(jù)集上首先使用恢復(fù)運(yùn)動(dòng)模型，用于為粗匹配提供初始位姿。隨后進(jìn)行特征匹配，將最終匹配結(jié)果和傳統(tǒng)方法匹配結(jié)果輸出，如圖7所示。

圖7 描述子匹配實(shí)驗(yàn)

實(shí)驗(yàn)結(jié)果表明，相比于傳統(tǒng)的匹配方法，改進(jìn)后的方法可以大大降低誤匹配現(xiàn)象，從而保證了系統(tǒng)的精度。

2.4 視覺里程計(jì)整體性能實(shí)驗(yàn)

評價(jià)視覺里程計(jì)主要包括軌跡誤差以及實(shí)時(shí)性能兩個(gè)指標(biāo)。軌跡誤差指標(biāo)采用絕對平移誤差來評估，其計(jì)算公式如式(10)所示：

(10)

實(shí)時(shí)性能則通過計(jì)算平均追蹤每一幀圖像的耗時(shí)來評定。

在KITTI數(shù)據(jù)集00～10序列上，分別測試所提視覺里程計(jì)算法和雙目模式下的ORB-SLAM3算法。由于所提視覺里程計(jì)算法沒有回環(huán)檢測模塊，因此在精度評估時(shí)還會(huì)測試雙目模式下去掉回環(huán)檢測功能的ORB-SLAM3算法，并對結(jié)果進(jìn)行對比分析。每次測試前均會(huì)將電腦進(jìn)行重啟操作，避免每次運(yùn)行實(shí)驗(yàn)結(jié)束后對接下來的測試造成影響。運(yùn)行時(shí)系統(tǒng)會(huì)記錄追蹤每一幀所消耗的時(shí)間；運(yùn)行結(jié)束后保存生成的數(shù)據(jù)集位姿數(shù)據(jù)，通過與真實(shí)位姿對比計(jì)算絕對平移誤差，同時(shí)計(jì)算追蹤每一幀的平均耗時(shí)。計(jì)算絕對平移誤差使用的工具為evo，它是一款用于視覺SLAM和視覺里程計(jì)軌跡評估的工具，可以評估軌跡與真值之間的誤差，并且支持多種數(shù)據(jù)集軌跡格式之間的相互轉(zhuǎn)換。計(jì)算追蹤每一幀所消耗的時(shí)間則由系統(tǒng)內(nèi)置函數(shù)實(shí)現(xiàn)。為了保證結(jié)果的準(zhǔn)確性，每個(gè)算法均測試5次，取最后的平均值作為結(jié)果進(jìn)行分析。

圖8顯示了所提視覺里程計(jì)算法在KITTI數(shù)據(jù)集各序列上建立的稀疏地圖。相較于稠密地圖，稀疏地圖消耗的資源更少，可以僅使用CPU而不借助GPU加速即可生成和維護(hù)，并且可以滿足定位需求。

圖8 視覺里程計(jì)在 KITTI數(shù)據(jù)集00～10序列上建立的稀疏地圖

表1顯示了在KITTI數(shù)據(jù)集上的軌跡精度。從表中數(shù)據(jù)可以看出，在00、02、05～09等序列上發(fā)生了回環(huán)場景，因此有回環(huán)檢測模塊并且進(jìn)行了全局優(yōu)化的ORB-SLAM3算法精度最高，關(guān)閉回環(huán)檢測功能后精度明顯下降；所提出的視覺里程計(jì)算法由于在滑動(dòng)窗口中采取了邊緣化關(guān)鍵幀的策略，保留了被剔除關(guān)鍵幀的約束信息，因此在所有序列中軌跡誤差相較于關(guān)閉回環(huán)檢測模塊的ORB-SLAM3算法更低，擁有著更高的精度水平，在發(fā)生回環(huán)場景中的序列精度會(huì)略低于有回環(huán)檢測功能的ORB-SLAM3算法。在沒有發(fā)生回環(huán)場景的03、04、10等序列上所提出的視覺里程計(jì)算法有著非常高的精度，與具有完整回環(huán)檢測模塊和全局優(yōu)化的ORB-SLAM3算法精度相當(dāng)。綜上可以看出，在所有序列下所提視覺里程計(jì)算法精度均高于去掉回環(huán)檢測功能的ORB-SLAM3算法；在無回環(huán)的場景下精度與進(jìn)行了回環(huán)檢測和全局優(yōu)化的ORB-SLAM3算法處于同一水平，僅在大范圍有回環(huán)場景的情況下精度有所下降，但仍處于較高水平。

表1 絕對平移誤差對比

表2顯示了算法在KITTI據(jù)集上平均每追蹤一幀圖像所需的時(shí)間。由于所提算法在追蹤線程主要通過光流追蹤特征點(diǎn)來獲取匹配關(guān)系，省去了重復(fù)計(jì)算描述子的過程，因此運(yùn)行速度有了極大的提升。相比于雙目模式下的ORB-SLAM3算法，所提出的視覺里程計(jì)算法追蹤每一幀所花費(fèi)的平均時(shí)間由52 ms降低到16 ms。其中序列01是高速公路場景，載具處于高速運(yùn)動(dòng)狀態(tài)，為了保證追蹤穩(wěn)定系統(tǒng)會(huì)更頻繁地生成關(guān)鍵幀，因此處理時(shí)間相比其他低速場景會(huì)有所增加，但是仍低于ORB-SLAM3算法；而在城市路段中低速場景下，追蹤每一幀的耗時(shí)僅需不到20 ms，實(shí)時(shí)性能有著巨大的提升。綜上可以看出，所提視覺里程計(jì)算法在實(shí)時(shí)性能上有著巨大的提高，在低算力設(shè)備上有著明顯的優(yōu)勢。

表2 追蹤每一幀圖像平均耗時(shí)

3 結(jié)束語

針對特征點(diǎn)法視覺里程計(jì)頻繁計(jì)算和匹配描述子導(dǎo)致系統(tǒng)實(shí)時(shí)性能變差的問題，提出一種結(jié)合光流追蹤的雙目視覺里程計(jì)算法。該算法在追蹤線程主要通過光流追蹤特征點(diǎn)來獲取匹配關(guān)系，計(jì)算并優(yōu)化相機(jī)位姿；生成關(guān)鍵幀時(shí)提取和匹配ORB特征點(diǎn)，并三角化出新的地圖點(diǎn)，隨后將新的關(guān)鍵幀和地圖點(diǎn)插入滑動(dòng)窗口中進(jìn)行優(yōu)化。由于該算法大部分時(shí)間采用光流追蹤，不需要計(jì)算和匹配描述子，同時(shí)采用滑動(dòng)窗口算法來優(yōu)化，保留了被剔除的關(guān)鍵幀約束關(guān)系，因而有較快的運(yùn)動(dòng)速度和較高的精度。在KITTI數(shù)據(jù)集上的實(shí)驗(yàn)表明，所提視覺里程計(jì)算法可以在較高精度的情況下，有著非?？斓倪\(yùn)行速度，具有較高的實(shí)用價(jià)值。由于本文所提出的視覺里程計(jì)算法沒有加入回環(huán)檢測和全局優(yōu)化功能，因此不是一個(gè)完整的視覺SLAM系統(tǒng)，在有回環(huán)的場景下精度有所降低。因此加入回環(huán)檢測和全局優(yōu)化功能是下一階段的主要研究內(nèi)容。