基于局部與全局優化的雙目視覺里程計算法

,,

(1.中國科學院上海微系統與信息技術研究所,上海 200050; 2.中國科學院大學,北京 100049)

0 概述

實時精確的自我定位是移動機器人實現自動導航的關鍵。視覺里程計[1]利用一個或多個攝像機獲取圖像序列并計算移動機器人的相對運動位姿,是一種相對定位的方法。與傳統的輪式里程計相比,視覺里程計不受車輪打滑或路面不平等環境限制,且獲取的視覺信息可用于避障和目標識別等視覺應用,具有更高的魯棒性和更廣的應用場景。

早期針對視覺里程計的研究都服務于星球探測車。文獻[2]采用單目相機設計了一種“滑動立體”的裝置,每當機器人停下時,相機水平滑行并等距拍攝9幀圖像,然后通過三角測量法重建特征點的三維坐標,并使用這些三維點求解運動信息。在文獻[2]研究的基礎上,文獻[3]使用雙目立體視覺系統,并把重建的三維點的協方差矩陣用于運動估計,一定程度上提高了精度。文獻[4]正式提出了Visual Odometry(VO),實現了魯棒去除外點的實時VO系統,并首次在數百米距離上對VO算法進行了測試。后期VO的總體框架基本依照文獻[4]方法,通過不斷改進特征提取算法、特征匹配和跟蹤算法、運動估計算法等,來獲取更準確、快速、魯棒的定位結果。視覺里程計最成功的應用是火星探測機器人“勇氣號”和“機遇號”[5],它們采用的視覺里程計系統在火星上出色地完成了任務,為視覺里程計的發展帶來極大動力。近年來視覺里程計開始被廣泛應用在室內外導航、自動駕駛、增強現實(Augmented Reality,AR)等領域。

根據所用的傳感器不同,視覺里程計可以分為單目、雙目和RGBD。單目視覺里程計缺少尺度信息,不能獲得絕對的位置信息;RGBD視覺里程計受深度相機的影響較大,深度誤差和有效距離限制導致其自我定位精度較低;雙目視覺里程計能夠通過左右相機立體匹配獲取深度信息,恢復機器人的絕對的位置信息并且精度較高。

按照深度信息重建的方式,雙目視覺里程計又可以分為基于特征的方法[6]和直接法[7]。基于特征的方法通過提取圖像的特征點進行特征匹配和稀疏三維重建,實時性較好;直接法對圖像中的所有像素進行匹配,得到稠密的三維重建點云,很難保證實時性。因此,本文從實時性的角度考慮,設計基于特征點的雙目視覺里程計。

視覺里程計的幀間運動估計存在誤差,并且這種誤差會累積,如何減小這種誤差提高定位精度成為視覺里程計的研究重點,可以根據是否引入概率模型將其分為基于概率的方法和基于非概率的方法?；诟怕实姆椒ㄖ饕谢跀U展卡爾曼濾波器(Extended Kalman Filter,EKF)[8]的方式和基于粒子濾波器(Particle Filter,PF)[9]的方式等?；诟怕实姆椒ㄊ茉肼暦植加绊懘?會帶來非線性誤差,在大規模場景下很難保證精度和魯棒性?；诜歉怕实姆椒ū容^著名是基于關鍵幀的單目即時定位與地圖構建算法PATM[10],該算法是非概率方法的基本框架。

本文設計一種基于特征點的雙目視覺里程計算法,根據關鍵幀的關系,針對幀間運動估計進行局部優化,并采用回環檢測對累積誤差進行全局優化。

1 基于特征點的雙目視覺里程計

基于特征點的雙目視覺里程計首先在雙目相機獲取的左右圖像上進行特征點匹配,通過匹配結果計算特征點深度信息;再根據前后幀的特征點跟蹤進行運動估計求解當前幀的位姿,包括旋轉矩陣R和平移矩陣T。

1.1 雙目相機模型

相機模型使用針孔模型,雙目相機在使用之前會經過立體標定[11],完成畸變矯正和平行校正。經過標定后的雙目相機模型如圖1所示。

