結合光流跟蹤和三焦點張量約束的雙目視覺里程計

程傳奇，郝向陽，張振杰，趙漫丹

（1. 信息工程大學 導航與空天目標工程學院，鄭州 450001；2. 北斗導航應用技術河南省協同創新中心，鄭州 450001）

結合光流跟蹤和三焦點張量約束的雙目視覺里程計

程傳奇1,2，郝向陽1,2，張振杰1,2，趙漫丹1,2

（1. 信息工程大學 導航與空天目標工程學院，鄭州 450001；2. 北斗導航應用技術河南省協同創新中心，鄭州 450001）

針對城市街道準確實時定位的問題，提出將光流跟蹤與三焦點張量約束結合的雙目視覺里程計方法。為提高運算效率，將圖像序列分為關鍵幀與非關鍵幀，對關鍵幀進行常規的特征點檢測與匹配，對非關鍵幀用Lucas-Kanade光流跟蹤特征點對。推導了基于前后幀、左右視圖三焦點張量約束的觀測方程，顧及動力學方程，組成卡爾曼濾波模型。考慮到觀測方程的非線性，采用迭代Sigma點卡爾曼濾波進行解算，解算過程中用 RANSAC穩健估計策略提純匹配，以增強系統整體穩健性。實驗結果表明：提出的算法在基本不損失精度（X方向優于5 m，Y方向優于4 m）的情況下，計算速度提升6.2倍，單幀圖像平均處理時間由0.3115 s下降為0.0503 s，能夠滿足城市定位實時準確的需求。

雙目視覺里程計；光流；三焦點張量；迭代Sigma點卡爾曼濾波

視覺里程計（VO）是視覺導航的一種特殊形式，由于它僅利用攝像機的輸入信息來估計載體的運動狀態[1]，近十幾年來已廣泛應用于室內/外陸地機器人[2]、水下機器人[3]及無人飛行器[4]等各類機器人自主導航定位中。

因VO導航高度自主及其低成本性，吸引了相關學者和機構的關注。VO分為單目視覺里程計和雙目視覺里程計兩種。在單目視覺里程計方面：Forster[5]提出了一種快速半直接單目視覺里程計（Semi-Direct Monocular Visual Odometry，SVO）算法，利用雙線程分別進行運動估計和地圖構建，提高了運行速度；馮國虎[6]提出了慣性傳感器與單目視覺里程計的組合導航算法，克服了單純由視覺估計相機姿態精度低造成的長距離導航誤差大的問題；Bachrach[7]和 Endres[8]利用 RGB-D相機搭建了運動估計、規劃和構圖的系統。但單目視覺由于缺少絕對尺度，在模型求解過程中，會出現尺度因子的模糊性，需要一定的約束和假設。

對于雙目視覺[9]，其固定基線便可進行視差圖的精確解算，可解決單目視覺中的尺度二義性。在雙目視覺里程計方面：Geiger[10]提出了一種基于立體像對實時三維重建算法，可實時估計攝像機位姿信息并恢復三維場景信息；Holzmann[11]設計了基于線特征的雙目視覺里程計，用IMU輔助檢測直線，實現了紋理不明顯環境內的定位。但以上算法在定位時都需要恢復場景三維信息，影響了實時性。Kitt[12]等人設計了一種基于隨機抽樣一致性（RANSAC）提純策略的雙目視覺里程計，通過計算圖像序列之間三焦點張量，無需進行三維重建即可進行運動估計，但運行效率較低，難以實現實時定位。目前，光流對圖像運動的估計已經可以滿足一定的精度要求[13]，因其運算速度快，基于光流法的視覺里程計也有研究[14-15]。文獻[16]提出把基于光流估計的VO與基于特征匹配的VO進行融合，取得了不錯的效果，但基于光流的運動估計精度較低，勢必會影響最終定位結果。

