基于虛擬特征點(diǎn)的視覺里程計(jì)運(yùn)動(dòng)物體干擾消除方法?

閆 銘 王 歡 崔瓊杰

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

視覺里程計(jì)（Visual Odometry［1］）是通過單個(gè)或者多個(gè)視頻傳感器的輸入估計(jì)運(yùn)載體（如車輛、機(jī)器人）自身運(yùn)動(dòng)的過程，由于其成本低、獲取信息容易的特點(diǎn)，應(yīng)用領(lǐng)域涵蓋機(jī)器人、增強(qiáng)現(xiàn)實(shí)和自動(dòng)駕駛等［2］。視覺里程計(jì)相比于傳統(tǒng)的輪式里程計(jì)，視覺里程計(jì)（VO）的優(yōu)勢是不受車輪在不平整地面或其他不良狀況下打滑的影響。在GPS 失效的情況下，視覺里程計(jì)是解決機(jī)器人在這些環(huán)境下精確自主定位問題的一種有效技術(shù)途徑。

解決移動(dòng)物體干擾比較常見的方法是在視覺里程計(jì)中加入一個(gè)代價(jià)函數(shù)，用來降低移動(dòng)物體特征點(diǎn)的貢獻(xiàn)度從而降低其對于視覺里程計(jì)位姿估計(jì)的干擾。Gutierrez［11］等比較了不同的損失函數(shù)對于位姿估計(jì)的作用，Kerl［12～13］等證明了這種方法對于場景中出現(xiàn)小的移動(dòng)物體時(shí)的魯棒性；在ElasticFusion［14］和Keller［15］等的方法中，當(dāng)一個(gè)特征點(diǎn)在多幀連續(xù)圖像中都被觀測到才算入內(nèi)點(diǎn)。盡管他們從不同角度對算法進(jìn)行了改進(jìn)，然而這些方法都沒有對場景中移動(dòng)元素進(jìn)行檢測和處理，因此當(dāng)移動(dòng)物體在圖像中占比較大時(shí)，這些方法很難取得好的效果。另一種解決策略是檢測并過濾掉移動(dòng)物體。Choi 等［16］根據(jù)運(yùn)動(dòng)物體的深度變化不同于靜態(tài)特征這一現(xiàn)象，統(tǒng)計(jì)雙目圖像中特征的深度置信地圖來進(jìn)行運(yùn)動(dòng)物體檢測；文獻(xiàn)［17～18］將光流或場景流計(jì)算引入視覺里程計(jì)中，通過判斷特征的光流或場景流進(jìn)行運(yùn)動(dòng)物體檢測。雖然上述方法能夠剔除部分動(dòng)態(tài)特征，但是容易在靜態(tài)特征較少時(shí)，因用于位姿估計(jì)的匹配點(diǎn)對太少而造成位姿估計(jì)誤差大。

針對這些問題，本文提出了一種移動(dòng)物體干擾情況下的視覺里程計(jì)優(yōu)化方法。它通過特征匹配點(diǎn)對之間在圖像中移動(dòng)的距離，統(tǒng)計(jì)特征點(diǎn)移動(dòng)距離直方圖，并選取合適的閾值來區(qū)分靜態(tài)場景和移動(dòng)物體的特征點(diǎn)，從而保證了對圖像中存在多個(gè)運(yùn)動(dòng)物體的情況的魯棒性。同時(shí)，我們充分利用圖像所提取到的移動(dòng)物體上的特征點(diǎn)信息，依據(jù)移動(dòng)物體運(yùn)動(dòng)的平滑性約束，通過當(dāng)前圖像和上一時(shí)刻圖像中移動(dòng)物體的特征點(diǎn)位置預(yù)測出下一時(shí)刻移動(dòng)物體特征點(diǎn)的位置，在局部地圖中構(gòu)造下一時(shí)刻的虛擬特征點(diǎn)，最后，通過下一時(shí)刻圖像特征點(diǎn)與包含虛擬特征點(diǎn)的局部地圖的匹配關(guān)系進(jìn)行位姿估計(jì)。實(shí)際場景實(shí)驗(yàn)表明，本文提出的方法提高了移動(dòng)物體干擾下視覺里程計(jì)算法的精確性和魯棒性。

2 系統(tǒng)概述