圖1 算法整體流程

1.1.1 坐標系定義

攝像機坐標系:OciXciYciZci(i=1,2)。i=1表示左相機;i=2表示右相機;光軸方向Oc1Zc1和Oc2Zc1平行;光心之間相距Oc1Oc2=Tx表示基線。

圖像坐標系:Oiuivi(i=1,2)。以像素為單位,左右相機具有相同的主點坐標(u0,v0)。

世界坐標系:OwXwYwZw。以左相機的攝像機坐標系為世界坐標系,并且第1幀圖像的攝像機坐標系作為全局的世界坐標系,所有圖像序列的位姿均相對于此世界坐標系表示。

1.1.2 投影模型與三維重建

世界坐標系中的點P(xw,yw,zw)投影到雙目相機中得到左右圖像中的P1(u1,v1)和P2(u2,v2)。設相機的內參為K,以左相機為追蹤相機,則在齊次坐標系下投影方程表示為:

(1)

其中,s為比例因子,設為1。

對于空間中點P,通過三角計算恢復三維世界坐標示意圖,如圖2所示。

圖2 雙目相機模型圖

P點在左右圖像中的視差d=u1-u2,由三角關系可得:

(2)

將式(2)代入式(1),得到:

(3)

1.2 特征點提取與匹配

雙目視覺里程計中的特征點匹配包括左右圖像之間的稀疏立體匹配和幀間運動的特征追蹤。為得到準確的特征點三維坐標,需要特征點算子具有可重復性和穩定性,包括光照不變性、旋轉不變性、尺度不變性等,同時兼顧實時性。本文選擇了一種基于GPU的加速尺度不變特征變換(Scale Invariant Feature Transform,SIFT)[12]算法SiftGPU[13]。SiftGPU縮短了SIFT算法的時間消耗,使其用于實時圖像處理成為可能。SIFT算子是一種提取局部特征的算法,在尺度空間尋找極值點,提取位置、尺度、旋轉不變量,并對視角變化、仿射變換、噪聲也保持一定程度的穩定性。

獲取左右圖像的SIFT特征點后,采用特征向量的歐式距離作為相似性度量。對于立體匹配,極線校正后的圖像匹配點基本處于同一行。對每個特征點,搜索其在另一幅圖像中同一行和相鄰2行的特征點計算歐式距離。比較每組特征的最近和次近距離的比值,當比值小于給定閾值,最近距離點判為匹配點,否則判為無效匹配點。降低閾值能提高匹配精度,增大閾值能獲得更多的匹配點。本文通過對大量變換場景的左右圖像匹配實驗,選擇0.6作為匹配閾值。分別以左右圖為基準進行匹配,滿足左右一致性才能成為為正確的匹配點。幀間運動的特征追蹤以左圖為基準,需要遍歷更多的特征點,匹配準則和稀疏立體匹配相同。

1.3 增量式的運動估計

每一幀含有左右2張圖像,以左圖為基準,經過左右立體匹配和前后幀間特征追蹤之后,得到了上一幀Ik-1和當前幀Ik的3D坐標匹配點集。假設從Ik-1到Ik相機經過旋轉矩陣R和平移矩陣T,如圖3所示。

圖3 三維重建示意圖

1.3.1 幀間運動估計求解

(4)

由于每一組匹配點存在誤差(如式(5)所示),因此通過3D到3D的匹配點集求解旋轉矩陣R和平移矩陣T,使所有匹配點的誤差平方和最小化(如式(6)所示)。

(5)

(6)

求解上述方程,首先對3D匹配點集坐標分別去質心化,令兩組匹配點集質心分別為:

(7)

因此,去質心后的匹配點坐標為:

(8)

將式(8)代入式(6)并展開得:

(9)

式(9)表示的優化問題可以等價于式(10)和式(11):

(10)

‖p′-Rp-T‖2=0

(11)

R=UVT

(12)

將式(12)代入式(11)得到:

T=p′-UVTp

(13)

(14)