本文提出一種結合光流跟蹤和三焦點張量約束的雙目視覺里程計算法，以文獻[12]算法為基礎，利用光流跟蹤同名點對，基于前后幀、左右視圖之間的三焦點張量約束，通過卡爾曼濾波進行運動估計，以期在保證精度的同時提高算法實時性。

1 基本原理

1.1 攝像機模型

攝像機坐標XC與圖像坐標x～的投影關系可表示為：

攝像機坐標系與世界坐標系之間的關系可由一個旋轉矩陣R和平移向量t（外參數）來表示：

結合式(1)和(2)，可得三維點到圖像坐標系的投影關系：

式中，矩陣P稱為攝像機投影矩陣。

1.2 三視圖幾何關系

計算機視覺中，使用三焦點張量T(3×3×3)描述三視圖幾何關系，它僅依賴于視圖間的運動和攝像機內參數，且可由這些視圖的攝像機矩陣唯一確定，而與景物結構無關[17]。

已知三臺攝像機的投影矩陣分別表示為：

那么三焦點張量元素可由式(4)計算：

式中：～ai表示PA去掉第i行；bq表示PB的第q行；cr表示PC的第r行。

圖1 三視圖對應點的關系Fig.1 Relationship between corresponding points in three images

這里我們利用三焦點張量把圖像A、B中的對應特征點xA?xB映射到第三幅圖像C中，如圖1所示：把圖像中通過點xB的任意一條直線lB投影到三維空間中。給定lB和三焦點張量T，那么有：

1.3 光流跟蹤

光流場反映圖像灰度模式的表面運動，據此可對序列圖像的特征點進行跟蹤。光流跟蹤的一個基本假設是：序列圖像的采樣時間間隔很小，鄰幀圖像同名點的灰度值保持不變。設t時刻的光流圖像為f(x,y,t)，則經過時間間隔Δt，圖像點坐標(x,y)運動至(x+Δx,y+Δy)處。根據以上假設，有：

把式(6)進行泰勒展開，得到：

式(6)(7)聯立，可得：

式(8)兩邊同時除以Δt，并使Δt→0求極限，得：

2 問題描述

視覺里程計問題就是如何由車載攝像機拍攝的連續影像，通過計算機視覺的方法，實時解算車輛的空間位置。由于攝像機固連在車輛上，所以視覺里程計實質就是對車載攝像機的實時位姿估計。

雙目視覺里程計就是通過相鄰時刻的雙目立體像對估計攝像機運動參數。圖2表示相鄰時刻雙目攝像機結構的示意圖，它描述了像平面、攝像機坐標系以及攝像機相對于前一時刻右攝像機的位姿關系。通過事先標定，容易獲取前一時刻左攝像機的相對位姿通過外參數標定和運動估計可分別獲取運動參數這就是雙目視覺里程計要研究的本質問題。

圖2 相鄰時刻雙目攝像機結構的示意圖Fig.2 Configuration of stereo cameras at adjacent times

3 算法實現

3.1 運動狀態參數化

式中，Vi和ωi(i=X,Y,Z)分別表示運動速度和角速度，ΔT表示鄰幀間隔時間。

3.2 三焦點張量約束的觀測模型

圖2中描述了攝像機投影矩陣P，不失一般性，令前一時刻右攝像機的坐標系與世界坐標系對準，即令PR,k=KR·[I|0]，那么已知旋轉矩陣和平移向量后，投影矩陣由式(13)～(15)計算：

利用投影矩陣P，可計算出兩個三焦點張量：TR表征當前時刻右像與前一時刻像對的關系；TL表征當前時刻左像與前一時刻像對的關系。考慮式(4)，可得：

式中，TR和TL由相機內參數以及雙目相機的運動參數決定。把式(16)和(17)代入三焦點張量點轉移關系，即公式(5)，得到：

式中，x表示對應像點，H表示非線性函數。這就是雙目視覺里程計的觀測方程。

3.3 卡爾曼濾波最優估計

