（每日一讀2019.10.23）低漂移、魯棒和快速的視覺-激光里程計和建圖（VLoam）

參考：https://www.jianshu.com/p/cb7098567711
論文：pdf

摘要

本文開發了一個低成本的立體視覺慣性定位系統，該系統利用有效的基於多狀態約束卡爾曼濾波（MSCKF）的視覺慣性里程計（VIO），同時利用先驗激光雷達圖提供有界誤差的三維導航。除了VIO中使用的標準稀疏視覺特徵測量外，在緊密耦合的MSCKF更新中，還利用視覺半稠密點雲到先前激光雷達地圖中進行全局校準，從而校正累積漂移。特別討論了視覺點雲和激光雷達點雲之間的交叉模態約束。通過montecarlo模擬和實際實驗驗證了該方法的有效性，結果表明，通過不同的探測方式建立的雲之間的lidar映射約束極大地提高了標準vio，並提供了有界誤差性能。

引用

最近在視覺里程計和激光雷達里程計方面取得的獨立成果很有希望，因爲它們可以爲六自由度狀態估計、建圖甚至障礙物檢測提供解決方案。單獨使用每一個傳感器都存在缺點。

• 視覺的缺點：視覺里程錶方法需要適度的照明條件，如果沒有足夠的視覺特徵，則會失敗。
• 激光的缺點：通過移動激光雷達的運動估計涉及點雲中的運動失真，因爲在連續激光雷達運動的不同時間接收距離測量。因此，運動常常需要用大量的變量來求解。在退化的場景中，例如那些由平面區域控制的場景，掃描匹配也會失敗。

在此，我們提出了一種單目相機和三維激光雷達相結合的自我運動估計的基本原理和第一原理方法。我們希望實時準確地估計6自由度運動以及環境的空間、度量表示，並在未知環境中導航機器人。雖然相機和激光雷達具有互補的優勢和劣勢，但使用傳統的濾波方法結合將他們比較難。我們的方法是緊耦合了這兩種模式，因此它既能處理包括平移和旋轉在內的快速運動，又能處理完全白化或黑化圖像中缺少光學紋理的情況。在非病態條件下，高精度的運動估計和環境重建是可能的。

我們提出的方法，即V-LOAM，探索了每種傳感器的優點，並彌補了另一種傳感器的缺點，因此在性能上比現有技術有了進一步的改進。該方法有兩個順序交錯的過程。第一種方法使用高頻視覺里程計作爲圖像幀速率（60hz）來估計運動。第二種方法使用低頻（1hz）的激光雷達里程計來改進運動估計，並消除由視覺里程計漂移引起的點雲失真。無失真點雲被匹配並註冊爲增量構建地圖。其結果是，視覺里程計處理快速運動，激光雷達里程計保證在不理想的照明條件的低漂移和魯棒性。我們發現這些地圖通常是準確的，無需進行後期處理。雖然環路閉合可以進一步改善地圖，但我們有意選擇不這樣做，因爲這項工作的重點是推動精確的里程估計的極限。

V-LOAM的基本算法具有一定的通用性，可以適用於不同類型的距離傳感器，如飛行時間攝像機。如果先前的映射可用，也可以將該方法配置爲僅提供本地化。

除了對Kitti里程計基準的評估[1]，我們還進一步試驗了廣角攝像機和魚眼攝像機。我們的結論是，魚眼相機具有更大的視場和更高的圖像失真，因而具有更強的魯棒性，但精度較低。然而，經過掃描匹配細化後，最終的運動估計達到了相同的精度水平。我們的實驗結果可以在公開的視頻中看到。

II 相關工作

基於視覺和激光雷達的方法通常用於狀態估計[2]。對於立體相機[3]，[4]，基線提供了幫助確定運動比例的參考。然而，如果使用單目相機[5]-[7]，在不借助其他傳感器或運動假設的情況下，運動的尺度通常是不可解的。RGB-D相機的引入爲視覺圖像與深度的關聯提供了一種有效的方法。使用RGB-D相機進行運動估計[8]，[9]可以很容易地進行縮放。一些RGB-D視覺里程計方法也被提出，顯示出有希望的結果[10]-[12]。然而，這些方法僅利用深度可用的成像區域，可能在沒有深度覆蓋的視覺圖像中浪費大量區域。我們系統中使用的視覺里程錶方法與[8]-[12]相似，因爲它們都使用具有附加深度的視覺圖像。然而，我們的方法是利用激光雷達的稀疏深度信息。它包括有深度和無深度的特徵。