雙目視覺里程計通過這種增量的方式從第1幀開始不斷獲得每幀相對于第1幀的位姿,實現定位的功能。

1.3.2 RANSAC運動估計

由于幀間運動的特征追蹤存在誤匹配點,錯誤的匹配點會導致式(10)的求解誤差增大。為降低幀間錯誤匹配點給運動估計帶來的影響,本文采用隨機采樣一致性方法(RANSAC)[15]去除誤匹配點。主要步驟如下:

1)從匹配點集中隨機選擇m(m≥3)個匹配點,利用1.3.1節的求解方法求解初始R、T。

2)根據初始R、T按式(5)計算誤差,如果小于給定閾值,該匹配點判斷為內點,否則為外點。

3)重復上述步驟S次,找到內點數量最多的點集。

4)使用上述內點集重新計算R、T,并且剔除外點內的匹配點。

2 局部位姿優化

上述雙目視覺里程計的運動估計只用了2幀之間的特征匹配,幀間約束較少,當2幀匹配點較少或誤匹配太多時,計算出的位姿誤差較大。本文在上述視覺里程計的基礎上提出了一種基于可變滑動窗口的局部優化方法,增加幀間約束,提高幀間運動估計的精度和魯棒性。

2.1 關鍵幀與非關鍵幀

為了提升計算效率,本文算法將圖像序列分為關鍵幀和非關鍵幀。每個非關鍵幀和上一個關鍵幀一一對應,非關鍵幀的位姿相對于該關鍵幀表示,關鍵幀的位姿相對于第一個關鍵幀即全局世界坐標系表示,如圖4所示。其中,第1幀設為關鍵幀,每隔fps幀設一個關鍵幀。由于旋轉過程中圖像變換過快,如果當前幀相對于關鍵幀新出現的特征點占比超過10%,則當前幀設為關鍵幀。

圖4 幀間運動估計圖

2.2 可變滑動窗口位姿優化

當新的關鍵幀到來時,會有一個可變滑動窗口形成,如圖5所示。其中,路標點{P}是關鍵幀所觀察到特征點對應的3D世界坐標點集,zk是路標點在第k個關鍵幀KFk上的投影坐標。滑動窗口內包含(m+1)個關鍵幀。窗口大小由當前幀所觀察到的路標點決定,當前幀確定為關鍵幀后,會尋找和當前幀觀察到相同路標點的關鍵幀,如果相同路標點的數量超過當前幀的一半,關鍵幀進入滑動窗口,滑動窗口大小加1。

圖5 關鍵幀和非關鍵幀示意圖

經過增量式運動估計后的關鍵幀KFk的初始位姿設為Xk,第i個路標點設為Pi,當在關鍵幀KFk上能夠觀測到Pi時,設觀測到的像素坐標為zk,i,則可以用觀測方程表示:

zk,i=h(Xk,Pi)+ek,i

(15)

因此,觀測誤差表示為:

ek,i=zk,i-h(Xk,Pi)

(16)

用x=[X1,X2,…,XN,P1,P2,…,PM]表示滑動窗口內的N個關鍵幀位姿和M個路標點3D坐標,則x是需要優化的變量。假設路標點{P}在N個關鍵幀里的投影坐標共有S個,即滿足zk,i有值的(k,i)共有S對,那么局部位姿優化將通過調整Pi和Xk的值來使得所有的觀測誤差平方和最小,即最小化以下目標函數:

(17)

其中,Ωj為信息矩陣,是觀測誤差協方差矩陣的逆。

對式(17)采用高斯-牛頓[16]方法求解,對每一個誤差項ej(x)和微小增量Δx,有:

(18)

將式(18)代入(17)式并作差ΔJ=J(x⊕Δx)-J(x),得到:

(19)

(20)

通過求解式(20)這個線性方程得到目標函數的梯度Δx,并對關鍵幀的位姿Xk和路標點坐標Pi進行更新:

x′=x⊕Δx

(21)

重復式(18)～式(21)直到優化變量x收斂,此時關鍵幀的位姿Xk和路標點Pi局部優化完成。非關鍵幀的全局位姿會隨著關鍵幀的位姿局部優化而得到更新。