為更好地利用載體運動的動力學模型，采用卡爾曼濾波進行最優估計。狀態參數為待估計的運動參數y=(VX,VY,VZ,ωX,ωY,ωZ)，觀測方程由式(18)和(19)組成，故卡爾曼濾波模型可寫為：

式中： f(·)為非線性狀態轉移函數；h(·)表示式(18)和(19)組成的非線性量測函數；z=(u,…,v)T表示4N維的觀測向量；

k+1R,k+1,1L,k+1,N wk～N(0,Qk)表示系統誤差，Qk為6×6維的系統誤差協方差矩陣；vk+1～N(0,Rk+1)為觀測噪聲，Rk+1為4N×4N維的測量誤差協方差矩陣，N為濾波使用的特征點對數。

求解上述模型，需要進行線性化。由于攝像機采樣頻率較高，一般情況下，認為相鄰時刻載體保持勻速運動，故狀態方程可寫為yk+1=yk+wk。量測方程一般采用泰勒公式進行線性化，這種方式稱為擴展卡爾曼濾波（Extended Kalman Filter，EKF）。EKF采用一階泰勒展開，模型強非線性時效果不好。為減小逼近誤差，狀態更新階段采用迭代方式進行，稱為迭代擴展卡爾曼濾波[18]（Iterated Extended Kalman Filter，IEKF）。還有一種方式用基于構造Sigma點的UT變換進行逼近，稱為無跡卡爾曼濾波（Unscented Kalman Filter，UKF），也有其迭代方式，稱為迭代 Sigma點卡爾曼濾波[19-20]（Iterated Sigma Point Kalman Filter，ISPKF）。考慮精度與實時性，本文采用ISPKF進行狀態估計，具體實施方案可參閱文獻[19]。

3.4 特征的提取與匹配

在特征點的提取與匹配階段，把序列影像分為兩類：關鍵幀和非關鍵幀。關鍵幀的特征提取與匹配采用FAST算子和歸一化互相關算法實現；非關鍵幀的特征匹配則使用 1.3節的光流跟蹤算法，文中采用Lucas-Kanade光流算法進行求解。視覺里程計中最耗時的一個步驟就是特征匹配，而光流跟蹤可解決該問題。

3.5 RANSAC特征提純策略

無論是關鍵幀的特征提取與匹配還是光流跟蹤的同名點，都不可避免存在誤匹配點，若不加判斷的采用所有匹配點進行濾波運算，會影響解算結果，嚴重時會引起發散。這里采用采用隨機抽樣一致（RANSAC）算法設計穩健估計策略。具體實現步驟如下：

1）從同名點對中隨機選取S個點對作為抽樣子集，因運動估計算法中至少需要3個點，故S≥3；

2）基于該子集，利用3.3節的ISPKF估計運動參數；

3）基于運動參數計算所有同名點對的重投影誤差（這里采用歐氏距離），若誤差大于設定閾值，認為該點為外點，反之，為內點；

4）利用所有內點基于ISPKF重新估計運動參數。

3.6 算法實現流程

本文設計的視覺里程計流程如圖3所示，具體包括以下步驟：

Step1：車輛運動中實時采集圖像序列。當前圖像為關鍵幀時，進行特征點提取與匹配；當前圖像為非關鍵幀時，進行特征點光流跟蹤，進而按照 Lucas-Kanade光流算法進行同名點定位。

Step2：獲取同名點后，即可得到系統觀測模型，即式(21)。

Step3：聯合系統狀態模型，利用ISPKF進行解算，獲取狀態最優估計。若當前圖像為關鍵幀，則直接進行位姿更新；若當前圖像為非關鍵幀（圖3中粗實線部分），則利用當前獲取位姿進行內點率判定，若通過，則更新位姿，若不通過，則把當前圖像當作關鍵幀重新進行運算。

若未能獲取足夠多的內點，則無法構建量測方程，此時直接進行狀態預報更新位姿，保證系統整體的穩健性。

