基于雙向重投影的雙目視覺里程計

張 濤，陳 浩，閆 捷，李 瑤

（1. 東南大學 儀器科學與工程學院，南京 210096；2. 東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；3. 北京電子工程總體研究所，北京 100854）

隨著科技的進步以及人們需求的增加，移動機器人等智能機器在人類社會越來越常見，尤其是近年來，無人機等微型機器人廣泛應用于搜索救援、邊境偵查、工業(yè)廠房檢查等[1]領域。但由于其載重的有限性以及電池的持久性限制，所攜帶的傳感器必須具有重量輕、功耗小、高可靠性以及易配置的特點。視覺里程計（Visual Odometry,VO）以其導航高度自主、低成本及便捷等特點，在無人機等自主機器人導航領域具有重要的作用。

視覺里程計通過研究圖像幀與幀間的變換關系，估計機器人相對周邊環(huán)境的位置與姿態(tài)信息[2]，其主要采用兩種計算方式[3]：直接法和特征點法。直接法利用圖像的亮度信息，通過最小化光度誤差來估計機器人的位姿信息。直接法省去了計算特征點和描述子的時間，但是直接法對單個像素沒有區(qū)分度，且在光照變化時會破壞灰度值不變的假設，使算法失敗。

特征點法可以分為兩步：1）檢測圖像特征點并建立描述子，采用極線約束等方法進行特征點匹配；2）利用第一步匹配結果，由三角測量建立局部地圖，解算最小化重投影誤差得到位姿信息。

相比直接法，特征點法運行穩(wěn)定，是視覺里程計的主流方法。

目前基于特征點法的視覺里程計存在以下問題：

1）在特征匹配階段樣本容量大、匹配準確率低，導致視覺里程計位姿估計精度下降。張云生[4]利用隨機采樣一致性方法（RANSAC）對匹配算法進行改進，提升了匹配精度；李小紅[5]等采用PROSAC（Progressive Sample Consensus）算法來消除誤匹配點，得到比RANSAC方法更好的魯棒性和計算效率；Wu Yue等[6]采用迭代最近點法增加正確匹配點對以改進RANSAC算法，但是此方法增加了樣本容量，以致算法效率降低；徐曉蘇等[7]運用聚類抽樣方法以降低錯誤匹配率；

2）視覺里程計僅通過相鄰兩幀進行位姿估計，必然會產(chǎn)生漂移誤差，此外相機分辨率也會帶來特征點的漂移，這些都會降低視覺里程計的精度。Bellavia F等[8]提出 SSLAM 算法，該算法通過優(yōu)化關鍵幀的選取來縮小空間點的確定范圍，提高空間點的確定精度；程傳奇等[9]采用非線性優(yōu)化方法并結合三維地圖點以削弱累計誤差；Badino H等[10]提出MFI算法，當特征點漂移到一定距離后，采用計算得到的預測特征點代替原特征點來減小漂移誤差，但是該算法需要時刻計算特征點的預測值，計算量大，耗時久。

本文針對誤匹配消除問題，采用在特征檢測階段選取壽命長的特征點、匹配階段進行環(huán)形匹配以及利用優(yōu)質匹配點對等方法選取優(yōu)質特征點以降低RANSAC算法樣本容量、提升正確匹配點對比例。針對特征點漂移與累積誤差問題，提出一種基于雙向重投影的位姿估計算法，解算下一幀圖像中的最小化重投影誤差得到初步的位姿信息，并將此位姿用于反向重投影，解算當前幀中最小化重投影誤差以提升位姿估計精度。

1 基本原理

1.1 坐標系約定

在相機成像中，小孔成像模型運用最廣泛。在此模型下，可以采用線性方程組來描述物體的空間坐標和圖像坐標之間的關系。圖1展示了模型中的坐標系關系，其中，為世界坐標系下的三維空間點，m為在圖像平面的投影點，為焦距，即光心到圖像平面的距離。

圖1 坐標系關系圖Fig.1 Coordinate system

各坐標系的定義如下：

像素坐標系——像素坐標系與圖像坐標系基本一致，只有兩處區(qū)別，一是將坐標原點移至圖像平面的左上角頂點，二是像素是此坐標系下的基本單位。像素坐標系相對于圖像坐標系的平移量為，即像素坐標系沿x軸正方向平移，沿y軸正方向平移，即可得到圖像坐標系。

1.2 雙目相機模型

圖2(a) 雙目相機的成像模型Fig.2(a) Imaging model of binocular camera