對於3D建圖，一個典型的傳感器是（2軸）3D激光雷達（13）。然而，這些激光雷達的使用是困難的，因爲激光雷達不斷範圍和移動，點雲存在運動失真。消除失真的一種方法是結合其他傳感器來恢復運動。例如，Scherer等人的導航系統[14]使用與IMU集成的立體視覺里程計來估計微型飛行器的運動。激光雷達雲是通過估計的運動來記錄的。Droeschel等人的方法[15]採用了多攝像機視覺里程計，然後是基於多分辨率點雲表示的掃描匹配方法。與[14]、[15]相比，我們的方法的不同之處在於，它將相機和激光雷達緊密耦合，這樣運動恢復只需要一個相機。該方法還考慮了視覺里程計漂移引起的點雲畸變，即在短時間內（1s）將漂移建模爲線性運動，並在掃描匹配過程中用線性運動模型進行校正。

典型的去除運動畸變的方法是在激光幀之間進行插值（比如視覺，輪速計，IMU等）

過去的研究還表明，狀態估計只能用三維激光雷達。例如，Tong等人匹配在由2軸激光雷達堆疊激光掃描產生的強度圖像中的視覺特徵，以解決運動（16）。運動採用勻速高斯過程建模。然而，由於該方法從激光圖像中提取視覺特徵，需要密集的點雲。另一種方法來自bosse和zlot[17]，[18]。該方法匹配局部點簇的幾何結構。他們使用由二維激光雷達和IMU組成的手持式繪圖設備，IMU通過彈簧連接到手杆上[17]。他們還使用多個2軸雷達地圖繪製地下礦[ 18 ]。該方法通過對分段數據進行批量優化處理，在分段之間加入邊界約束，實現軌跡的恢復。該方法適用於離線調查，但不適用於在線實時應用。

提出的方法基於我們在[19]，[20]中的工作，其中視覺里程計方法DEMO和激光雷達里程計方法LOAM分別被提出。LOAM需要平滑運動，並依賴於IMU來補償高頻運動。本文對LOAM進行了改進，使新方法V-LOAM以視覺里程計的輸出作爲運動先驗，然後是lidar里程計。視覺里程計中的攝像機模型也進行了修改，並與魚眼攝像機兼容。實驗結果表明，v-loam具有較低的漂移。結合了高頻視覺里程計和魚眼相機也使系統能夠處理快速運動。

III 座標系和規定

本文所要解決的問題是估計相機和激光雷達系統的運動，並用所估計的運動建立被穿越環境的地圖。我們假設攝像機是由一般的中央攝像機模型[21]建模的。有了這樣的相機模型，我們的系統可以同時使用普通相機和魚眼相機（見實驗部分）。我們假設攝像機的內部參數是已知的。同時對相機與激光雷達之間的外部參數進行了標定。這允許我們對兩個傳感器使用一個座標系，即傳感器座標系。爲了計算簡單，我們選擇傳感器座標系與攝像機座標系一致-所有激光點在接收時都投影到攝像機座標系中。作爲本文的慣例，我們使用左大寫來表示座標系。在下面，讓我們定義

• 傳感器座標系 {S} 爲攝像機光學中心。X軸指向左邊，Y軸指向上，Z軸點與攝像機主軸一致。
• 世界座標系 {W} 是在起始位置與{S}同向的座標系。

在定義了假設和座標系的情況下，我們的里程測量和地圖繪製問題被描述爲:
在給定視覺圖像和激光雷達雲圖的情況下，確定{S}相對於{W}的姿態，建立{W}中環境的地圖。

IV 系統總結

圖2示出了軟件系統的示意圖。整個系統分爲兩部分。視覺里程計主要估計幀間運動，頻率爲圖像的幀率。特徵跟蹤模塊提取並匹配連續圖像之間的視覺特徵。深度地圖註冊塊在局部深度地圖上註冊激光雷達雲，並將深度與視覺特徵相關聯。幀到幀運動估計塊利用視覺特徵計算運動估計。

激光雷達里程計部分，作者定義一個sweep作爲三維激光雷達完成一次全掃描覆蓋。如果激光雷達慢軸連續旋轉，sweep通常是一個全球面旋轉。然而，如果慢軸來回旋轉，則sweep是朝着同一方向的順時針或逆時針方向旋轉。在我們的系統中，sweep持續1s。激光雷達里程計部分每次掃描執行一次，處理整個掃描中感知到的點雲。首先，sweep到sweep優化模塊在連續掃描之間匹配點雲，以優化運動估計並消除點雲中的失真。然後，掃描到地圖構建模塊匹配並加入到當前構建地圖的點雲上，併發布與地圖相關的傳感器姿態。傳感器姿態以高頻圖像幀的速率輸出兩部分變換的積分。

