基于雙目相機的視覺里程計

趙文愷，李 剛

(廣西大學 電氣工程學院，廣西 南寧 530004)

0 引 言

近年來，隨著機器人行業(yè)的快速發(fā)展，視覺定位技術(shù)成為機器人導航領(lǐng)域的重要研究方向[1,2]，其中視覺里程計(visual odometry，VO)是機器人定位的重要方法之一。若以傳感器選型為基礎(chǔ)，可對視覺里程計進行詳細地分類，共涉及3種類型，依次是單目、雙目和深度相機[3]，在這3種相機類型中，深度相機與單目相機均存在一定的缺陷，例如，具有相對較大的噪聲等[4]，故而，在VO領(lǐng)域被普遍運用的相機類型主要是雙目相機[5-7]。由于采集圖像信息在一定程度上存在差異性，故而，又可細分為兩種方法，第一，特征點法[8]，此方法具有相對而言比較強的魯棒性，即便噪音十分嘈雜，也可順利地完成工作，因此，此方法也成為一項核心方法；第二，直接法[9]，應用頻率相對較低。

現(xiàn)階段，針對VO領(lǐng)域，無論是國內(nèi)，還是國外，均已獲得了許多優(yōu)異的研究成果。根據(jù)文獻[10]可知，其以視覺詞典為核心，通過一系列設(shè)計得到了視覺里程計，通過對圖像庫構(gòu)造字典匹配位姿，這種方法體現(xiàn)出了十分明顯的優(yōu)勢，即具有相對較高的匹配精度，然而，由于整個操作過程具有一定的復雜性，故而，具體的應用依然存在局限性。曹文平[11]對此做出了相應的改進，運用BM立體匹配算法，基于特定的方式計算了雙目視差，并以像素灰度為基礎(chǔ)，基于有效手段完成了代價函數(shù)的構(gòu)造操作，盡管具有相對而言比較高的運算速度，但依然存在一定的缺陷，這主要體現(xiàn)于視差圖方面，十分容易導致空洞點的出現(xiàn)。此后，文獻[12]提出了一種改進后的雙目相機，此相機的VO主要以直接法為核心而完成相應的設(shè)計，表現(xiàn)出了一定的優(yōu)勢，主要指對大規(guī)模稠密地圖的恢復方面，然而，該相機的應用卻需要借助于合適的光照，故限制性因素較強。

現(xiàn)階段，視覺里程計依然面臨一系列問題，例如，設(shè)備成本相對較高、定位精度有待于進一步改進等，對此，本設(shè)計運用的視覺里程計主要有一種，即以雙目相機為核心的視覺里程計。提取特征點時，本文主要運用了ORB算法，使特征點范圍在原有基礎(chǔ)上有所增加，從而確保了信息的全面性，同時，還最大程度確保了ORB的實時響應性；為了使視差獲取更具精確度，本設(shè)計構(gòu)建代價方程計算視差時，主要基于BRIEF描述子完成了相應的構(gòu)建過程；為了達到深入改進VO精度的目的，以PnP的3D-2D模型為基礎(chǔ)，提出一種特定的迭代算法，即提供初值的算法，并基于g2o對相機所在位姿進行計算。

1 基于改進ORB的特征點檢測與匹配

針對特征點提取，最常見的一些算法有SIFT，SURF，ORB。在這些算法中，SIFT算法與SURF算法均具有一定的缺陷，因而，不能滿足本設(shè)計的相應要求?；趯O實時性的考慮，本文對特征點提取時，運用的算法主要有一種，即基于ORB的特征點提取算法。一般而言，ORB算法對特征匹配時，主要通過兩個操作完成相應的匹配過程，第一，對FAST關(guān)鍵點進行提??；第二，完成BRIEF描述子的構(gòu)造過程，在滿足尺度不變性的基礎(chǔ)上，并盡可能確保旋轉(zhuǎn)不變性的前提下，引入灰度質(zhì)心，并引入圖像金字塔。對于ORB算法，盡管其最大程度確保了特征提取的運算速度，然而，在閾值方面，因FAST的設(shè)置相對而言比較單調(diào)，十分容易導致分布不均現(xiàn)象，將在極大程度上影響配準精度。

1.1 基于改進ORB的特征點檢測