3 全局位姿優化

局部位姿優化能夠減小雙目視覺里程計的幀間運動估計的誤差,隨著相機的運動,幀間運動估計的誤差會因為累積而變大,導致軌跡漂移。為了減小累積誤差,本文采用了一種基于回環檢測的全局位姿優化方法,一旦回環檢測成功,將對回環內的所有關鍵幀位姿和路標點進行全局優化。

3.1 回環檢測

回環檢測需要在圖像序列中找到出現在同一位置的關鍵幀,可以通過模式識別的方式來實現。本文采用詞袋模型[18]進行回環檢測。

詞袋模型最早出現在信息檢索領域中,忽略文本的語法與詞序,用無序的單詞表示文檔。在計算機視覺中,圖像類比于文檔,圖像塊中的特征向量類比于單詞,則圖像的詞袋模型可用圖像中所有圖像塊的特征向量表示。本算法使用SiftGPU提取圖像中的局部不變特征向量作為視覺詞匯,并構建相應單詞表,用單詞表中的單詞來表示每一個關鍵幀。具體步驟如下:

1)利用SiftGPU對大量的圖像數據集進行特征提取和描述,獲得作為視覺詞匯的特征向量集。

2)利用K-means[19]聚類方法對特征向量集進行聚類,得到K個聚類中心,并且用聚類中心構造sift單詞表。

3)對關鍵幀提取特征點和特征向量后,統計特征向量在單詞表中出現的次數,將關鍵幀表示成關于sift單詞表的K維向量。

對每一個關鍵幀會記錄其Sift詞袋模型向量,當新的關鍵幀到來時,通過計算詞袋模型向量之間的距離來查詢之前關鍵幀中相似的關鍵幀。將當前幀和相似度超過閾值的關鍵幀進行特征匹配和幀間運動估計,如果相似關鍵幀相對于當前關鍵幀具有微小的位移t和足夠多的匹配點,且相似關鍵幀到當前關鍵幀之間初始位姿存在較大的位移,則判斷回環成功。

3.2 全局優化窗口位姿優化

當前關鍵幀KFk與關鍵幀KFj回環檢測成功,如圖6所示。

圖6 局部優化中的可滑動窗口示意圖

(22)

同時在KFk處構造可變滑動窗口,利用2.2節的方法對當前關鍵幀KFk和滑動窗口內的關鍵幀KFj～KFj+m的位姿進行局部優化。

當可變滑動窗口中的關鍵幀局部位姿優化完畢,可變滑動窗口擴大至整個回環變成全局優化窗口,開始進行全局優化。全局優化的計算方法和式(17)～式(21)相同,不同的是在優化的過程中保持關鍵幀KFk和可變滑動窗口內的關鍵幀KFj～KFj+m的位姿不變,此時的優化變量變為x=[Xj+m+1,Xj+m+2,…,Xk-1,P1,P2,…,PM],目標函數變為:

(23)

全局優化完成對全局優化窗口內的所有關鍵幀位姿和路標點坐標的優化,同時更新非關鍵幀的位姿。經過全局優化后的移動機器人累積位姿誤差得到顯著消除,軌跡漂移減少。

至此,基于局部和全局優化的雙目視覺里程計算法的流程如圖7所示。

圖7 全局優化示意圖

4 實驗與結果分析

4.1 實驗數據與運行環境

本文實驗所用電腦型號為MS-7885(CPU酷睿i7-5820k,主頻3.30 GHz,內存16 GB),運行系統為Linux。測試數據選擇計算機視覺算法測評平臺KITTI[20]的雙目圖像數據集。該數據集是由搭載PointGray Flea2雙目相機和OXTS RT3003 IMU/GPS導航系統的車輛在城鎮街道行駛獲取的,其中雙目圖像序列作為算法的輸入,IMU/GPS的數據作為參考真實值評價算法的的精度。實驗數據相關參數如表1和表2所示,實驗環境的真實場景如圖8所示。

表1 雙目相機參數

表2 圖像序列參數

