一種結合光流追蹤的雙目視覺里程計算法

李偉東，朱旭浩

(大連理工大學 汽車工程學院，遼寧 大連 116024)

0 引言

里程計技術最早出現于機器人領域，它是指機器人在未知環境中通過傳感器捕捉環境特征來確定自身位置和姿態，同時構建一個增量地圖。使用相機作為傳感器的里程計稱為視覺里程計。相較于激光雷達傳感器，相機部署方便，價格低廉，并且可以獲取更為豐富的視覺信息。隨著自動駕駛技術的發展，視覺里程計也越來越多地被用于對載具進行定位和地圖構建，因此基于視覺的里程計受到了廣泛關注?；谔卣髌ヅ涞奶卣鼽c法通常需要提取特征描述子并對其進行匹配，這個過程會消耗較多的資源和時間，在一些無法搭載高算力設備的自動駕駛場景中實時性較差；直接法則不需要耗費時間計算提取描述子，在只計算稀疏像素點的光度誤差時速度較快，但是對于光照變化非常敏感，精度低于特征點法視覺里程計，并且缺乏描述子用于系統追蹤失敗時進行重定位。因此一個輕量快速的高精度視覺里程計仍然是一個關鍵的研究問題。

根據數據關聯方式的不同，視覺里程計可以分為基于光度誤差的直接法和基于特征匹配的特征點法[1]。直接法視覺里程計通過最小化不同圖像之間的光度誤差來恢復相機運動。LSD-SLAM(Large Scale Direct Monocular SLAM)[2]是Engel等人提出的一種直接法視覺SLAM，它基于單目相機運行，并且可以在CPU上進行半稠密建圖。為了提高全局精度，系統還加入了回環檢測模塊。相較于傳統直接法，LSD-SLAM將光度誤差以及深度噪聲一起進行優化從而實現了較好的效果，但是由于采用了單目相機，所以無法恢復絕對尺度信息。Stereo LSD-SLAM[3]則是將LSD-SLAM拓展到雙目相機上，解決了單目相機無法恢復絕對尺度信息的問題。DSO(Direct Sparse Odometry)[4]是一種純直接法視覺里程計，它將數據關聯以及位姿估計統一成一個非線性優化問題，并且使用了滑動窗口算法，既控制了優化規模，提高優化速度，同時又保證了一定的系統精度。其缺點是由于圖像之間的光度誤差函數是一個非凸函數，因而優化時非常容易陷入局部最優值，只適合于移動較小的場景中。SVO(Semi Direct Monocular Visual Odometry)[5]是Forster等提出的一個結合了特征點法和直接法的半直接法視覺里程計。其首先提取稀疏特征點，使用直接法進行不同圖像之間對齊從而計算位姿；同時使用深度濾波器來估計深度信息。優點是運行速度非?？?，在低功耗設備上也有較高的幀率；缺點是其對光照情況非常敏感，在光照變化大的情況下非常容易追蹤失敗。

特征點法視覺里程計通過提取視覺特征，獲取匹配關系后使用光束法平差[6]計算和優化相機的位姿與地圖點位置。PTAM[7]是Klein等人提出的一個視覺里程計系統，其包含了追蹤和建圖兩個線程來同時恢復相機運動并優化地圖點和位姿，首次將多線程用于視覺里程計算法中。Raulmur等提出的ORB-SLAM[8]則在PTAM兩個線程基礎上增加了回環檢測線程，其利用詞袋模型[9]進行相似度檢測，在檢測到發生回環后進行回環矯正來全局優化。ORB-SLAM采用的傳感器為單目相機，無法恢復絕對尺度，并且非常容易出現尺度漂移的現象。在此基礎之上ORB-SLAM2[10]加入了對雙目相機以及RGBD相機的支持，不僅可以恢復絕對尺度，并且在RGBD模式下可以實現稠密建圖。隨后的ORB-SLAM3[11]則加入了魚眼相機模型，并且可以融合IMU傳感器，精度有了很大的提高。除了點特征外，線特征也可以用于位姿估計，在特征點缺失的場景下會有更好的魯棒性。PL-SLAM[12]是Pumarola等提出的一個將線特征用于視覺里程計上的視覺SLAM算法，該算法結合了線特征和點特征，在一些特征點缺失的場景中更加魯棒，精度更高，但是同時也提高了計算復雜度，導致系統計算資源消耗變大。Gomez等則將PL_SLAM擴展到雙目相機上，不僅提高了系統精度，同時還可以恢復絕對尺度[13]。