本文視覺里程計(jì)的實(shí)現(xiàn)基于特征點(diǎn)的方法。特征點(diǎn)的提取和匹配是特征點(diǎn)法視覺里程計(jì)中尤為關(guān)鍵的一步。常用的特征點(diǎn)提取技術(shù)有SUSAN［19］，Harris［20］，SIFT［21］，SURF［22］等。Rublee 等在2011 年 提 出 了ORB［23］（Oriented FAST and Rotated BRIEF）算法，它使用改進(jìn)的FAST［24］算法來檢測特征點(diǎn)，有著極快的檢測速度，并且通過改進(jìn)FAST角點(diǎn)并結(jié)合描述子，取得了良好的效果。總地來說，ORB 是質(zhì)量與性能之間較好的折中。本文也將采用ORB作為我們的特征點(diǎn)提取算法。

本文的視覺里程計(jì)系統(tǒng)框架如圖1 所示，主要包括特征提取與位姿估計(jì)兩個(gè)部分。在特征提取階段，我們對每一幀F(xiàn)k提取ORB 特征點(diǎn)，并檢測和區(qū)分靜態(tài)場景和移動(dòng)物體特征點(diǎn)；在位姿估計(jì)階段，我們將當(dāng)前RGB-D 圖像幀提取到的特征點(diǎn)與局部地圖建立匹配關(guān)系，通過RANSAC PnP 估計(jì)相機(jī)位姿，然后使用g2o（General Graph Optimization）［26］通過最小化重投影誤差進(jìn)行位姿迭代優(yōu)化，最后更新局部地圖點(diǎn)以及虛擬特征點(diǎn)。

圖1 系統(tǒng)框架圖

3 基于幀間特征點(diǎn)移動(dòng)距離直方圖的移動(dòng)物體特征點(diǎn)檢測

針對場景中的移動(dòng)物體干擾問題，目前最為成熟、有效的魯棒估計(jì)方法就是隨機(jī)采樣一致性算法（Random Sample Consensus，RANSAC）。在視覺里程計(jì)，RANSAC 對于普通的圖像噪聲或者圖像中只存在小范圍的運(yùn)動(dòng)物體，能夠取得非常好的效果。但是如果攝像機(jī)所采集的圖像中運(yùn)動(dòng)物體占據(jù)了很大一部分，RANSAC 算法將會失效，影響最終的運(yùn)動(dòng)估計(jì)。本文針對這一問題，提出了一種基于幀間特征點(diǎn)移動(dòng)距離直方圖的移動(dòng)物體特征點(diǎn)檢測方法。

當(dāng)場景中有移動(dòng)物體干擾時(shí)，我們提取到的特征點(diǎn)會有一部分落在移動(dòng)物體上。我們對包含移動(dòng)物體干擾場景中的圖像進(jìn)行特征檢測，為了更加直觀地進(jìn)行展示，圖2 中給出了幾對圖像幀間特征點(diǎn)的檢測和匹配結(jié)果。

圖2 包含移動(dòng)物體場景下的特征點(diǎn)檢測及匹配結(jié)果

可以注意到，雖然相機(jī)自身的運(yùn)動(dòng)使靜態(tài)場景和移動(dòng)物體在圖像中的位置都是變化的，但是由于移動(dòng)物體自身的運(yùn)動(dòng)，使得靜態(tài)場景中的特征點(diǎn)和移動(dòng)物體上的特征點(diǎn)在圖像中的移動(dòng)距離是不同的，如圖3。根據(jù)這一現(xiàn)象，本文提出一種基于圖像平面特征點(diǎn)移動(dòng)距離直方圖的移動(dòng)物體特征點(diǎn)檢測方法。