V 視覺里程計

本節總結了視覺里程計的方法。對於激光雷達雲，該方法使用視覺里程計的估計運動來更新和維護深度地圖。
在計算運動時，根據深度的來源，它涉及三種視覺特徵：

• depthmap中的深度值
• 基於先前估計的運動狀態進行三角化得到的深度值
• 無用的深度值

然而，要使用視野超過180°的魚眼相機，我們從現在開始使用術語“距離”（特徵的深度是其距離在$SZ$方向上的投影）。 讓我們用右上標K,

表示圖像幀，用$i$表示視覺特徵集。\vec
對於與距離相關聯的特徵$i、i屬於I$，其在{$S^k$}中的座標表示爲$SX^k_i=[Sx^k_i；Sy^k_i；Sz^k_i]^T$。對於距離未知的特徵，我們使用其標準化座標$S\vec{X}^k_i=[S\vec{x}^k_i；S\vec{y}^k_i；S\vec{z}^k_i]^T$，其中$S\vec{X}^k_i$的歐式距離是1。我們將傳感器的運動建模爲剛體變換。設$R$和$T$是一個$3×3$的旋轉矩陣和一個$3×1$的平移向量，用於描述幀到幀的運動。我們把運動表述爲



上述步驟表明，距離已知的特徵提供兩個方程（2）-（3），距離未知的特徵提供一個方程（4）。在求解運動方程時，我們將所有方程組疊加起來，用6個未知量來表示6自由度運動，從而得到運動估計問題。這個問題用levenbergmarquardt方法解決。運動估計採用穩健的擬合框架來處理特徵跟蹤誤差。根據每個特徵在（2）-（3）或（4）中的殘差爲其分配權重。殘差較大的特徵被賦予較小的權重，而殘差大於閾值的特徵被認爲是離羣值並被賦予零權重。如果找到收斂或達到最大迭代次數，則優化終止。

維持深度圖時，從激光雷達點雲中接收到的新點將添加到深度圖中。只保留相機前面的點，而在某個時間之前接收到的點會被去掉。depthmap被縮小以保持恆定的點密度，並投影到最後一個圖像幀，該圖像幀建立了對前一幀的變換，即幀k-1。我們用一個距離和兩個角度在球面座標系中表示深度圖上的點。這些點基於兩個角座標存儲在2dkd樹中。當距離與特徵相關聯時，我們從每個特徵中找到depthmap上最近的三個點。這三個點形成一個局部平面片，通過將光線從相機中心投影到平面片，從這三個點插值距離。

此外，如果某些特徵距離depthmap不可用，但它們被跟蹤的距離超過了一定距離，則我們使用跟蹤特徵的圖像序列對它們進行三角化。圖3示出了對應於圖1（左圖像）的重構特徵的示例。綠點是其距離與深度映射相關聯的特徵，藍點是通過三角剖分（圖1中的紅點具有未知距離）。

VI 激光雷達里程計

由視覺里程計估計的幀到幀運動通過激光雷達里程計方法進一步細化。lidar里程計包含了點雲粗到精處理的兩個主要步驟：

• 1.sweep 到sweep 細化步驟在連續掃描之間匹配點雲以細化運動估計
• 2sweep 到地圖註冊步驟匹配並在地圖上註冊點雲。
  
  圖4示出了sweep 到sweep 細化步驟的功能。橙色曲線表示由視覺里程計估計的傳感器的非線性運動。視覺里程計的漂移通常被認爲是慢動作。我們用藍線表示的掃描（持續1s）內的接觸速度來模擬漂移。當使用由視覺里程計恢復的運動來記錄激光雷達點雲時，漂移會導致激光雷達雲的失真。sweep 到sweep 細化步驟在激光雷達雲匹配中加入線性運動模型以消除失真。

我們用右上標$m， m 含於Z^+$表示sweep ，用$p^m$表示sweep m時所感知到的lidar點雲。對於每個$p^m$，我們通過計算局部sweep中的曲率來提取尖銳邊緣上的合併點（即邊緣點，平面上的點，平面點的幾何特徵）。我們避免選擇相鄰點的點以及與激光束大致平行的被遮擋區域或局部表面邊界上的點。這些點可能包含較大的噪聲或隨着時間的推移而改變位置。圖5給出了當傳感器在建築物前面導航時從掃描中檢測到的邊緣點和平面點的示例。