圖8 實驗環境真實場景

4.2 算法性能分析與比較

4.2.1 算法性能分析

為清楚地看到局部優化和全局優化對位姿誤差的消除效果,本文分別對算法在不做優化、只做局部優化、只做全局優化、局部和全局優化結合的定位效果進行了實驗分析。圖9是算法在4種情況下計算出的相機運動軌跡和實際參考運動軌跡的比較?？梢钥吹?算法在不做優化時計算出的運動軌跡出現明顯漂移,經過局部和全局優化后,運動軌跡和參考值基本重合。

圖9 局部優化和全局優化對相機運動軌跡的影響

實驗分別比較了不同的距離和速度下位姿誤差,結果如圖10所示。其中距離范圍為50 m～800 m,每間隔50 m計算相應的旋轉誤差和平移誤差,速度從6 m/s～16 m/s,每間隔2 m/s計算相應的旋轉誤差和平移誤差,表3是相應的統計誤差范圍。

圖10 局部優化和全局優化對位姿誤差的影響

誤差項旋轉誤差/(deg·m-1)平移誤差/%不做優化0.0210～0.0451.10～6.80局部優化0.0016～0.0110.60～0.90全局優化0.0020～0.0140.20～1.40局部+全局優化0.0006～0.0070.09～0.45

可以看出,算法在不做優化時,旋轉誤差和平移誤差較大。旋轉誤差最大達到0.045 deg/m,平移誤差最大達到6.8%,同時圖10(b)和圖10(d)顯示,隨著運動距離和速度的增加,平移誤差有逐漸增大的過程,這是算法在不做優化時位姿誤差累積的結果。同時可以看到局部優化和全局優化分別能夠單獨減小位姿誤差,而且局部優化對位姿誤差的消除效果稍微要好于全局優化,這是因為局部優化始終在進行,而全局優化只有在回環檢測成功的時候進行。

從圖10(b)中可以看到,距離達到600 m時,全局優化的平移誤差開始小于局部優化。這是由于此時場景中出現回環,全局優化開始進行,減小位姿誤差的累積,定位更加精確。當局部優化和全局優化同時進行時,隨著運動距離和速度的增加,旋轉誤差和平移誤差基本保持最小并下降至趨于穩定,定位誤差最小。

表4顯示了本文算法的時間消耗?？梢钥闯?基于局部優化和全局優化的雙目視覺里程計沒有顯著增加每幀的處理時間,每幀耗時約90 ms,能夠達到10 frame/s以上,基本滿足實時性要求。

表4 平均每幀耗時比較 ms

4.2.2 算法性能比較

為了比較本文算法和其他雙目視覺里程計算法精度,本文同時實驗了Andreas Geiger等人開發的視覺里程計C++庫libviso2[21]。libviso2是基于特征點匹配的雙目視覺里程計,但其算法沒有本文算法中的局部優化和全局優化。

圖11是本文算法和libviso2算法分別在實驗數據集運行后的平面軌跡圖?？梢悦黠@看出,libviso2算法的運動軌跡在較長時間后開始偏離參考值,且偏移量越來越大。而本文算法運動軌跡基本和參考值保持一致,沒有明顯的偏移。

圖11 本文算法和libviso2算法的相機運動軌跡圖

圖12是本文算法和libviso2算法計算出的位姿誤差分析圖,從中可以明顯看出,本文的算法在不同速度和距離下旋轉誤差和平移誤差都明顯低于libviso2算法。libviso2算法的旋轉誤差最大達到0.018 deg/m,平移誤差最大達到1.2%,而本文算法旋轉誤差最大為0.007 deg/m,平移誤差最大為0.45%。從圖12(b)中可以看到,隨著距離的增加,libviso2算法的平移誤差逐漸增加,而本文算法的平移誤差逐漸減小,說明局部優化和全局優化能夠顯著減小幀間運動估計的誤差和累積誤差,定位更精確。

綜上所述,本文算法在滿足實時性要求的基礎上,能夠顯著減小位姿誤差,提高定位精度。

5 結束語