精度與運行速度是評價視覺里程計的兩個重要指標。特征點法視覺里程計會消耗大量時間去計算每一幀圖像中關鍵點的描述子，對實時性有較大影響；直接法視覺里程計省去了描述子計算過程，但是對于光照極為敏感，相較于特征點法精度有所降低，同時缺乏描述子用于在系統追蹤失敗時進行重定位。為了實現保持較高精度的同時有更好的實時性能，提出了一種結合了光流追蹤的視覺里程計算法。該算法基于特征點法，在追蹤階段主要通過光流追蹤來獲得匹配關系；生成關鍵幀時使用描述子來獲取匹配關系，并且在追蹤失敗時會使用描述子匹配進行重定位。由于只在提取關鍵幀和重定位時計算和匹配描述子，算法的運行速度有了大幅提升，同時保持了較高的精度。

1 視覺里程計算法

視覺里程計選取的傳感器為雙目相機。相較于單目相機，雙目相機可以計算得到像素點深度信息，解決了尺度不確定的問題，并且在追蹤過程中有著更好的魯棒性。算法包括追蹤線程和滑動窗口優化線程兩大部分。追蹤線程主要用于計算相機當前位姿，滑動窗口優化線程則會對關鍵幀及地圖點進行優化，保證較高的計算精度。相機捕捉到的圖像作為輸入，輸出內容包括關鍵幀和地圖點，兩者共同構成了地圖。每個關鍵幀也保留了以其為參考的普通幀相對位姿。

視覺里程計在運行時首先會進行初始化操作，通過提取當前圖像中的ORB特征點進行匹配。由于初始化生成的地圖和位姿對整個系統運行的精度有著較大影響，因此初始化過程所提取特征點數量較多。當匹配成功的特征點對數量滿足要求后會三角化生成新的地圖點，并將對應的圖像設置為關鍵幀。在生成初始地圖后系統會對其中的關鍵幀和地圖點位姿進行一次全體優化，保證系統的初始精度，同時會根據關鍵幀的位姿來計算并初始化運動模型。當成功完成初始化工作后，系統會使用運動模型進行追蹤，若系統追蹤失敗則會切換到參考關鍵幀追蹤進行重定位來恢復相機當前的位姿和運動模型。在運行過程中系統會判斷當前是否需要生成新的關鍵幀。當滿足生成關鍵幀的條件時，系統會對當前幀進行ORB特征點提取操作，同時將其與上一關鍵幀中所提取的ORB特征點進行匹配，并通過三角化生成新的地圖點。同時將新的關鍵幀和地圖點插入到滑動窗口中進行優化，剔除冗余關鍵幀和地圖點。該算法框架如圖1所示。

圖1 算法框架

1.1 特征提取

常見的視覺特征點有SIFT特征點、SURF特征點以及ORB特征點。相較于SIFT和SURF特征點，ORB特征點在保持旋轉、尺度不變性的前提下有更高的計算速度，因此系統選取ORB特征點來進行數據關聯。

ORB[14]特征點包含具有方向信息的FAST角點和BRIEF描述子兩部分。FAST角點通過檢測像素灰度變化來確定，其原理如圖2所示。FAST角點通過以下幾個步驟進行提?。?/p>

圖2 FAST角點

1)選取一個像素p，記錄它的亮度I；

2)設定閾值T，并以像素p為中心，選取半徑為3的圓周上的像素點；

3)若有連續N個點的亮度大于I+T或者小于I-T，則將像素點p選取為FAST角點。

傳統的提取方法閾值亮度閾值T固定不變，容易導致在整張圖像中出現FAST角點過于集中和分布不均勻的現象，使得系統計算精度降低。為了緩解該現象，系統在提取FAST角點時會將圖像分割成多個區塊，對每個區塊進行FAST角點提取。若某個區域提取到的FAST角點數量過少，系統會將該區域的亮度閾值T減小，從而提取到更多的角點[15]。當FAST角點提取完畢后，系統會使用四叉樹均分策略剔除掉冗余的角點，使分布更加均勻。

