標題：SVO:Semidirect Visual Odometry for Monocular and Multi-Camera Systems

作者：Forster Christian, Zhang Zichao, Gassner Michael, Werlberger Manuel, Scaramuzza Davide

來源：TRO 2017

今天我們要精讀的文章事來自UZH的svo2。這篇工作在2016就發表瞭, 但是源碼在2021才釋出. 本文提出的半直接視覺裡程計方法很好的結合瞭直接法的精度、魯棒性， 和特征點法能夠保證計算效率、全局一致性的優點, 在運行速度和全局精度兩個指標上取得瞭很好的結果. 同時, 本文提出瞭一種魯棒的深度估計算法, 使得系統可以在較弱紋理區域也能正常運行. 本文方法在若果數據集上都去瞭sota的表現.

摘要:

直接法視覺裡程計的優勢得到普遍認可, 但是在運行速度、全局一致性等指標上明顯劣於特征點法視覺裡程計. 借鑒兩種方法的優點, 我們提出瞭半直接法視覺裡程計: 使用直接法跟蹤梯度較大的圖像區域, 使用特征點法優化地圖點和相機位姿. 同時, 本文提出瞭一種基於概率的魯棒的深度估計算法, 使得我們在低紋理、重復紋理場景中也能很好的實現跟蹤. 本文方法可以拓展到廣角相機、多相機系統, 同時可以將其他形式的運動先驗信息(imu/gps/wheel-odometry)加到系統中. 在多個數據集上測試驗證瞭本文方法的優越性.

算法流程:

本文方法的整體框圖如下:

運動估計/跟蹤線程使用本文提出的半直接法來完成運動估計, 具體分為三步:

• 稀疏直接法把當前幀圖像與上一幀圖像對齊, 估計幀間運動;
• 從上一個關鍵幀中重投影地圖點, 使用重投影像素塊對齊當前幀的2D特征點, 優化幀間運動運動造成的漂移;
• 最小二乘優化相機位姿和地圖點, 消除第二步2D特征對齊引入的對極誤差;

建圖線程使用本文提出的深度估計濾波器對每個關鍵幀上的特征點/邊進行深度估計, 當濾波器的方差低於閾值時則認為該點/邊的深度穩定瞭，立即構建一個地圖點.

Notation:

相機 C ​在時刻 k ​處的灰度圖記為 I_k^c:Omega^c subset mathbb R^2 ​, ​ Omega^c 表示圖像域. 空間一點​ mathbb rho in mathbb R^3 通過相機投影模型 mathbb u = pi (mathbb rho) ​映射到圖像坐標 u ​. 給定圖像坐標 mathbb u in mathcal R^c_k ​以及它對應的逆深度​ rho , 則它對應的空間點可以通過反投影得到 mathbb{rho} = pi^{-1}_{rho} (mathbb u) ​, 這裡是假設​ mathcal R^c_k 區域內的深度值是已知的. 第​ k^{th} 幀圖像的位姿記為 T_{kw} in SE(3) ​, 表示將世界坐標系下的點 _w mathbb rho ​變換到當前幀相機坐標系​: _k mathbb rho = T_{kw} cdot _w mathbb rho

運動估計

運動估計過程中, 我們假設已知深度信息, 比如要用到的角點/邊的深度值.

A. 稀疏圖像對齊:

類似於直接法LSD-SLAM, 本方法中的圖像對齊算法也是最小化光度誤差, 但是本文圖像對齊算法隻對地圖點對應的角點所在的圖像塊最小化光度誤差. 估計幀間運動​ T_{kk-1} = T_{B_kB_{k-1}} 的目標函數為:

光度誤差​ r_{I^c_u} 是地圖點 mathbf{ rho_u} ​在相鄰圖像幀​ I^c_{k-1} 、 I^c_{k} ​的觀測的強度誤差:

因此, 本文方法在初始化時仍然需要提取稀疏特征點, 而不是像LSD-SLAM那樣直接提取圖像的灰度梯度較大的區域. 這裡需要解釋清楚的是: dense方法是指利用參考圖像幀上所有像素點和深度信息來做幀間運動估計, semi-dense方法指的是利用圖像上像素梯度較大的像素點來做幀間運動估計, 而本文是隻使用一些稀疏的角點/邊來進行幀間運動估計.

B. 2D特征點對齊:

考慮到幀間運動估計極容易產生漂移, 因此我們需要把當前幀與歷史幀再做一次對齊來消除漂移. 這裡引入關鍵幀策略, 關鍵幀上會將地圖點(事先提取特征點(FAST), 利用圖像位姿和深度信息反投影得到)投影到當前幀, 然後把這些重投影的特征點擴大為8x8的像素塊, 特征點對齊問題轉換為2D像素塊的對齊優化問題. 像素塊的對齊過程考慮瞭這兩幀圖像之間的相對位姿變換引起的二維圖像的仿射變換, 最小化像素塊中的梯度直方圖誤差來更新當前幀上的特征點位置 mathbf {u'^*} ​; 對corner feature, 最小二乘優化目標函數如公式(4): 其中 mathbb u' ​為關鍵幀 r ​的地圖點在當前幀​ k 上的的投影點, ​ mathcal P 為以​ mathbb u' 為中心的一個像素塊, ​ Delta {mathbb u} 為計算​ mathcal P 中像素梯度和的一個迭代算子:

Edge feature的最小二乘優化目標函數公式(5). 我們減少edge特征的維度為1維, 使edge特征隻沿著edge法向量​的方向優化:

9c341b333138ba4e07deb6adccf63009

C. 優化相機位姿和地圖點

通過2D特征點對齊, 我們建立瞭瞭亞像素級的特征點對應關系feature correspondences. 由於步驟B改變瞭當前幀上特征點的"觀測位置"(通過重投影地圖點得到, 並非是提取得到的), 因此還需要進一步優化當前幀位姿和地圖點 chi = {T_{kw}, rho_i} ​:

環境建圖:

A. 深度濾波器

在運動估計環節中, 我們用到瞭每個關鍵幀上的特征點, 並且假設這些特征點的深度是已知的. 但是對於關鍵幀上新提取的特征點, 它的深度是對其在多個圖像上的觀測進行遞歸貝葉斯濾波得到的.

關鍵幀的選取策略是當corner和edge feature的跟蹤長度小於某一閾值時, 就把當前幀選做關鍵幀. 所以每個depth filter其實屬於某一個關鍵幀上的一個特征點, depth filter的逆深度不確定度初始化為很大的一個值. 我們稱該特征點的像素塊為reference patch, 由於新提取的特征點在其他普通幀/關鍵幀上並沒有事先提取, 所以需要在這些已知相對位姿​ {I_k, T_{kr}} 的關鍵幀/普通幀上, 根據對極線來搜索有最小光度誤差的patch correspondence, 然後三角化該特征點得到深度值 tilde rho_i^k ​ 來送入depth filter.

當有足夠多的depth estimation送入depth filter後, depth filter的不確定值低於一定閾值後, 我們就將該特征點三角化, 用於運動估計環節. 對於edge feature使用同樣的方法來計算深度, 但是不考慮法線方向與對極線方向平行的edge feature.

理想情況下, 我們希望使用類似蒙特卡洛的方法來估計特征點逆深度真值, 但是這樣復雜度太高. 如Fig.6所示, 我們對比瞭用Gaussian分佈和Gaussian+Uniform分佈描述逆深度​的效果區別:

79ed7e565520192776f609e64f4e395c

具體的, Gaussian+Uniform分佈模型包含逆深度變量 rho ​和內點概率​ r :

d17bb8c72f2bf7848bbd3edcee758620

這一章節的理論部分可以參考<<Video-based, Real-Time Multi View Stereo>>的"Probabilitic depth sensor"小結.

Fig.6和Fig.7的實驗都證明瞭Gaussian+Uniform分佈模型的depth filter能夠更好的應對噪聲外點:

B. 多相機系統

由於我們定義瞭body坐標系, 假設多相機系統 c in mathbf C 的任一臺相機​的外參 T_{CB} ​已知, 那麼在運動估計環節我們隻需要把每個相機的光度誤差和重投影誤差加入到優化目標函數中. 比如在稀疏圖像對齊步驟中, 把公式(1)改寫為公式(11):

在優化相機位姿和地圖點步驟同理.

運動先驗

在一些快速運動以及無紋理場景, 我們支持加入一些運動先驗來應對視覺運動估計環節出錯的問題. 如公式(12)所示, 我們隻需要把運動先驗(IMU或者GPS)加入到幀間運動估計的目標函數中即可:

實現細節:

A. 初始化

系統的初始化使用前兩幀關鍵幀, 提取稀疏特征點(FAST), 並完成初始化地圖. 對於多目相機, 初始地圖的位置由兩兩雙目匹配的均值確定.

B. 稀疏圖像對齊

圖像對齊過程, 我們采用4x4的像素塊來完成幀間運動的估計. 為瞭應對劇烈運動, 我們設計瞭一種coarse-to-fine的圖像對齊策略, 本質就是不同分辨率的圖像金字塔, 這裡我們使用瞭5 level金字塔, 實際上在第3層就結束圖像對齊過程的計算.

C. 特征對齊

特征對齊階段, 我們使用瞭8x8的像素塊. 考慮到參考關鍵幀可能距離當前關鍵幀較遠, 我們設計瞭一個仿射變換矩陣來處理照明條件的變化. 整個算法運行時, 我們限制特征點數量<180.

D.建圖

在建圖過程, 我們把圖像分割成若幹個32x32的單元, 每個關鍵幀上, 我們隻對每個單元中一個FAST角點初始化一個深度估計濾波器. 對於沒有提取到FAST角點的單元, 我們去提取梯度最大的邊特征並且初始化深度估計濾波器. 深度估計需要匹配前後兩幀特征點/邊, 由於我們已知幀間運動, 因此我們這裡隻在對極線附近搜索特征點/邊. 這裡我們使用瞭一個8x8大小的像素塊來進行對極搜索.

實驗結果:

我們首先在合成數據集上測試瞭本文提出的圖像對齊算法, 隨後在22個不同數據集序列上驗證瞭整個算法流程, 並且與其他sota算法對比.

A. 稀疏、半稠密、稠密的圖像對齊算法測試結果:

我們在Urban Canyon數據集上測試本文提出的稀疏圖像對齊算法、半稠密圖像對齊算法、稠密圖像對齊算法. 我們任選一幀 I_r ​作為參考幀, 計算其後60幀圖像​ k in {r+1,…,r+60} 相對於參考幀的相對位姿 T_{rk} ​, 兩幀間距0.2m, 每幀圖像的平均深度約為12.4m. 對齊算法在每個圖像對​上運行800次, 並且施加瞭一定的初始噪聲. 相對位姿的估計值與真值偏差小於0.1m的則認為是收斂的估計結果.

對於本文提出的稀疏圖像對齊算法, 我們在參考幀​ I_r 中提取瞭100個FAST角點並且根據已知深度三角化出角點對應的地圖點. 我們測試瞭不同的像素塊大小:1x1到5x5的消融實驗，並且和半稠密法:LSD和稠密法DTAM的圖像對齊結果對比, 對比結果如圖所示:

從Fig 11可以看出稀疏圖像對齊法在​距離​較遠時精度遠差於半稠密法, 所以我們隻將稀疏圖像對齊算法應用於 I_k ​與​ I_{k+1} 之間, 而不是應用於關鍵幀上.

B. 在真實/合成數據集上整體算法測試

我們把本文提出的算法與其他sota算法在EuRoC、TUM、ICL-NUIM數據集上進行測試對比.

EuRoC上的測試結果表明: svo在雙目數據集上能夠取得較好的精度, 但是在單目數據集上精度劣於ORB-SLAM/DSO. 但是從運行時效率上, svo更勝一籌:

SVO運行速度快是因為它的圖像前端並沒有提取特征點和描述子, 而隻是在關鍵幀上提取一定數量的FAST.

TUM數據集上測試結果發現LSD-SLAM/ORB-SLAM都優於svo, 可能是因為他們開啟瞭回環檢測的原因.

ICL-NUIM數據集是一個合成的RGB-D slam數據集， 該數據集因為有很多純旋轉的運動以及比較困難的紋理場景, 所以比較具有挑戰性. 我們在4段軌跡上運行瞭5次不同的vo算法, 統計對比如下表:

討論:

A. Efficiency

svo不需要提取特征, 直接通過對齊像素塊的梯度值來估計幀間運動, 特別適用於高幀率的相機.

B. Accuracy

svo通過直接對齊2d feature來獲得亞像素級精度, 然後基於重投影誤差優化地圖點和相機位姿. 對於精度要求不高的額應用場景, 我們隻需要對當前幀圖像位姿進行優化, 否則就對所有歷史幀進行BA優化.

C. Robustness

因為svo不提取特征點, 所以可以很好的應對紋理較少或者重復紋理的場景, 本文提出的深度估計濾波方法也比傳統的多視角重建地圖點的方法要好，因為濾波方法在喂入更多的觀測數據後會更加準確並且對outlier不敏感.