基于上述問題，本文運用的算法主要是特定的ORB算法，此算法主要以四叉樹圖像分割為基礎(chǔ)[13]，對多閾值FAST提取特征點進行了相應的設(shè)置。首先采取一系列有效措施完成了高斯圖像金字塔的構(gòu)建操作，以各個尺度為基礎(chǔ)，對相應的單元格進行劃分，而其又在一定程度上有關(guān)于待檢測關(guān)鍵點，以各個單元格為基準，采取有效措施完成相應地FAST檢測，對于關(guān)鍵點數(shù)目，若基于一系列假設(shè)檢測即可獲得其數(shù)值，則隨后展開后續(xù)單元格的檢測，否則將進入二次檢測過程。檢測后，對于獲得的特征點，再次基于特定的方式完成相應的劃分操作，主要基于四叉樹方式，為了使劃分結(jié)果在一定程度上具有平衡性，應以特征點數(shù)目為核心，完成相應的順序排列操作，然后展開相應的節(jié)點管理工作。最后對其特征方向展開具體的計算操作，并完成描述子的計算過程。

存儲特征點時，采取的存儲方式相對而言比較特殊，主要為四叉樹方式，不但能夠最大程度地節(jié)約存儲空間，還能夠使特征點的分布更具勻稱性。詳情如圖1所示。

圖1 改進ORB算法框架

四叉樹主要運用于二位空間分割，并對其做出了詳細定義，即一個節(jié)點下，可存在的節(jié)點數(shù)相對而言比較小，最多有4個節(jié)點，將圖像細分為多個區(qū)域，主要有4個區(qū)域，詳情如圖2所示，整個空間被細分為多個同等的象限，共計4個，如圖2所示，分別是UL,UR,BL,BR，n1,n2,n3,n4為展開節(jié)點，圓圈代表的含義主要有一點，主要指檢測到的FAST角點。

圖2 四叉樹索引

1.2 基于改進ORB的特征點匹配

視覺里程計中，最核心的一項環(huán)節(jié)為特征匹配環(huán)節(jié)，若特征匹配具有較好的合理性，則十分有利于后續(xù)的位姿估計。針對匹配方法，比較常見的方法主要涉及兩種，第一種，暴力匹配(brute-force matcher)，此算法相對而言比較簡便，然而，若特征點數(shù)量在一定程度上有所增加，則總體的運行時間必定會有所延長；第二種，快速近似最近鄰(FLANN)，此算法主要運用于對實時性要求相對較高的場合。然而，若僅以FLANN為基準，則必定會在一定程度上導致誤匹配，故而，本設(shè)計運用的算法則相對特殊，主要為FLANN和RANSAC的組合匹配算法。

RANSAC算法具有自身獨特的優(yōu)勢，主要體現(xiàn)于一點，即可使噪聲得到最大程度地消除。在匹配圖像時，RANSAC算法的關(guān)鍵步驟為采取有效方式完成單應性矩陣的計算過程，這體現(xiàn)于多視圖幾何中，主要指兩幅圖片特征點處于共同平面上的映射關(guān)系。假定像素點坐標是(u,v)，單應性矩陣H定義為

(1)

若要計算H矩陣的8個參數(shù)，至少需要4對匹配點，而實際給定點數(shù)遠遠大于8對，因此使用隨機一致性檢驗求解。描述如下：

(1)選定輸入?yún)?shù)時，以多數(shù)的匹配點為核心，采取特定的方式選定這些匹配點的4對;

(2)經(jīng)過相應的采集處理后，以所得8個點為基礎(chǔ)，采取有效方式完成單應性矩陣H的估算過程;

(3)以上述H矩陣為基準，采取特定的計算方式對剩余匹配成功點對數(shù)n進行相應的驗證;

(4)迭代K次，找到最大n滿足的矩陣H。

2 基于BRIEF描述子的雙目視差

立體匹配共涉及兩種，第一種，局部立體匹配[14]，這種算法又可細分為兩種，分別為BM和SGBM，具有相對而言比較差的視覺效果；第二種，全局立體匹配，此算法具有相對較高的計算精度，然而，整體的計算量相對龐大，在具體運用過程中，并無較好的應用型。本設(shè)計運用的算法同樣未局部立體匹配算法，但這種算法主要以BRIEF描述子為基礎(chǔ)，故整體性能有所改進，對特征點視差的提取具有一定的優(yōu)勢。

此前，基于ORB特征提取能夠獲得BRIEF描述子，BRIEF是一種十分特殊的二進制編碼，其組成方式主要為向量組成，即0,1向量，在特征點周圍的圖像塊A處隨意選定像素點p1和p2，A(p1) 代表的含義相對固定，主要為p1點灰度值，A(p2) 同樣具有固定含義，即p2點灰度值，若I(p1) 與I(p2) 相比，后者數(shù)值相對較大，則取0，反之，則取1。定義式(2)為

(2)

以得到的BRIEF向量為特征信息進行匹配，算法描述如下：

(1)將待匹配特征點為核心，采取有效方式與手段在參考圖像中構(gòu)建圖像主窗口，中心點的表征方式主要基于一種，即多個像素的BRIEF表征;