太湖流域綜合規(guī)劃經(jīng)國務(wù)院批復(fù)，太湖流域水環(huán)境綜合治理總體方案水利部分修編完成，太湖流域及東南諸河水中長期供求規(guī)劃全面開展，流域水利規(guī)劃體系基本形成。在流域水利工作中，充分發(fā)揮規(guī)劃的指導(dǎo)約束作用，堅(jiān)持統(tǒng)籌協(xié)調(diào)，有力促進(jìn)了太湖流域治理與管理工作；加快推進(jìn)流域水環(huán)境綜合治理，完成太浦閘除險(xiǎn)加固主體工程建設(shè)，新溝河延伸拓浚工程、苕溪清水入湖河道整治工程項(xiàng)目可研已經(jīng)發(fā)改委批復(fù)，新孟河延伸拓浚工程可研報(bào)告已經(jīng)中咨公司評估，其他項(xiàng)目前期工作有序推進(jìn)。經(jīng)過有關(guān)各方的共同努力，2013年太湖流域水環(huán)境綜合治理成效明顯。監(jiān)測數(shù)據(jù)表明，太湖及河網(wǎng)水質(zhì)進(jìn)一步得到改善，太湖連續(xù)六年實(shí)現(xiàn)“安全度夏”。

圖3 圖像平面特征點(diǎn)移動(dòng)距離圖

對于連續(xù)兩幀有移動(dòng)物體干擾的圖像kt-1和kt，分別提取ORB特征點(diǎn)并進(jìn)行匹配。對于每一個(gè)kt中的特征點(diǎn)pt及其與kt-1中匹配的特征點(diǎn)pt-1，其在圖像kt和kt-1的位置坐標(biāo)分別為(ut,vt)和(ut-1,vt-1)，對圖像kt中每一個(gè)特征點(diǎn)pt計(jì)算其在圖像上移動(dòng)的距離d ：

對kt中每個(gè)特征點(diǎn)在圖像上移動(dòng)的距離統(tǒng)計(jì)直方圖，圖4 給出本文實(shí)驗(yàn)數(shù)據(jù)集中包含移動(dòng)物體圖像幀的統(tǒng)計(jì)結(jié)果。

圖4 特征點(diǎn)移動(dòng)距離統(tǒng)計(jì)直方圖

由圖4 特征點(diǎn)移動(dòng)距離統(tǒng)計(jì)結(jié)果可知，在包含移動(dòng)物體的場景中，圖像中靜態(tài)場景和移動(dòng)物體上的特征點(diǎn)的移動(dòng)距離存在差異性。我們將區(qū)分靜態(tài)背景和移動(dòng)物體的移動(dòng)距離閾值記為T ；對于當(dāng)前圖像所提取的特征點(diǎn)及其統(tǒng)計(jì)到的特征點(diǎn)移動(dòng)距離，將特征點(diǎn)總數(shù)量記為N ，屬于靜態(tài)背景的特征點(diǎn)數(shù)量記為N1，屬于移動(dòng)物體的特征點(diǎn)數(shù)量記為N2，那么：

其中ω1表示屬于靜態(tài)背景的特征點(diǎn)數(shù)量占所有提取到的特征點(diǎn)的數(shù)量比例；ω2表示屬于移動(dòng)物體的特征點(diǎn)數(shù)量占所有提取到的特征點(diǎn)的數(shù)量比例。

式中μ1表示靜態(tài)背景特征點(diǎn)的平均移動(dòng)距離；μ2表示移動(dòng)物體上特征點(diǎn)的平均移動(dòng)距離；μ 表示圖像中所有特征點(diǎn)的總平均移動(dòng)距離。

g 為計(jì)算出的靜態(tài)背景特征點(diǎn)與移動(dòng)物體特征點(diǎn)的類間方差。將式（3）帶入式（4）得到：

圖5 移動(dòng)物體特征點(diǎn)檢測結(jié)果

遍歷所有T 的可能取值，最終得到使得g 最大的T 作為最終閾值從而來區(qū)分靜態(tài)場景特征點(diǎn)和移動(dòng)物體上的特征點(diǎn)。圖5 中可以看到，我們的方法可以比較好地區(qū)分出靜態(tài)場景和移動(dòng)物體上的特征點(diǎn)。

4 基于虛擬特征點(diǎn)的相機(jī)位姿估計(jì)

4.1 虛擬特征點(diǎn)生成

對于靜態(tài)場景中的特征點(diǎn)，我們使用文獻(xiàn)［25］的方法構(gòu)造與維護(hù)局部地圖，并將當(dāng)前幀與地圖點(diǎn)進(jìn)行匹配，計(jì)算位姿。同時(shí)，本文中充分利用圖像所提取到的移動(dòng)物體上的特征點(diǎn)信息，利用其生成虛擬特征點(diǎn)加入局部地圖用于視覺里程計(jì)的位姿估計(jì)。