FAST角點方向通過灰度質心法來確定，步驟如下：

1)在一個圖像塊B中定義它的矩為：

mpq=∑x，y∈BxpyqI(x，y)

(1)

其中：p，q={0，1}。

2)圖像塊的質心C為：

(2)

3)設O為圖像塊的幾何中心，通過向量將FAST角點方向定義為：

θ=arctan(m01/m10)

(3)

通過灰度質心法計算得到FAST角點方向可以保證特征點的旋轉不變性。

BRIEF[16]描述子是一種二進制描述子，通過比較FAST角點附近的像素亮度關系可以獲得一串由0和1構成的向量。由于其使用了二進制表達，因此相較于其他描述子有著非?？斓挠嬎闼俣?。旋轉不變性通過FAST角點的方向來確定，尺度不變性則引入圖像金字塔來實現。如圖3所示，通過將圖像按照一定比例縮放形成圖像金字塔，在每一層進行特征點提取，從而保證在特征匹配時的尺度不變性。

圖3 圖像金字塔

1.2 特征匹配

特征匹配用于獲取同一特征點在不同圖像中的像素位置。獲取特征點匹配關系后，系統會通過最小化重投影誤差來得到相機位姿。為了提高系統的運行速度，同時保證計算精度，系統采用了光流追蹤和描述子匹配兩種匹配方法。

運動模型追蹤通過光流追蹤特征點來獲得匹配關系。光流用于描述圖像之間像素的運動，通過光流可以追蹤特征點在不同幀之間的位置從而獲取匹配關系。相較于描述子匹配，光流追蹤可以省去提取描述子的過程，速度更快。算法采用Luckas-Kanade[17]光流，簡稱LK光流，其工作原理如圖4所示。LK光流是一種稀疏光流，用于追蹤稀疏像素，其基于兩個假設：

圖4 LK光流法示意圖

1)灰度不變假設，即空間中任意一點的灰度值在不同圖像中是相同的。圖像中(x，y)處的像素從t時刻運動到t+dt時刻，坐標變為(x+dx，y+dy)，其灰度值不變，用公式表示為I(x+dx，y+dy，t+dt)=I(x，y，t)。對其進行泰勒展開，整理可得：

(4)

2)在同一個窗口內的所有像素會做相同的運動。假設在同一窗口中選取了n個像素點，那么則有：

(5)

當相機快速移動時，光流追蹤特征點容易失敗。為了緩解該現象，系統使用了圖像金字塔來進行光流追蹤。圖像金字塔首先對圖像進行由精至粗進行降采樣處理，同時上層的光流追蹤結果作為下一層的計算初始值，實現由粗至精追蹤光流，從而緩解快速運動帶來的影響。

通過光流追蹤獲取特征點會出現一些誤匹配現象，導致系統計算精度降低，因此系統會進一步使用隨機抽樣一致性(RANSAC，random sample consensus)[18]算法進行過濾，剔除錯誤的匹配關系。通過光流追蹤來獲取特征點匹配關系不需要計算和匹配描述子，因此運行速度有了較大提升。

生成關鍵幀和參考關鍵幀追蹤會使用描述子匹配來獲取特征點匹配關系。描述子用于描述關鍵點周圍的信息，通過對比特征點的描述子可以判斷匹配關系。系統采用的描述子為BRIEF描述子。它是一種二進制的描述子，通過計其漢明距離可以確定特征點之間的匹配關系。傳統的匹配方法在遇到大量特征點時耗時非常大，并且會出現大量誤匹配現象，降低系統的實時性能和計算精度。因此系統采用改進后的匹配方法，通過以下步驟進行特征匹配：

1)通過運動模型提供的初始位姿計算得出特征點在下一幀圖像中的粗略位置，并與位于該位置半徑8個像素以內提取到的特征點進行匹配，選出漢明距離最短且滿足閾值要求的特征點作為匹配點。若未成功匹配，系統則會將半徑提高至14個像素，進一步匹配。