(2)相同行內(nèi)，以目標圖像為基準，用窗口完成相應的掃描操作，并分別構(gòu)建等同的子窗口;

(3)構(gòu)造代價函數(shù)(cost funcation)，對特定的相似度進行評估，主要指主窗口與子窗口，并在確定目標點時，以此為基準，即以具有相對較高相似度的最高窗口的中點確定為目標點;

(4)以左右圖像為基礎(chǔ)，采取適當?shù)挠嬎惴绞酱_定其像素點視差;

(5)對(1)～(4)步驟進行重復操作，獲得所有有用的像素點視差。

詳情如圖3所示。

圖3 立體匹配

常用代價函數(shù)構(gòu)造方法有：

SAD(sum of absolute difference)

(3)

SSD(sum of squared difference)

(4)

NCC(normalized cross correlation)

(5)

式中：L、R代表的含義相對而言比較固定，主要指為左右像素特征，i，j同樣具有各自的固定含義，分別為遍歷整個窗口元素，d值的計算次數(shù)每增加一次，則應相應地增加1，當S的數(shù)值處于峰值時，則相應的匹配會處于結(jié)束狀態(tài)，而所得點便是匹配點，d代表的含義相對固定，指視差。

3 運動估計

以前后時刻兩幀圖像為基礎(chǔ)，采取特定的操作方式獲得特征匹配點后，則應對其運動展開合理的預估?；陔p目相機，借助于內(nèi)置的特定運行方式能夠得到特征點3D坐標，并可由此得到特征點在相應圖像的2D坐標，故而，對相機運動進行估計時，可基于PnP(perspective-n-point)予以操作。PnP是3D-2D運動恢復模型，具體的求解方法共有多種，例如，EPnP等。然而，所用的方法均為線性計算方法，均有一定的缺陷。本設(shè)計所用的運動估計方法則恰好相反，主要為以非線性優(yōu)化為基礎(chǔ)的方法，先以線性方法為核心，采取有效措施完成相應的初值求解過程，再完成最小二乘方程的構(gòu)造操作，從而實現(xiàn)優(yōu)化位姿的目的[15]。

3.1 PnP投影模型

PnP在求解位姿時至少需要3對3D-2D點，模型如圖4所示。

圖4 PnP投影模型

此中，相機光心為P，世界坐標系3D點與像素坐標2D點相對而言比較固定，主要指A，B，C與a，b，c，字母的大寫與小寫之間具有較好的一致性。由此可知：△Pab～△PAB，△Pbc～△PBC，△Pac～△PAC。由余弦定理可得

PA2+PB2-2PA·PB·cos(a,b)=AB2
PB2+PC2-2PB·PC·cos(b,c)=BC2
PA2+PC2-2PA·PC·cos(a,c)=AC2

(6)

設(shè)u=AB2/PC2,v=BC2/AB2,w=AC2/AB2， 化簡得

(1-u)y2-ux2-ycos(a,b)+2uxycos(a,b)+1=0
(1-w)x2-wy2-xcos(a,b)+2wxycos(a,b)+1=0

(7)

可知該方程組為二元二次方程，利用吳消元法求解PA，PB，PC距離，最后得出相機運動。

3.2 非線性優(yōu)化運動估計

上述求解為線性方法，在3D點或2D點受噪聲干擾，出現(xiàn)誤匹配時位姿估計結(jié)果不佳，算法失效，因此可以把PnP構(gòu)造成非線性最小二乘問題，通過最小化重投影誤差(reprojection error)求解最優(yōu)值。

假設(shè)計算n個三維空間點Pi(Xi,Yi,Zi) 及其投影點Ui(ui,vi,zi)，為相機位姿 (R,t) 的李代數(shù)，相機內(nèi)參為K，關(guān)系式如下

(8)

由于存在噪聲，預測值與觀測值存在誤差，構(gòu)建最小二乘方程如式(9)

(9)

3.3 圖優(yōu)化的PnP求解

為了獲得相對而言比較低的方程值，應采取有效方法與手段計算最優(yōu)位姿。所謂圖優(yōu)化，主要指以圖像的方式對優(yōu)化問題加以詳細地描述，而凸優(yōu)化的求解方式則應基于一定的軟件，即g2o?；趫D的頂點等方式的詳細構(gòu)建完成最優(yōu)位姿的求解操作，此中，頂點代表的含義相對而言比較固定，主要指待優(yōu)化變量，相機的位姿，邊為誤差項。詳情如圖5所示。

圖5 PnP圖優(yōu)化

運動位姿求解流程如下：

(1)運動位姿初值的獲取方式主要基于一種，即傳統(tǒng)PnP線性求解法;

