基于虛擬特征點的視覺里程計運動物體干擾消除方法?

閆 銘 王 歡 崔瓊杰

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

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

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

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

2 系統概述

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

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

圖1 系統框架圖

3 基于幀間特征點移動距離直方圖的移動物體特征點檢測

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

當場景中有移動物體干擾時，我們提取到的特征點會有一部分落在移動物體上。我們對包含移動物體干擾場景中的圖像進行特征檢測，為了更加直觀地進行展示，圖2 中給出了幾對圖像幀間特征點的檢測和匹配結果。

圖2 包含移動物體場景下的特征點檢測及匹配結果

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

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

圖3 圖像平面特征點移動距離圖

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

對kt中每個特征點在圖像上移動的距離統計直方圖，圖4 給出本文實驗數據集中包含移動物體圖像幀的統計結果。

圖4 特征點移動距離統計直方圖

由圖4 特征點移動距離統計結果可知，在包含移動物體的場景中，圖像中靜態場景和移動物體上的特征點的移動距離存在差異性。我們將區分靜態背景和移動物體的移動距離閾值記為T ；對于當前圖像所提取的特征點及其統計到的特征點移動距離，將特征點總數量記為N ，屬于靜態背景的特征點數量記為N1，屬于移動物體的特征點數量記為N2，那么：

其中ω1表示屬于靜態背景的特征點數量占所有提取到的特征點的數量比例；ω2表示屬于移動物體的特征點數量占所有提取到的特征點的數量比例。

式中μ1表示靜態背景特征點的平均移動距離；μ2表示移動物體上特征點的平均移動距離；μ 表示圖像中所有特征點的總平均移動距離。

g 為計算出的靜態背景特征點與移動物體特征點的類間方差。將式（3）帶入式（4）得到：

圖5 移動物體特征點檢測結果

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

4 基于虛擬特征點的相機位姿估計

4.1 虛擬特征點生成

對于靜態場景中的特征點，我們使用文獻［25］的方法構造與維護局部地圖，并將當前幀與地圖點進行匹配，計算位姿。同時，本文中充分利用圖像所提取到的移動物體上的特征點信息，利用其生成虛擬特征點加入局部地圖用于視覺里程計的位姿估計。

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

其中，Rcw和tcw分別為相機當前坐標系與世界坐標系變換的旋轉矩陣和平移向量；(fx,fy)為相機焦距；(cx,cy)為光學投影中心坐標。

我們假設行移動物體的運動是一個連續性的過程，滿足物體運動平滑約束。于是我們在局部地圖中加入生成的虛擬特征點，用于下一時刻圖像特征點與局部地圖點的匹配。

圖6 虛擬特征點生成原理圖（L為局部地圖點集合，R為移動物體區域特征點集合，V為根據（14）生成的虛擬特征點）

4.2 相機位姿估計

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

式中K 為相機內參矩陣。

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

5 實驗

5.1 實驗環境和設置

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

5.2 數據集與評測指標

TUM［27］數據集是慕尼黑工業大學實驗場景數據集，其中fr3 系列數據集是一個典型的動態場景數據集，提供了一系列存在不同程度移動物體干擾的室內RGB-D 數據集，經常用來評估視覺里程計算法在移動物體干擾條件下的精確性和魯棒性。本文中使用TUM fr3 系列數據集來評估本文提出的方法在動態場景中的表現。

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

5.3 實驗結果分析

表1 中給出walking_static 據集下的實驗結果。我們將同等條件下運行本文提出的優化方法與不運行優化算法的結果進行對比，來檢驗本文提出的視覺里程計方法的精確性和魯棒性。

表1 RMSE對比

圖7 給出了兩種方法在當前移動物體干擾場景數據集下的平移和旋轉的相對位姿誤差。

圖7 相對位姿誤差對比圖

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

與ORB-VO 的對比實驗結果如表2 所示（加粗部分為誤差較小者）。在低程度的動態場景中（表2 中前四個數據集），本文提出的方法與ORB-VO的位姿估計誤差相當。而當場景中移動物體運動幅度較大且在圖像中占比較高時（表2 中后三個數據集），本文提出的方法位姿估計效果明顯優于ORB-VO。

表2 fr3各數據集RMSE對比

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

6 結語

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