2)根據灰度質心法計算出的特征點方向來計算其在兩幀之間的旋轉角度，并將所有匹配成功的特征點旋轉角度進行統計，將旋轉角度偏離較大的特征點剔除，最后使用RANSAC算法進一步剔除外點，保證系統的整體精度。

1.3 關鍵幀和地圖點

關鍵幀[19]是指一系列連續圖像中最有代表性的一幀，其包含的信息質量很高，相當于普通幀進行過濾和優化的結果。采取關鍵幀策略可以有效降低信息的冗余度，控制系統優化的規模，防止無效或錯誤信息對系統的干擾。系統會提取關鍵幀的ORB特征點，并通過提取到的特征點三角化生成地圖點，將其插入到滑動窗口中進行優化。為了確保系統的穩定性，降低追蹤失敗的機率，關鍵幀的選取采用比較寬松的策略。系統初始化時會將第一幀設置為關鍵幀，三角化生成初始地圖點，并將其位姿設置為坐標系原點。初始化后系統在運行過程中若滿足以下條件之一則會生成新的關鍵幀：

1)追蹤到的特征點數量小于設定閾值；

2)距離上次生成關鍵幀超過10幀；

3)當前圖像與上次生成的關鍵幀旋轉角度或者平移距離超過設定閾值。

關鍵幀之間提取并成功匹配的ORB特征點會用于三角化生成地圖點。由于只采用提取到的ORB特征點來三角化生成地圖點而并非所有的像素點，因此系統生成的是一個稀疏地圖。相較于稠密地圖，建立稀疏地圖可以大大降低資源消耗，提高運行速度，使視覺里程計可以運行在低算力平臺上，同時滿足較高精度的定位需求。

1.4 運動模型追蹤與參考關鍵幀追蹤

系統追蹤過程中主要采取運動模型追蹤，其通過光流追蹤來獲取特征點匹配關系，不需計算和匹配描述子，因此速度較快。運動模型會根據式(6)使用上一幀的初始位姿T1和速度δT來計算當前幀的初始位姿T2，并以T2作為初始值將地圖點投影的位置與相機觀測到的位置做差，構建出一個非線性優化問題，并使用列文伯格-馬夸爾特算法進行優化求解。如式(7)所示，其中K為相機內參，μ為特征點的像素坐標，P為地圖點坐標，T為相機位姿。

T2=δT*T1

(6)

(7)

系統初始化或者追蹤失敗時會采取關鍵幀追蹤，其通過描述子來獲取匹配關系。當系統初始時運動模型為空，此時會進行關鍵幀生成步驟，通過描述子匹配來計算相機位姿，初始化運動模型，隨后使用運動模型追蹤。當運動模型追蹤失敗時，系統會提取當前幀的ORB特征點，并與滑動窗口內的關鍵幀進行匹配，成功匹配后會生成新的關鍵幀并恢復運動模型，繼續使用運動模型追蹤。

1.5 滑動窗口優化

系統生成關鍵幀后，系統在后端會對關鍵幀和地圖點進行優化，其本質是一個非線性優化問題。設zij為相機在Ti處觀察到的地圖點pj產生的數據，那么觀測誤差為：

eij=zij-h(Ti，pj)

(8)

系統所優化的整體代價函數為：

(9)

為了保證系統的運行速度，系統采用滑動窗口算法[20]來進行優化?；瑒哟翱谒惴ㄊ侵府斚到y生成新的關鍵幀后，為了維持一定的計算量，防止優化規模不斷擴大，會設置一個窗口大小，將新的關鍵幀插入，同時將冗余的關鍵幀移除。若滑動窗口內關鍵幀數量沒有達到閾值，系統會對窗口內所有關鍵幀和地圖點進行優化。當關鍵幀數量超過設定的閾值時，系統會對窗口內的關鍵幀判斷，將共視程度最小的關鍵幀設置為冗余關鍵幀，并使用舒爾補將其從滑動窗口中邊緣化；對于沒有追蹤到的地圖點則直接將其從滑動窗口中剔除。相較于直接將關鍵幀位姿固定不進行迭代優化或者直接將關鍵幀約束信息刪除的做法，該方法可以有效保留冗余關鍵幀的約束關系，在保證有較高的計算精度的同時也控制了用于優化的地圖規模，提高了系統的實時性能。