圖3 視覺里程計設計原理圖Fig.3 Flow chart of visual odometry

4 實驗與分析

4.1 實驗數據介紹

搭建實驗平臺（搭載Flea2 Firewire相機和OXTS RT 3000 IMU/GPS組合測量系統的車輛），在城市中進行跑車實驗。采集的數據包括Flea2 Firewire相機采集的高質量立體圖像序列，以及 OXTS RT 3000 IMU/GPS測量系統采集的地理數據（把它作為真值）。預先在標定場進行相機標定。文中實驗數據集具體參數見表1。

如圖4所示，圖4(a)(b)(c)為連續三幀影像（篇幅所限，這里只顯示了左相機采集影像），圖4(c)中黑色線條為光流跟蹤的特征點效果。光流場正確地反映了運動場，圖中黑色線條方向主要為徑向，說明車輛在前行。

表1 實驗數據集參數Tab.1 Parameters of datasets

圖4 圖像序列以及光流跟蹤效果顯示Fig.4 Image sequences and effect of optical-flow tracking

4.2 算法性能分析

為驗證本文算法的有效性，把本文算法與 Bernd Kitt等提出的視覺里程計算法（以下簡稱Kitt算法）進行對比分析，具體解算結果如圖(5)～(8)所示。本文中所有實驗都在MATLAB 2012B中進行，實驗環境為Lenovo E530筆記本電腦（酷睿i5-3210CPU，主頻2.50 GHz，4G內存）。

圖5為本文算法、Kitt算法解算軌跡與IMU/GPS測量軌跡對比圖。相機中心與IMU/GPS中心不重合，相對于 IMU/GPS，相機在天向、前向、左側分別有1.8 m、0.58 m、0.12 m 的偏移。另外，相機相對于IMU/GPS的俯仰角為-3.7°。這里進行對比時，已經把兩種數據對準，可以看出，兩種算法解算的軌跡都與真值吻合，驗證了算法的合理性。

圖5（a） 解算軌跡（數據集1）Fig.5(a) Results of the proposed method and Kitt’s method (Dataset 1)

圖5（b） 解算軌跡（數據集2）Fig.5(b) Results of the proposed method and Kitt’s method (Dataset 2)

圖6為兩種算法解算X、Y方向誤差，可以看出，兩種算法在兩個方向上誤差都是越來越大，但整體誤差在可接受范圍之內。這是由于視覺里程計每次解算出相機在采集鄰幀圖像時刻的相對位姿，然后根據相對位姿進行航跡推算，這種導航方式不可避免會產生誤差累積效應。另外，數據是在城市中采集的，不可避免地有遮擋等因素引起GPS失效，故OXTS RT 3000 IMU/GPS測量系統的精度并沒有達到標稱精度（優于0.05 m）。這里把它作為真值，主要是比較兩種算法的相對優劣。

圖 7為本文算法與 Kitt算法解算結果的相對誤差。容易看出，本文算法精度與Kitt算法相當，數據集1和數據集2的解算結果X方向誤差在5 m以內，Y方向誤差在4 m以內。

綜上所述，實驗結果可以驗證本文算法運動估計的可靠性與正確性。

圖6（a） 解算誤差（與IMU/GPS數據對比）（數據集1）Fig.6(a) Errors of the proposed method and Kitt’s method with respect to IMU/GPS (Dataset 1)

圖7（a） 本文算法誤差（與Kitt算法相比）（數據集1）Fig.7(a) Errors of the proposed method with respect to Kitt’s method (Dataset 1)

圖8（a） 算法運行時間與匹配內點數目（數據集1）Fig.8(a) Runtime and number of inliers of the proposed method and Kitt’s method (Dataset 1)

圖6（b） 解算誤差（與IMU/GPS數據對比）（數據集2）Fig.6(b) Errors of the proposed method and Kitt’s method with respect to IMU/GPS (Dataset 2)