本文提出基于局部和全局優化的雙目視覺里程計算法,設計了利用加速SIFT特征點匹配和RANSAC運動估計的雙目視覺里程計。通過可變滑動窗口對位姿進行局部優化,減小幀間運動估計的誤差,并采用回環檢測對位姿進行全局優化減小誤差累積。實驗結果表明,本文算法在滿足實時性要求的同時具有較高的定位精度。目前多傳感器融合是移動機器人導航的趨勢,因此,下一步將設計雙目和慣性導航融合的視覺里程計,提高定位算法的魯棒性和精度。

[1] SCARAMUZZA D,FRAUNDORFER F.Visual Odometry[J].IEEE Robotics & Automation Magazine,2011,18(4):80-92.

[2] MORAVEC H P.Obstacle Avoidance and Navigation in the Real World by a Seeing Robot Rover[M].Stanford,USA:Stanford University,1980.

[3] MATTHIES L,SHAFER S A.Error Modeling in Stereo Navigation[J].IEEE Journal of Robotics & Automation,1987,3(3):239-248.

[4] NISTER D,NARODITSKY O,BERGEN J.Visual Odometry[C]//Proceedings of 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition.Washington D.C.,USA:IEEE Press,2004:652-659.

[5] MAIMONE M,CHENG Y,MATTHIES L.Two Years of Visual Odometry on the Mars Exploration Rovers[J].Journal of Field Robotics,2007,24(3):169-186.

[7] ENGEL J,SCH?PS T,CREMERS D.LSD-SLAM:Large-scale Direct Monocular SLAM[C]//Proceedings of European Conference on Computer Vision.Berlin,Germany:Springer,2014:834-849.

[8] DAVISON A J,REID I D,MOLTON N D,et al.MonoSLAM:Real-time Single Camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2007,29(6):1052-1067.

[9] MONTEMERLO M,THRUN S,KOLLER D,et al.FastSLAM:A Factored Solution to the Simultaneous Localization and Mapping Problem[C]//Proceedings of National Conference on Artificial Intelligence.New York,USA:ACM Press,2002:593-598.

[10] KLEIN G,MURRAY D.Parallel Tracking and Mapping for Small AR Workspaces[C]//Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.Washington D.C.,USA:IEEE Press,2007:225-234.

[11] 劉俸材,謝明紅,王 偉.雙目視覺的立體標定方法[J].計算機工程與設計,2011,32(4):1508-1512.

[12] LOWE D G.Distinctive Image Features from Scale-invariant Keypoints[J].International Journal of Computer Vision,2004,60(2):91-110.

[13] WU Changchang.SiftGPU:A GPU Implementation of Scale Invariant Feature Transform(SIFT)[D].Chapel Hill,USA:University of North Carolina at Chapel Hill,2013.

[14] 張賢達.矩陣分析與應用[M].北京:清華大學出版社,2004.

[15] RAGURAM R,CHUM O,POLLEFEYS M,et al.USAC:A Universal Framework for Random Sample Consensus[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2013,35(8):2022-2038.

[16] DENNIS J E,SCHNABEL R B.Numerical Methods for Unconstrained Optimization and Nonlinear Equations(Classics in Applied Mathematics,16)[M].[S.l.]:Prentice-Hall,1983.

[17] VARADARAJAN V S.Lie Groups,Lie Algebras,and Their Representations[M].Berlin,Germany:Springer,2013.

[19] AGARWAL S.Data Mining:Data Mining Concepts and Techniques[C]//Proceedings of International Conference on Machine Intelligence and Research Advancement.Washington D.C.,USA:IEEE Press,2013:203-207.

[20] GEIGER A,LENZ P,STILLER C,et al.Vision Meets Robotics:The KITTI Dataset[J].International Journal of Robotics Research,2013,32(11):1231-1237.

[21] GEIGER A,ZIEGLER J,STILLER C.StereoScan:Dense 3d Reconstruction in Real-time[C]//Proceedings of Intelligent Vehicles Symposium.Washington D.C.,USA:IEEE Press,2011:963-968.