圖2(b) 雙目相機幾何模型Fig.2(b) Geometric model of binocular camera

2 算法流程

本文以R. Mur-Artal等人的ORB-SLAM2算法[11]為基礎，對其中的位姿估計部分進行改進，以期提升位姿估計精度。改進分為三個階段：1）特征點檢測階段；2）特征點匹配與誤匹配消除階段；3）重投影階段。

整體設計流程如圖3所示，具體包括以下步驟：

Step 1：獲取算法所需雙目圖像（本文采用KITTI數(shù)據(jù)集），在ORB-SLAM2特征點提取的基礎上，記錄所提取特征點的壽命，并建立對應的描述子；

Step 2：對所得的特征點進行環(huán)形匹配，選取優(yōu)質匹配點對，并采用RANSAC算法進行誤匹配消除；

Step 3：建立局部地圖，并對空間點進行投影，解算最小重投影誤差：

Step 4：更新姿態(tài)矩陣與空間點，進行反向投影，解算此時最小重投影誤差，解算結果求逆，可得姿態(tài)矩陣，完成位姿估計。

圖3 本文算法流程圖Fig.3 Flow chart of the algorithm

3 算法實現(xiàn)

3.1 ORB特征點提取

特征點由關鍵點和描述子兩部分組成。ORB算法是在2011年國際計算機視覺大會（ICCV）上提出的一種新型方法[12]。ORB算法中的關鍵點提取采用FAST關鍵點[13]檢測算法。FAST是一種角點，主要檢測局部像素灰度變化明顯的地方，速度非?？?。在以候選像素點為圓心、半徑為3像素的圓周內，有N個像素點與候選像素點的亮度差別明顯，則該候選像素點即為FAST關鍵點。設為候選像素點，且

其中，I(o)為候選特征點的灰度值，I(x)為候選點周圍圓周上一點的灰度值，ω為以o 為圓心的半徑為 3個像素的圓周區(qū)域，εd為符合條件的點數(shù)閾值，通常為 9、11、12，N 為符合界定條件的像素個數(shù)。如果N大于閾值，則認為o 是一個特征點。

FAST關鍵點檢測計算簡單、速度快，但FAST特征點不具有方向性和尺度信息，因此ORB通過構建圖像金字塔與采用灰度質心法實現(xiàn)了尺度不變性和旋轉不變性。所謂質心是指以圖像塊灰度值作為權重的中心，考慮特征點o 的一個鄰域B，定義圖像塊的矩為

其中，p,q ∈(0,1)，r 表示圖像塊B 的半徑，I(x,y)表示圖像塊(x,y)位置處的灰度值，并利用矩來定義特征點的質心：

至此，F(xiàn)AST關鍵點具有了尺度不變性和旋轉不變性，大大提升了其健壯性。

在提取關鍵點后，ORB算法利用改進的 BRIEF特征向量來構造特征點描述子。對特征點o 的一個S×S鄰域D(其中，S一般為31)，定義一個二進制測試準則τ：

其中，q(x)為點x 處的灰度值。在鄰域D 選擇n 個點對進行二進制測試，BRIEF描述子得到一個n 維二進制碼：

由于BRIEF不具有旋轉不變性，因此利用式(8)求得的特征點的方向夾角θ。假設生成特征點描述符的n 個測試點對為(xi,yi)，由式(10)可以定義一個2×n的矩陣：

對各測試點對，利用θ形成的旋轉矩陣Rθ，構造經(jīng)過旋轉之后的匹配點的位置Sθ：

在新的點集位置上構建二進制串描述符，此時描述子已具有旋轉不變性。至此ORB特征點提取結束，下一步將進行特征點匹配與誤匹配消除。

3.2 特征點匹配與誤匹配消除

傳統(tǒng)的 RANSAC方法將所有的匹配結果作為樣本，樣本容量大，含有誤匹配較多，正確匹配比例低，效率低下。因此本文從減少誤匹配數(shù)，降低樣本容量方面改進RANSAC算法，具體步驟如下：

Step 1：對左右兩幅圖像提取ORB特征，并記錄每個特征點的存活幀數(shù)，即特征點壽命。特征點的壽命越長，其可靠性越高。采取網(wǎng)格法將圖像分成10n×10n的網(wǎng)格，對每個網(wǎng)格點內的特征點，按壽命大小進行排序，每個網(wǎng)格只保留前n個特征點，并使用其首次出現(xiàn)時的描述子。對同一個特征點使用相同的描述子可以減小漂移誤差。