圖7（b） 本文算法誤差（與Kitt算法相比）（數據集2）Fig.7(b) Errors of the proposed method with respect to Kitt’s method (Dataset 2)

圖8（b） 算法運行時間與匹配內點數目（數據集2）Fig.8(b) Runtime and number of inliers of the proposed method and Kitt’s method (Dataset 2)

圖8為算法運算時間與匹配內點數對比。容易看出，雖然本文算法匹配內點數有所減少（平均內點數由296減少為122），但經過上述分析，解算精度并未損失多少（X方向優于5 m，Y方向優于4 m）；同時可看出，本文算法運行效率顯著提升，Kitt算法單幀影像平均處理時間為0.3115 s，本文算法單幀影像平均處理時間為0.0503 s，速度提升了6.2倍，且在消費級筆記本電腦配置下實現了實時導航定位，充分說明了算法在實時性方面的優越性，能夠滿足車輛在城市街道實時準確定位的需求。

5 結 語

本文提出了一種結合光流跟蹤和三焦點張量約束的雙目視覺里程計算法以提高車輛城市導航的精度和實時性，推導了基于三焦點張量約束的圖像序列觀測方程，結合載體動力學模型，建立了雙目視覺里程計的 ISPKF運動參數估計方法。在運動估計過程中用光流跟蹤策略定位圖像同名點，并以 RANSAC穩健估計策略提純同名點，城市實測實驗數據驗證了算法的優良性能。但視覺里程計航跡推算導航的本質使得定位誤差會累積，因此與其他傳感器的組合導航就顯得尤為重要，下一步將開展組合模式的視覺里程計算法研究。

（References）：

[1] Yousif K, Bab-Hadiashar A, Hoseinnezhad R. An overview to visual odometry and visual SLAM: Applications to mobile robotics[J]. Intelligent Industrial Systems, 2015, 1(4): 1-23.

[2] Konolige K, Agrawal M, Solà J. Large-scale visual odometry for rough terrain[J]. International Journal of Robotics Research, 2011, 66: 201-212.

[3] Wongsuwan K, Sukvichai K. Visual odometry estimation for an underwater robot navigation[C]//International Technical Conference on Circuits Systems, Computers and Communications. 2015: 496-499.

[4] Annaiyan A, Yadav M, Olivares-Mendez M A, et al. Visual odometry based absolute target geo-location from micro aerial vehicle[C]//IEEE International Conference on Robotics, Automation, Control and Embedded Systems. 2015: 1-7.

[5] Forster C, Pizzoli M, Scaramuzza D. SVO: Fast semidirect monocular visual odometry[C]//IEEE International Conference on Robotics & Automation. 2014: 15-22.

