基于光束法平差的RGB-D視覺里程計

劉祥

（上海海事大學信息工程學院,上海 201306）

0 引言

視覺里程計一直是機器視覺中研究的熱點問題。視覺傳感器，例如數碼相機，已經成為機器人導航系統中不可缺少的組成部分，通過相機獲取并處理圖像，獲得機器人當前的位置、姿態和距離等信息，簡稱視覺里程計。它僅僅通過機器人攜帶的相機獲取到的圖像，來追蹤和判斷機器人當前的位置和姿態，為機器人導航提供有效依據。為了估計機器人的運動情況，傳統方法采用激光傳感器，利用迭代最近點法對激光點云數據進行匹配，從而求解機器人的位姿。它的算法思想是在兩次掃描點云數據之間重復進行選擇對應關系點對，計算最優的剛體變換，直到滿足正確匹配的收斂精度要求。在實際應用中，由于初始值的選擇不確定極易出現局部最優解的情況，這在一定程度上限制了它的應用。相反的，視覺里程計法成為當下的主流方法，它通過對圖像進行特征提取、特征匹配、位姿估計以及優化等完成室內定位任務。

視覺里程計的發展已經有20多年了，在國內外廣泛應用于商業和軍事導航系統中。將視覺里程計技術應用于機器人中，機器人可以追蹤自己的位置，進行定位，同時可以構建周圍環境的地圖。隨著圖像處理技術的發展，也因視覺傳感器方便攜帶，功耗低，無需GPS等優點，視覺里程計技術在室內導航和定位得到了廣泛的應用。使用普通的相機一直是視覺里程計的經典方法，而近幾年推出的商業傳感器可以同時捕獲彩色圖像和深度圖像，即RGB-D傳感器，它無需三角測量等算法便可得到圖像深度數據，在三維重構和里程計方面有著巨大的應用價值。

考慮到實時性和實用性，在對圖像序列進行特征提取時，并沒有采用效果最好的SIFT特征，而是選用了ORB特征，該特征具有較好的尺度不變性和旋轉不變性，同時能達到工程實用的目的。首先，根據特征描述子對相鄰時間序列的圖像進行匹配，采用隨機抽樣一致法進行反復迭代，剔除誤匹配，求出最優的本質矩陣。然后通過矩陣分解，得到旋轉量和平移量，從而恢復出機器人的運動軌跡和姿態。最后采用光束法平差優化，對局部位姿和周圍的點云地圖進行局部和全局優化，從而減小里程計累積漂移誤差并提高定位精度。

1 視覺里程計算法

1.1 特征提取與匹配

特征點的提取和選擇，首先要考慮到處理每幀圖像不能耗時過多，同時不能占用過多的CPU資源，為此我們選用ORB特征。它是一種提取圖像視覺特征的算法，包括特征點的檢測和特征點的描述兩部分，它采用FAST角點檢測作為關鍵點，采用BRIEF描述子做特征描述[1]。FAST角點具有運算速度非常快的優點，同時可以檢測出豐富的角點特征，但是不具有方向性。ORB算法在其基礎上進行改進，通過定義形心，規定了特征點的主方向，使其具有了旋轉不變性。同時，通過圖像金字塔，在每層圖像上進行角點檢測，使其具有較好的尺度不變性。FAST特征點如圖1所示：

圖1 FAST特征點

FAST特征點的檢測和判定原則：通過對比中心像素點與周圍像素點的像素值的灰度差異，超過某個閾值時即判定該像素為特征點。通過（1）式角點響應函數可以判斷FAST特征點。

式中：I（x）表示待測點周圍內可選的隨機點的灰度值。

ORB算法首先計算特征點周圍的灰度的形心，再將特征點到該特征點周圍的圖形形心的方向作為該特征點的主方向[2]，局部區域矩的公式如下：

則計算出特征點周圍的區域內的圖像的灰度形心為：

此時則可以判定FAST特征點的主方向為：