2 實驗結果與分析

實驗首先會驗證特征點提取、光流追蹤特征點以及描述子匹配特征點的效果，進行定性分析。最后在數據集上對整個視覺里程計算法進行實驗，作為對比，會同時對雙目模式下的ORB-SLAM3算法進行實驗，評估兩者軌跡誤差以及實時性能，并對實驗結果進行分析。操作系統采用的是Ubuntu18，CPU型號為AMD Ryzen 7 4800U，主頻1.80 GHz，內存大小為16 GB，不使用GPU加速，特征點提取數量預設為1 000。數據集采用的是KITTI[21]數據集00～10序列，其包含了城市道路、高速公路等多種場景，灰度圖由雙目相機采集并進行了矯正，同時提供了位姿真實數據。

2.1 特征點提取實驗

系統會在初始化過程和生成關鍵幀時會提取圖像中的ORB特征點。傳統的ORB特征點提取方法會出現特征點過于集中的現象，導致計算數值上精度下降。系統所采用的優化后的提取方法可以使ORB特征點均勻分布在圖像中，并且可以將冗余的特征點剔除，避免出現特征點過于集中的現象，從而提升系統高數值計算精度和穩定性。

在KITTI數據集上分別使用傳統ORB特征點提取方法和改進后的方法進行特征點提取，并將結果輸出為圖片進行對比。實驗的結果如圖5所示。

圖5 特征點提取實驗

從實驗結果可以看出，相較于傳統的ORB特征點提取方法，改進后的方法可以有效地將特征點均勻分布在圖像中，避免了特征點集中的現象。

2.2 光流追蹤特征點實驗

追蹤線程主要采用光流追蹤特征點獲取匹配關系，同時以上一幀圖像的位姿為初始值，通過最小化重投影誤差來計算和優化得到當前位姿。光流追蹤可以避免描述子的計算和匹配過程，使得系統有著較高的運行速度。算法采用的是多層光流，相較于單層光流，在高速運動時仍能較好地追蹤特征點。追蹤成功后系統會使用RANSAC算法過濾掉一些錯誤的追蹤信息，提高系統的精度。在KITTI數據集上，分別使用單層光流和系統所采用的多層光流對特征點進行追蹤，并將追蹤結果繪制輸出進行對比。實驗結果如圖6所示。

圖6 光流追蹤實驗

從實驗結果可以看出，優化后的多層光流可以追蹤到更多的特征點，同時錯誤的結果也大大減少。

2.3 描述子匹配特征點實驗

生成關鍵幀時系統會提取當前圖像中的ORB特征點，并使用描述子進行特征匹配。使用描述子漢明距離的傳統匹配方法會出現大量錯誤匹配，在遇到大量特征點時還會嚴重消耗資源，導致最后的運行速度和計算精度降低。系統采取的優化后的匹配方法，可以有效降低誤匹配現象，有著更高的計算精度。在KITTI數據集上首先使用恢復運動模型，用于為粗匹配提供初始位姿。隨后進行特征匹配，將最終匹配結果和傳統方法匹配結果輸出，如圖7所示。

圖7 描述子匹配實驗

實驗結果表明，相比于傳統的匹配方法，改進后的方法可以大大降低誤匹配現象，從而保證了系統的精度。

2.4 視覺里程計整體性能實驗

評價視覺里程計主要包括軌跡誤差以及實時性能兩個指標。軌跡誤差指標采用絕對平移誤差來評估，其計算公式如式(10)所示：

(10)

實時性能則通過計算平均追蹤每一幀圖像的耗時來評定。