對于當(dāng)前t 時(shí)刻圖像幀所提取到的移動(dòng)物體上特征點(diǎn)集合M 中的每一個(gè)深度已知的特征點(diǎn)，其像素坐標(biāo)為(u,v)，深度為z，將其投影為局部地圖點(diǎn)Pt：

其中，Rcw和tcw分別為相機(jī)當(dāng)前坐標(biāo)系與世界坐標(biāo)系變換的旋轉(zhuǎn)矩陣和平移向量；(fx,fy)為相機(jī)焦距；(cx,cy)為光學(xué)投影中心坐標(biāo)。

我們假設(shè)行移動(dòng)物體的運(yùn)動(dòng)是一個(gè)連續(xù)性的過程，滿足物體運(yùn)動(dòng)平滑約束。于是我們在局部地圖中加入生成的虛擬特征點(diǎn)，用于下一時(shí)刻圖像特征點(diǎn)與局部地圖點(diǎn)的匹配。

圖6 虛擬特征點(diǎn)生成原理圖（L為局部地圖點(diǎn)集合，R為移動(dòng)物體區(qū)域特征點(diǎn)集合，V為根據(jù)（14）生成的虛擬特征點(diǎn)）

4.2 相機(jī)位姿估計(jì)

根據(jù)當(dāng)前幀圖像的特征點(diǎn)與包含虛擬特征點(diǎn)的局部地圖點(diǎn)的匹配關(guān)系，我們首先由RANSAC PnP 求得相機(jī)的位姿作為初值，然后通過最小化重投影誤差的迭代優(yōu)化方式估計(jì)相機(jī)位姿。考慮相機(jī)坐標(biāo)系下n 個(gè)三維空間點(diǎn)pi，其投影的像素坐標(biāo)為ui，像素位置與空間點(diǎn)位置的關(guān)系如下：

式中K 為相機(jī)內(nèi)參矩陣。

由于噪聲影響，式（9）存在誤差。因此，我們構(gòu)建最小二乘問題，最小化重投影誤差來獲得最終的相機(jī)位姿：

5 實(shí)驗(yàn)

5.1 實(shí)驗(yàn)環(huán)境和設(shè)置

本文實(shí)驗(yàn)所用電腦配置為：2.6GHz Intel Core i7-4720HQ CPU，內(nèi)存大小為8G，實(shí)驗(yàn)中未使用GPU 加速，算法運(yùn)行系統(tǒng)為Ubuntu 16.04。我們使用包含移動(dòng)物體干擾的數(shù)據(jù)集對本文提出的動(dòng)態(tài)場景下視覺里程計(jì)優(yōu)化方法進(jìn)行評估，同時(shí)，與同等條件下不運(yùn)行本文提出的移動(dòng)物體處理算法進(jìn)行對比，來檢驗(yàn)本文提出的視覺里程計(jì)方法在動(dòng)態(tài)場景下的有效性。此外，我們將本文提出的方法與特征點(diǎn)法中具有代表性的ORB-SLAM［25］進(jìn)行實(shí)驗(yàn)對比。由于ORB-SLAM是一個(gè)完整的SLAM系統(tǒng)，公平起見，我們關(guān)掉了ORB-SLAM 的回環(huán)檢測和全局優(yōu)化，只保留其視覺里程計(jì)部分（本文中稱為ORB-VO）與本文方法進(jìn)行實(shí)驗(yàn)對比。

5.2 數(shù)據(jù)集與評測指標(biāo)

TUM［27］數(shù)據(jù)集是慕尼黑工業(yè)大學(xué)實(shí)驗(yàn)場景數(shù)據(jù)集，其中fr3 系列數(shù)據(jù)集是一個(gè)典型的動(dòng)態(tài)場景數(shù)據(jù)集，提供了一系列存在不同程度移動(dòng)物體干擾的室內(nèi)RGB-D 數(shù)據(jù)集，經(jīng)常用來評估視覺里程計(jì)算法在移動(dòng)物體干擾條件下的精確性和魯棒性。本文中使用TUM fr3 系列數(shù)據(jù)集來評估本文提出的方法在動(dòng)態(tài)場景中的表現(xiàn)。