ORB特征點的描述采用BRIEF描述子，它是由二進制位組成的一個向量，每個二進制位的0和1代表了特征點周圍像素的大小關系。我們用特征點周圍的128個像素點比較大小關系后得到128維的向量來描述該特征點[3]。通過幀與幀，或者幀與地圖之間的特征描述子的匹配，我們可以估計圖像之間的變換關系并進行位姿優化等。由于我們采用了BRIEF描述子，所以需要采用漢明距離來衡量描述子的相似程度，然后確立特征點的對應關系。

由于圖像特征比較豐富，且需要匹配當前幀和地圖之間的關系，若采用暴力匹配法則所需時間過久，所以采用快速近似最近鄰算法，該算法適合于匹配點數量極多的情況。該方法是根據K均值或KD-Tree搜索操作所實現的，可以根據數據集的分布特點,對映射精度和空間資源消耗的要求來推薦索引類型和檢索的參數，在高位空間最近鄰查找不受局部敏感哈希影響。快速近似最近鄰算法模型的特征空間一般是n維的實數的向量空間，它的關鍵在于使用歐氏距離找到特征點的鄰居。圖2是特征提取和匹配的結果：

圖2 特征提取與匹配

1.2 剔除誤匹配

特征匹配后我們可以看到，并不是每一個特征點都能正確匹配，存在一定的誤匹配。因此我們選用RANSAC算法來剔除誤匹配[4]，該算法的核心思想是根據兩幀圖像之間的對極約束關系，并通過多次反復迭代來剔除誤匹配，并根據最優的匹配對計算出本質矩陣。該算法的具體做法是：

①先設定采樣的次數N，有效匹配點與對極線間的距離閾值M；同時將相鄰兩幀圖像的所有特征點的像素坐標做歸一化處理，分別得到歸一化的變換矩陣。

②在匹配點集中隨機選取八個點計算本質矩陣。

③利用本質矩陣分別與其余的匹配點計算匹配點與對極線之間的距離。

④找出尚未匹配的點集中，對極線距離滿足小于閾值M的匹配點。

⑤統計滿足上述條件的特征點的數ni和特征點的索引號。

⑥反復執行上述步驟，直到誤差函數取值達到最小，并且標記對應的索引號，則我們認為此時的匹配滿足最優的匹配。

⑦最后我們用最佳的匹配點集計算出本質矩陣。

該算法是通過反復迭代，并不斷更新基本矩陣的值，找到最佳匹配關系[5]。采樣次數N設置過大，會增加處理每幀圖像的時長，設置過小則不能找到最佳的匹配對，算法失效，起不到剔除誤匹配的效果[6]。下圖是經過RANSAC算法后，誤匹配已經明顯減少。

圖3 剔除誤匹配后

1.3 位姿估計

相同一個攝像機采集的兩幀圖像之間是有很強的約束關系的，它們之間滿足下式：

其中X'和X分別代表匹配圖像上的一對匹配特征點的像素坐標，F是基本矩陣，表明了兩幀圖像之間的變換關系，它是一個秩為2的3×3的齊次矩陣。

采用歸一化8點法計算基本矩陣，它的重要思想是在求取基本矩陣之前對數據進行歸一化，這樣做并不會顯著增加計算量，并且可以容易的求取矩陣[7]。下面給出8點算法的具體步驟：

①歸一化：根據X,TX變換圖像的像素坐標，其中T為歸一化的矩陣變換，包括平移和縮放兩部分。

②由最小奇異值的奇異矢量確定基本矩陣F，即得到線性解。也可以由其行列式的值為零得到強迫約束解。

(三)新會計制度中增加預算會計的概念。使得單位會計要素包括財務會計要素和預算會計要素，實行財務會計和預算會計平行記賬法，單位對于納入部門預算管理的現金收支業務，在采用財務會計核算的同時又進行預算會計核算，增加了預算收支、預算結余的賬務處理，不僅清晰反映了財務主體的運行狀況還有效反映和監督了會計主體預算收支執行情況。

③解除歸一化，得到基本矩陣F。

得到基本矩陣后，并在保持投影點不變的情況下，對基本矩陣分解得到四個可能的解。如圖4所示。

圖4 四種可能情況的解