在KITTI數據集00～10序列上，分別測試所提視覺里程計算法和雙目模式下的ORB-SLAM3算法。由于所提視覺里程計算法沒有回環檢測模塊，因此在精度評估時還會測試雙目模式下去掉回環檢測功能的ORB-SLAM3算法，并對結果進行對比分析。每次測試前均會將電腦進行重啟操作，避免每次運行實驗結束后對接下來的測試造成影響。運行時系統會記錄追蹤每一幀所消耗的時間；運行結束后保存生成的數據集位姿數據，通過與真實位姿對比計算絕對平移誤差，同時計算追蹤每一幀的平均耗時。計算絕對平移誤差使用的工具為evo，它是一款用于視覺SLAM和視覺里程計軌跡評估的工具，可以評估軌跡與真值之間的誤差，并且支持多種數據集軌跡格式之間的相互轉換。計算追蹤每一幀所消耗的時間則由系統內置函數實現。為了保證結果的準確性，每個算法均測試5次，取最后的平均值作為結果進行分析。

圖8顯示了所提視覺里程計算法在KITTI數據集各序列上建立的稀疏地圖。相較于稠密地圖，稀疏地圖消耗的資源更少，可以僅使用CPU而不借助GPU加速即可生成和維護，并且可以滿足定位需求。

圖8 視覺里程計在 KITTI數據集00～10序列上建立的稀疏地圖

表1顯示了在KITTI數據集上的軌跡精度。從表中數據可以看出，在00、02、05～09等序列上發生了回環場景，因此有回環檢測模塊并且進行了全局優化的ORB-SLAM3算法精度最高，關閉回環檢測功能后精度明顯下降；所提出的視覺里程計算法由于在滑動窗口中采取了邊緣化關鍵幀的策略，保留了被剔除關鍵幀的約束信息，因此在所有序列中軌跡誤差相較于關閉回環檢測模塊的ORB-SLAM3算法更低，擁有著更高的精度水平，在發生回環場景中的序列精度會略低于有回環檢測功能的ORB-SLAM3算法。在沒有發生回環場景的03、04、10等序列上所提出的視覺里程計算法有著非常高的精度，與具有完整回環檢測模塊和全局優化的ORB-SLAM3算法精度相當。綜上可以看出，在所有序列下所提視覺里程計算法精度均高于去掉回環檢測功能的ORB-SLAM3算法；在無回環的場景下精度與進行了回環檢測和全局優化的ORB-SLAM3算法處于同一水平，僅在大范圍有回環場景的情況下精度有所下降，但仍處于較高水平。

表1 絕對平移誤差對比

表2顯示了算法在KITTI據集上平均每追蹤一幀圖像所需的時間。由于所提算法在追蹤線程主要通過光流追蹤特征點來獲取匹配關系，省去了重復計算描述子的過程，因此運行速度有了極大的提升。相比于雙目模式下的ORB-SLAM3算法，所提出的視覺里程計算法追蹤每一幀所花費的平均時間由52 ms降低到16 ms。其中序列01是高速公路場景，載具處于高速運動狀態，為了保證追蹤穩定系統會更頻繁地生成關鍵幀，因此處理時間相比其他低速場景會有所增加，但是仍低于ORB-SLAM3算法；而在城市路段中低速場景下，追蹤每一幀的耗時僅需不到20 ms，實時性能有著巨大的提升。綜上可以看出，所提視覺里程計算法在實時性能上有著巨大的提高，在低算力設備上有著明顯的優勢。

表2 追蹤每一幀圖像平均耗時

3 結束語

針對特征點法視覺里程計頻繁計算和匹配描述子導致系統實時性能變差的問題，提出一種結合光流追蹤的雙目視覺里程計算法。該算法在追蹤線程主要通過光流追蹤特征點來獲取匹配關系，計算并優化相機位姿；生成關鍵幀時提取和匹配ORB特征點，并三角化出新的地圖點，隨后將新的關鍵幀和地圖點插入滑動窗口中進行優化。由于該算法大部分時間采用光流追蹤，不需要計算和匹配描述子，同時采用滑動窗口算法來優化，保留了被剔除的關鍵幀約束關系，因而有較快的運動速度和較高的精度。在KITTI數據集上的實驗表明，所提視覺里程計算法可以在較高精度的情況下，有著非?？斓倪\行速度，具有較高的實用價值。由于本文所提出的視覺里程計算法沒有加入回環檢測和全局優化功能，因此不是一個完整的視覺SLAM系統，在有回環的場景下精度有所降低。因此加入回環檢測和全局優化功能是下一階段的主要研究內容。