Step 2：對特征點進行環(huán)形匹配，如圖4所示。首先采用極線約束，對當前一幀 Ii和下一幀圖像 Ij的特征點進行左右圖像匹配，得到左右匹配點對然后利用最近鄰方法，求取最小漢明距離，對當前幀 Ii和下一幀圖像Ij的特征點進行前后幀間匹配，得到匹配點對當四組匹配形成環(huán)形時，則視為有效匹配，否則匹配失敗。

圖4 環(huán)形匹配Fig.4 Circle match

Step 3：將所有匹配結果分為優(yōu)質匹配和一般匹配，只在優(yōu)質匹配中進行 RANSAC算法，縮減了樣本量，提升了效率。優(yōu)質匹配有兩個判定標準：1）壽命長的特征點的匹配對；2）漢明距離小的匹配點對。

對同一張圖片進行三種匹配實驗：ORB原始匹配、經(jīng)過RANSAC后的匹配、經(jīng)過改進RANSAC之后的匹配。匹配結果如圖5～7所示。

由圖5可以看出，原始的ORB特征點最多，但是其中誤匹配數(shù)量也是最多，若以此匹配結果進行位姿估計，耗時久且誤差大；從圖6可以看出，經(jīng)過RANSAC算法去除誤匹配后，匹配點對數(shù)量大大減少，誤匹配數(shù)量也僅有少量殘留且剩余匹配點對較集中，不利于后期位姿估計的解算；圖7采用了本文的算法，可以看出正確匹配所占比例大大增加，并使正確匹配對分散開來，增強了算法的穩(wěn)定性，為后續(xù)的位姿估計奠定了良好的基礎。

圖5 ORB原始匹配Fig.5 ORB original matching

圖6 RANSAC匹配Fig.6 Matching by RANSAC

圖7 改進的RANSAC匹配Fig.7 Matching by improved RANSAC

3.3 基于雙向重投影的位姿估計方法

在得到匹配點對并進行誤匹配消除后，需要建立局部地圖并將當前幀的空間點投影到下一幀圖像中，并計算投影點的最小重投影誤差。但是由于特征點的漂移會導致空間點的確定帶有較大誤差，在計算重投影誤差時誤差會累積，對精度造成較大影響。因此，本文提出一種基于雙向重投影的位姿估計方法，其流程如圖8所示。

圖8 二次重投影方法流程圖Fig.8 Flow chart of the twice reprojection method

其中，K為相機內參矩陣，R為世界坐標系到相機坐標系的旋轉矩陣，t為世界坐標系到相機坐標系的平移矩陣。

由于相機位姿未知以及觀測點有噪聲，由式(13)確定的投影點集存在誤差，即重投影誤差，因此構建最小二乘問題，解算最優(yōu)的相機位姿，使得重投影誤差D(Pi,j)最小：

圖9 重投影誤差示意圖Fig.9 Schematic diagram of a reprojection error

根據(jù)此時的姿態(tài)矩陣，利用式(15)更新空間點Qn=(xn,yn,zn)：

其中，Qn-1是上一次得到的空間點。

如圖10所示，將更新后的空間點集 Qn投影至當前幀，重新計算重投影誤差 D(Qj,i)：

圖10 雙向重投影示意圖Fig.10 Diagram of bi-directional reprojection

4 實驗結果與分析

以視覺SLAM公開數(shù)據(jù)集KITTI[14]為測試樣本，KITTI數(shù)據(jù)集由德國卡爾斯魯厄理工學院和豐田美國技術研究院聯(lián)合創(chuàng)辦，是目前國際上最大的自動駕駛場景下的計算機視覺算法評測數(shù)據(jù)集。該數(shù)據(jù)集是通過在城市地區(qū)、農村地區(qū)以及高速公路上行駛而獲得的，每張圖像上最多可顯示15輛汽車和30名行人。

如圖11所示，數(shù)據(jù)采集平臺裝配有2個灰度攝像機、2個彩色攝像機、一個Velodyne 64線3D激光雷達、4個光學鏡頭以及1個GPS導航系統(tǒng)，其以10 Hz的頻率采樣了 22個雙目圖像序列（8位 png灰度圖像），前11個圖像序列含有真實數(shù)據(jù)值，適合用來測定算法。其中相機坐標系定義為“右-下-前”，雷達坐標系定義為“前-左-上”，GPS和 IMU坐標系定義為“前-左-上”。數(shù)據(jù)集所給真值與本文所得結果均為相機坐標系下數(shù)據(jù)。