(2)以位姿初值為基準，將其設(shè)定為圖優(yōu)化模型頂點，并對邊進行了相應地定義，即定義為重投影誤差;

(3)以L—M為基準完成相應的迭代操作;

(4)基于g2o，采取一系列有效措施與手段獲得最優(yōu)運動位姿。

4 實驗結(jié)果與分析

(1)實驗環(huán)境

實驗平臺采用華碩A55V，配置Intel(R) Core CPU i5-3210M，主頻2.5 GHZ，4 G內(nèi)存，軟件環(huán)境為Clion+Ubuntu14.04操作系統(tǒng)。雙目視覺傳感器的組成主要基于一種方式，即以型號XG200的兩臺工業(yè)相機為基礎(chǔ)，采取特定的技術(shù)手段對其予以支架組合方式，獲得相應的雙目視覺傳感器?？刹扇∈謩诱{(diào)節(jié)方式對焦距進行相應地調(diào)節(jié)，同時，還可基于相同的方式調(diào)節(jié)基線距離，圖像分辨率640*480。

(2)特征提取與特征匹配

實驗過程中，對每張圖片進行了相應的設(shè)定，所提特征點的數(shù)量是固定的，最多不超過500個點。圖6為OpenCV中ORB與改進ORB特征提取結(jié)果對照，由此可知，對于特征部分，若基于改進后的ORB算法，則可獲得相對而言比較勻稱的分布。

對相鄰兩幀圖像進行匹配，圖7(a)與圖7(b)代表的含義分別是只有FLANN算法的結(jié)果、FLANN和RANSAC組合匹配算法的結(jié)果，據(jù)此可知，后者可以獲得相對而言比較優(yōu)良的VO特征匹配效果。

圖7 消除誤匹配效果

(3)測試數(shù)據(jù)集

根據(jù)表1，以相鄰兩幀圖像為基準，采取一系列有效措施對兩者的定位精度進行相應的測量，假定坐標系原點(0,0)為最初位置，采樣節(jié)點的數(shù)量共計4個，對比兩者的誤差。

據(jù)表1能夠得知，在兩幀圖像間位置進行相應計算時，該視覺里程計具有相對而言比較高的定位精度，且平均相對誤差比較低，具體誤差百分比為2.65%，基本符合相應的要求。

對于優(yōu)化前與優(yōu)化后的位置誤差進行詳細測定，并對于平均每幀耗時進行相應地測定，對數(shù)據(jù)進行采集時，以Y軸為基礎(chǔ)，使雙目相機展開一定距離的直線運動，運動距離為50 m。因相機始終處于相對而言比較固定的高度位置，相機的整個運動也始終保持直線運動趨勢，故而，相應誤差的來源則共涉及一種途徑，即Y軸移動距離。據(jù)圖8能夠得知，針對視覺里程計，若采取優(yōu)化處理后，則可使誤差在較大程度上有所減小，然而，位置誤差同樣會伴隨采樣距離的增加而增大，這是由于本文的優(yōu)化處理操作具有一定的局限性，僅針對前端視覺里程計相鄰兩幀圖像間的位姿，后端并未做出相應地優(yōu)化處理，與具體情況具有較好的一致性。

據(jù)表2，對于運行時間，將優(yōu)化與未優(yōu)化時，均加入其中，同時，對于誤差范圍，亦是如此，由此可知，若采取優(yōu)化處理后，每幀圖像處理時間并無顯著增加，然而，卻在一定程度上降低了相對誤差，總體而言，符合相應的需求。

表1 采樣與視覺定位位置對比/m

圖8 采樣位置與位置誤差

表2 運行時間和誤差范圍

5 結(jié)束語

本設(shè)計對視覺里程提出了一定的要求，主要是應能滿足對相鄰兩幀相機運動狀態(tài)進行實時估計的目的，然后，由于現(xiàn)階段視覺里程計存在一系列問題，例如，定位精度相對較低等，故，提出了一種改進視覺里程計的方案，此方案主要以雙目相機為基準。在現(xiàn)有ORB算法的基礎(chǔ)上進行改進，提取描述子作為信息獲得視差，恢復特征點三維坐標，采用FLANN和RANSAC組合算法進行匹配，對匹配到的特征點構(gòu)建最小二乘問題，求得最佳位姿。根據(jù)實驗結(jié)果可知，對于相鄰兩幀圖像，該方案可以較好地對兩者之間的位姿進行估計，符合了相應的要求。同時，運用的整個過程中，因并未展開相應的優(yōu)化處理操作，會導致一定的累積誤差，故應實施一定的人工校準，后續(xù)工作也將對此進行改進。