我們根據P點應該在兩幅視圖中都具有正的深度即可篩選出正確的解。分解基本矩陣后得到R和T，R是一個正交旋轉矩陣，T是一個平移矩陣。求解出R和T，即完成相機運動的估計，得到相機的運動軌跡和姿態[8]。

1.4 光束法平差優化算法

估計相機的位姿和3D點的空間位置一直是計算機視覺的經典問題，優化相機位姿和圖像中的特征點，在視覺里程計中是很重要的一個部分[9]。采用光束法平差對路標點和相機位姿進行優化，在給定相同場景不同圖像當中的多個2D點時，則光束法平差可以表示為一個非線性最小二乘問題：

求解這個方程的最小值，需要獲得較多的觀察圖像，才能得到一個較好的解。在實際操作中，通過迭代的方式來求解。通常采用L-M梯度下降策略來求取最優位姿,在求解過程中如果能提供良好的初始值，則算法會快速收斂[10]。在此之前，可用8點法求解初值。其次是步長的控制，選擇一個合理的步長算法會立即收斂，否則算法可能會在幾個谷底之間跳轉，出現不收斂的情況。

使用局部的光束法平差對關鍵幀和路標點進行優化，這樣有利于提高局部的定位精度，同時也可以減少累積誤差。在構建好地圖之后，采用全局的光束法平差對整體的位姿和地圖中的路標點進行優化調整。光束法平差的應用，有利于減少噪聲對系統帶來的影響，它同時調整圖像的觀測值和估計值，使整個系統得到持續的優化。

2 算法實現及結果分析

算法在Ubuntu14.04操作系統實現，使用了開源的計算機視覺庫OpenCV，并且采用C++11新標準編程，提高了系統整體的穩定性。在程序設計過程中，一個幀通常包括了一個圖像和特征點、位姿、內參等信息，由于相機采集的數據很多，所以我們用關鍵幀來代表整個相機的運動軌跡，同時利用關鍵幀恢復出運動和三維的點云地圖和空間結構。此外，地圖點對于定位來講，可以提供豐富的位置信息，我們還可以將新的幀與路標點進行匹配，從而估計運動信息[11]。對于基于特點的視覺里程計來講，最關鍵的是相鄰兩幀圖像之間的運動估計，即當前幀與參考幀的匹配關系，下面給出它的操作步驟：

①對采集到的新的當前幀，提取關鍵點和描述子。

②如果系統沒有初始化，則以該幀為參考幀，并且根據對應的深度圖來計算特征點的3D位置信息，然后返回①。

④判斷上述估計是否成功。

⑤如果算法成功，則把當前幀作為新的參考幀，返回①。

⑥如果失敗，我們統計連續丟失的幀數，當連續丟失的幀數過大，則設置里程計的狀態為丟失，算法結束。若未超過，返回①。

本算法最終在廣泛使用的TUM數據集上進行測試，TUM的數據集通過外部設備和GPS獲取了標準軌跡，同時提供了一些比較工具和腳本程序，適合做研究。本次試驗從三維重構結果、定位精度、旋轉誤差和平移誤差來對算法進行衡量。三維重構結果如圖5所示，其中黃色線代表的是相機的運動軌跡，從三維重構的結果來看，空間點的3D位置還是比較準確的，同時也基本恢復了場景的結構。

圖5 場景的三維重構結果

圖6的定位結果中，紅色線代表的是估計軌跡與標準軌跡的差值，可以看到在數據集的多個場景下，算法表現優秀，特別是在長時間運行的（d）數據集中，系統的累積漂移誤差很小。接下來，對比光束法平差算法和普通視覺里程計的旋轉誤差和平移誤差，在不同數據集的實驗結果見表1。

表1 旋轉和平移誤差結果

通過對比研究，我們可以明顯的看到使用光束法平差優化后，算法能有效減小幀與幀之間的旋轉和平移誤差，由此提高了里程計的定位精度。

3 結語

通過加入光束法平差優化算法與普通視覺里程計進行對比研究，驗證了算法的有效性。從實驗結果看，該算法可以有效地減少平移誤差和旋轉誤差，提高定位精度，并且在一定程度上，減小了里程計的累積誤差。

圖6 定位結果分析