設$E^m$和$H^m$爲從$p^m$中提取的邊緣點和平面點的集合。我們將$E^m$和$H^m$與上次掃描的激光雷達雲$p^{m-1}$匹配。這裏，注意在掃描m-1完成後，$p^{m-1}$中的失真被修正。因此，我們只需要對當前掃描應用線性運動模型。將$T^`$定義爲在掃描m期間視覺里程漂移的6×1向量，並將$t^m$定義爲該掃描的開始時間。對於點$i，i 屬於 E^m並 H^m，$在時間$t_i$時獲取數據，$t^m$和$t_i$之間的對應漂移線性插值爲

對於$E^m$中的每個點，我們找到$p^{m-1}$中形成邊緣線段的兩個最近的邊緣點。對於$H^m$中的每個點，我們找到三個最近的平面點，它們形成一個局部平面片。該過程使用兩個3D kd樹，一個存儲邊緣點，另一個存儲$p^{m-1}$中的平面點。根據邊緣點與平面點的對應關係，導出了一個描述點與其對應關係之間距離的方程:

其中$S X^m_i$是點$i$的座標，$i屬於E^m和H^m，$在{$S$}中，$d_i$是與其對應關係的距離。結合（5）和（6），我們得到了$T^`$的函數。$T^`$的求解過程是將每個邊點和平面點的函數疊加，然後使總距離最小。非線性優化採用levenbergmarquardt方法。通過計算$T^`$，我們消除了$P^m$中的失真。

最後，sweep數據在地圖中匹配並在當前構建的地圖上加入無失真的激光雷達雲。將$q^m$定義爲掃描$m$結束時的地圖雲。如圖6所示，此步驟將$P^m$與$q^{m-1}$匹配，併合並兩個點雲以構建新的地圖雲$q^m$。從$P^m$中提取相同類型的邊緣點和平面點。考慮到地圖雲的密度特性，通過計算特徵值和特徵向量，通過檢驗$q^{m-1}$中局部點簇的分佈來確定特徵點的對應關係。
具體地說，一個大特徵值和兩個小特徵值表示邊緣線段，兩個大特徵值和一個小特徵值表示局部平面片。掃描匹配涉及迭代最近點方法[22]，類似於沒有運動模型的掃描到掃描細化步驟。

圖6 解釋了激光數據加入地圖的步驟。

當$P^m$在地圖上註冊後，在世界座標系{W}中，還發布了一個關於地圖上傳感器姿態的變換。由於這些變換每次掃描只計算一次，因此我們將它們與來自視覺里程計的高頻幀到幀運動變換相結合。如圖7所示，結果是以圖像幀速率的高頻集成姿勢輸出。

圖7 藍色部分表示激光雷達發佈的關於世界座標系{W}中傳感器姿態的低頻變換。橙色部分表示視覺里程計在包含幀到幀運動的高頻下發布的變換。這兩種變換被集成在一起，以產生圖像幀速率下的高頻傳感器姿態輸出。

VII實驗

作者使用的是自制的傳感器系統，一個180度的魚眼相機和一個單線2D激光雷達UTM-30LX。雷達安裝在一個-90到+90，角度分辨率爲0.25的電機轉軸上來模擬3D激光雷達。
電腦配置爲2.5GHz，四核的linux PC
該方法使用Kanade Lucas Tomasi（KLT）方法（23）最大跟蹤300個Harris角點。爲了均勻地分佈視覺特徵，圖像被分成5×6個相同的子區域，而每個子區域最多提供10個特徵。

圖9和圖10是室內和室外的效果。這些都是由人手持進行測試的。

VIII 總結

我們提出了一種使用相機和三維激光雷達相結合的實時里程測量和地圖繪製方法。這是通過一種視覺里程錶方法估計自我運動在高頻和激光雷達里程錶方法，細化運動估計和糾正漂移在低頻。這兩個部分的配合可以實現精確和穩健的運動估計，即視覺里程計處理快速運動，而激光雷達里程計保證低漂移。該方法在室內和室外使用我們自己的實驗收集的數據集，用廣角相機和魚眼相機進行測試。該方法在Kitti里程計基準上進行了進一步的評估，平均相對位置漂移爲0.75%。實驗結果還表明了該方法在傳感器高速運動和光照變化較大時的魯棒性。