本文使用相對位姿誤差（RPE）作為本實(shí)驗(yàn)的評估準(zhǔn)則。相對位姿誤差（RPE）用來衡量在一定時(shí)間間隔Δ 內(nèi)估計(jì)軌跡的局部精度。因此，相對位姿誤差（RPE）可以用來衡量軌跡漂移的程度。在i 時(shí)刻的相對位姿誤差定義為其中Qi和Pi分別為i 時(shí)刻的真實(shí)位姿和估計(jì)位姿。另外，我們進(jìn)一步計(jì)算估計(jì)位姿平移RPE和旋轉(zhuǎn)RPE 的均方誤差（RMSE）來評估本文提出的視覺里程計(jì)優(yōu)化方法的表現(xiàn)。實(shí)驗(yàn)中Δ=1。

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

表1 中給出walking_static 據(jù)集下的實(shí)驗(yàn)結(jié)果。我們將同等條件下運(yùn)行本文提出的優(yōu)化方法與不運(yùn)行優(yōu)化算法的結(jié)果進(jìn)行對比，來檢驗(yàn)本文提出的視覺里程計(jì)方法的精確性和魯棒性。

表1 RMSE對比

圖7 給出了兩種方法在當(dāng)前移動(dòng)物體干擾場景數(shù)據(jù)集下的平移和旋轉(zhuǎn)的相對位姿誤差。

圖7 相對位姿誤差對比圖

圖7 中（a）、（b）可以看到，由于場景中人體移動(dòng)的影響，如果不對場景中的移動(dòng)物體進(jìn)行處理，會導(dǎo)致位姿估計(jì)出現(xiàn)較大的偏差；加入本文提出的優(yōu)化方法后，如圖7 中（c）、（d），本文的方法可以有效地針對場景中存在的移動(dòng)物體干擾，仍保持較為準(zhǔn)確的位姿估計(jì)。這是由于本文的方法中考慮了場景中移動(dòng)物體上的特征點(diǎn)，避免了這些特征點(diǎn)錯(cuò)誤的與靜態(tài)特征點(diǎn)匹配，而是通過這些特征點(diǎn)生成虛擬特征點(diǎn)與局部地圖匹配，從而減少了場景中移動(dòng)物體對于視里程計(jì)算法的錯(cuò)誤影響。

與ORB-VO 的對比實(shí)驗(yàn)結(jié)果如表2 所示（加粗部分為誤差較小者）。在低程度的動(dòng)態(tài)場景中（表2 中前四個(gè)數(shù)據(jù)集），本文提出的方法與ORB-VO的位姿估計(jì)誤差相當(dāng)。而當(dāng)場景中移動(dòng)物體運(yùn)動(dòng)幅度較大且在圖像中占比較高時(shí)（表2 中后三個(gè)數(shù)據(jù)集），本文提出的方法位姿估計(jì)效果明顯優(yōu)于ORB-VO。

表2 fr3各數(shù)據(jù)集RMSE對比

從以上實(shí)驗(yàn)中可以看到，本文提出的算法針對場景中存在移動(dòng)物體干擾的情況下，很好地克服了移動(dòng)物體對于視覺里程計(jì)算法的影響，且能保持較強(qiáng)的魯棒性。

6 結(jié)語

如何在動(dòng)態(tài)環(huán)境下仍然保持比較準(zhǔn)確的定位和建圖精度，是現(xiàn)有的眾多SLAM 和視覺里程計(jì)方法的挑戰(zhàn)之一。本文針對這一問題提出了一種基于虛擬特征點(diǎn)的視覺里程計(jì)運(yùn)動(dòng)物體干擾消除方法。通過對靜態(tài)場景和運(yùn)動(dòng)物體上的特征點(diǎn)加以區(qū)分，并生成虛擬特征點(diǎn)加入局部地圖用于位姿估計(jì)的方法，旨在提高動(dòng)態(tài)場景下視覺里程計(jì)算法的定位和位姿估計(jì)的精確性和魯棒性。對比實(shí)驗(yàn)表明，本文提出的動(dòng)態(tài)場景下視覺里程計(jì)優(yōu)化方法針對動(dòng)態(tài)場景下存在移動(dòng)物體干擾情況下的相機(jī)位姿估計(jì)有較大提升。