本文的實驗環(huán)境為Lenovo X240筆記本電腦（酷睿i7-4600CPU，主頻2.50GHZ，4G內存），以Ubuntu16.04為實驗平臺，選取KITTI數(shù)據(jù)集中的第1序列和第7序列。第1序列共有1104幀圖像，運動時間為114 s，運行速度較快；第 7序列共有 1101幀圖像，運行時間為 114 s，運動軌跡為閉環(huán)且慢速運動。分別采用ORBSLAM2方法[9]和本文算法建立視覺里程計模型，仿真實驗結果如圖12～15所示，圖中的(x,y,z)為相機坐標系下的坐標。

圖11 實驗數(shù)據(jù)采集平臺Fig.11 Experimental data acquisition platform

圖12顯示了兩種算法在序列1的相對位置和姿態(tài)誤差。從圖12中可以看出，兩種算法在各個方向上的位置誤差都是越來越大，符合視覺里程計模型。與ORBSLAM2相比，本文算法解算的位置和姿態(tài)角誤差都較小，累積誤差明顯較小。在X方向，ORB-SLAM2的最大誤差大約為19 m，而本文算法的最大誤差約為15 m，在Z方向（即車輛前進方向），ORB-SLAM2的最大誤差大約為 25 m，而本文算法的最大誤差約為20 m，精度提升了大約20%，且本文算法的穩(wěn)定性優(yōu)于ORB-SLAM2。

圖12(a) 位置誤差（序列1）Fig.12(a) Position error of sequence 1

圖12(b) 姿態(tài)角誤差（序列1）Fig.12(b) Attitude angle error of sequence 1

圖13(a) 位置誤差（序列7）Fig.13(a) Position error of sequence 7

圖13(b) 姿態(tài)角誤差（序列7）Fig.13(b) Attitude angle error of sequence 7

圖13顯示了兩種算法在一個閉環(huán)運動中（序列7）的相對位置和姿態(tài)誤差。從圖13中可以看出，ORBSLAM2在X和Z方向的誤差均在1.2 m以內，本文算法在X方向的誤差在1 m以內，Z方向誤差在0.9 m以內，精度提升大約20%。

圖14與圖15顯示了本文算法與 ORB-SLAM2以及真實值的運動軌跡。圖14的運動大部分近似于直線運動，誤差隨著運動越來越明顯。由圖14可以看出，本文算法與 ORB-SLAM2算法的誤差都是越來越大，即包含累積誤差，本文算法的誤差增長幅度相對較小，整體誤差約是ORB-SLAM2誤差的80%。圖15中運動從原點(0,0)出發(fā)，逆時針進行運動，最終回到原點。本文算法和 ORB-SLAM2都能得到較為理想的結果。與圖14相比，圖15所示運動因為速度較慢以及閉環(huán)檢測等環(huán)節(jié)的存在，算法會對累積誤差進行修正，因此圖15中行程算法優(yōu)化效果最好。從圖14軌跡可以看出，本文算法在直線行程中的優(yōu)化效果最好，定位精度提升最高 20%左右，優(yōu)于ORB-SLAM2的結果。

圖14(a) 解算軌跡（序列1）Fig.14(a) Trajectory of sequence 1

圖14(b) 部分放大（序列1）Fig.14(b) Partial enlargement of sequence 1

圖15(a) 解算軌跡（序列7）Fig.15(a) Trajectory of sequence 7

圖15(b) 部分放大（序列7）Fig.15(b) Partial enlargement of sequence 7

表 1列出了兩種算法在兩個序列中的均方根誤差（RMSE）。

表1 兩種算法的均方根誤差Tab.1 RMSE of the two algorithms

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

5 結 論

本文針對視覺里程計中的誤差累積問題，首先改進了傳統(tǒng)的 RANSAC算法，通過特征點壽命長短、優(yōu)質匹配等方法選取優(yōu)質特征點以縮減 RANSAC樣本容量，從而提高了 RANSAC算法的效率；然后提出一種基于雙向重投影的位姿估計算法，將初次解算最小化重投影誤差后的位姿估計結果應用到反向投影中，提高了位姿估計的精度；最后，利用SLAM公開數(shù)據(jù)集KITTI進行了仿真實驗。

仿真結果表明，與 ORBSLAM2進行對比，本文算法在精度和穩(wěn)定性上都有提升，驗證了本文改進算法的正確性，說明了本文的研究具有一定的價值。

為進一步提高定位精度、抑制累積誤差，下一步將考慮與其他傳感器進行組合導航[15]，開展組合模式的視覺里程計算法研究。