[6] 馮國虎, 吳文啟, 曹聚亮, 等. 單目視覺里程計/慣性組合導航算法[J]. 中國慣性技術學報, 2011, 19(3): 302-306. Feng Guo-hu, Wu Wen-qi, Cao Ju-liang, et al. Algorithm for monocular visual Odometry/SINS integrated navigation[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 302-306.

[7] Bachrach A, Prentice S, He R, et al. Estimation, planning, and mapping for autonomous flight using an RGB-D camera in GPS-denied environments[J]. International Journal of Robotics Research, 2012, 31(11): 1320-1343.

[8] Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2014, 30(1): 177-187.

[9] 李宇波, 朱效洲, 盧惠民, 等. 視覺里程計技術綜述[J].計算機應用研究, 2012, 29(8): 2801-2805. Li Yu-bo, Zhu Xiao-zhou, Lu Hui-min, et al. Review on visual odometry technology[J]. Application Reasearch of Computers, 2012, 29(8): 2801-2805.

[10] Geiger A, Ziegler J, Stiller C. StereoScan: Dense 3d reconstruction in real-time[J]. IEEE Intelligent Vehicles Symposium, 2011, 32(14): 963-968.

[11] Holzmann T, Fraundorfer F, Bischof H. Direct stereo visual odometry based on lines[C]//International Conference on Computer Vision Theory and Applications. 2016.

[12] Kitt B, Geiger A, Lategahn H. Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme[C]//IEEE Intelligent Vehicles Symposium. 2010: 486-492.

[13] Sun D, Roth S, Black M J. Secrets of optical flow estimation and their principles[C]//IEEE Computer Society Conference on Computer Vision and Pattern Recognition. 2010: 2432-2439.

[14] Campbell J, Sukthankar R, Nourbakhsh I. Visual odometry using commodity optical flow[J]. Proceedings of the American Association of Artificial Intelligence (AAAI), 2004: 1008-1009.

[15] Wang C, Zhao C, Yang J. Monocular odometry in country roads based on phase-derived optical flow and 4-DOF ego-motion model[J]. Industrial Robot, 2011, 38(5): 509-520.

[16] 鄭馳, 項志宇, 劉濟林. 融合光流與特征點匹配的單目視覺里程計[J]. 浙江大學學報(工學版), 2014, 42(2): 279-284. Zheng Chi, Xiang Zhi-yu, Liu Ji-lin. Monocular vision odometry based on the fusion of optical flow and feature points matching[J]. Journal of Zhejiang University (Engineering Science), 2014, 42(2): 279-284.

[17] Hartley R, Zisserman A. Multiple view geometry in computer vision[M]. 2nd ed. Cambridge University Press, 2008.

[18] Shojaei K, Shahri A M. Experimental study of iterated Kalman filters for simultaneous localization and mapping of autonomous mobile robots[J]. Journal of Intelligent & Robotic Systems, 2011, 63(3-4): 575-594.

[19] Sibley G, Sukhatme G, Matthies L. The iterated sigma point Kalman filter with applications to long range stereo [C]//Conference: Robotics: Science and Systems, 2006.

[20] Huang Y, Zhang Y, Li N, et al. Design of sigma-point Kalman filter with recursive updated measurement[J]. Circuits Systems & Signal Processing, 2015: 1-16.

Stereo visual odometry based on Kalman fusion of optical flow tracking and trifocal tensor constraint

CHENG Chuan-qi1,2, HAO Xiang-yang1,2, ZHANG Zhen-jie1,2, ZHAO Man-dan1,2
(1. School of Navigation and Aerospace Engineering, Information Engineering University, Zhengzhou 450001, China; 2. Beidou Navigation Technology Collaborative Innovation Center of Henan, Zhengzhou 450001, China)

For the problem of real-time precise localization in the urban street, a stereo visual odometry based on Kalman fusion of optical-flow tracking and trifocal tensor constraint is proposed. To speed up the algorithm, the image sequences are classified into key frames and non-key frames. A conventional featurepoint detection and matching method is utilized to process the key frames, and the Lucas Kanade optical flow method is utilized to track the correspondences in non-key frames. The mathematical model of observation based on trifocal tensor constraint among image triples is derived, which is combined with dynamic equation to form the Kalman filter model. An iterated sigma-point Kalman filter is employed to cope with the nonlinear system. To improve the robustness of the visual odometry, a RANSAC-based method is applied into the pure matching points in motion estimation. Experimental results demonstrate that, compared with Kitt’s method, the proposed method can acquire the same accuracy with the speed increasing 6.2 times, i.e., the error in the X direction and Y direction is 5 m and 4 m, respectively, while the average runtime of single frame decreases from 0.3115 s to 0.0503 s, which meets the needs of real-time accurate localization in cities.

stereo visual odometry; optical flow; trifocal tensor; iterated sigma-point Kalman filter

V448.2

：A

2016-04-18；

：2016-07-08

國家863計劃資助項目（2015AA7034057A）；國家自然科學基金項目（61173077）

程傳奇（1989—），男，博士生，主要研究導航定位與位置服務。E-mail: legend3q@163.com

1005-6734(2016)04-0473-07

10.13695/j.cnki.12-1222/o3.2016.04.010