本文轉載自「計算機視覺工坊」，該公眾號重點在於介紹計算機視覺方向的知識點，主要包括3D算法、2D算法、自動駕駛、光學介紹、算法與編程等。
論文題目：《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》 發表在2015年的ICRA上，是一篇經典的視覺激光融合的SLAM系統框架，但是作者未開源代碼。在公眾號「3D視覺工坊」后台，回復「V-LOAM」，即可獲得原文。
摘要
本文提出了一個視覺激光融合的框架，提升了系統在缺乏視覺特征和在有挑戰的運動情況下的魯棒性。這個方法利用視覺里程計估計機器的運動，高速率低精度的對准點雲。然后基於激光雷達的scan-match來優化運動估和點雲對准，在KITTI的01序列達到了0.75%的位置漂移，系統在高速運動和照明變化的場景中也有很高的魯棒性。
介紹
視覺里程計需要在光線漸變、特征比較多的場景中運行；激光在測量點雲的時候存在運動畸變(激光采集一幀點雲需要0.1s，所以一幀的點並不是在同一時刻采集的)，因此激光雷達的運動往往需要大量的變量來求解。不僅如此，在場景中平面較多的退化場景中，scan-match也很容易失敗。這個系統包括兩個交替的過程：
-
第一個是視覺里程計以60hz的速度來估計運動。
-
激光里程計利用低頻率1hz來優化運動估計，去除由於里程的漂移造成的點雲的運動畸變。
對無畸變的點雲進行匹配並配准以逐步構建地圖。結果是視覺里程計解決快速的運動，激光雷達可確保在不良照明條件下的低漂移和魯棒性。實驗結果表明地圖很准確的無需后期處理。盡管閉環可以進一步改善地圖精度但本文選擇不這樣做，因為這項工作的重點是實現更精准的里程計。
V-LOAM可以利用不同類型的測距傳感器，在有先驗地圖的時候本系統也可以只定位。本文實驗得到利用魚眼相機得到更好的魯棒性，但是由於大視角和嚴重的畸變精度比較差，經過激光雷達優化后精度可以得到提升。
本文的方法采用相機和激光雷達緊耦合的方式，從而僅需一個攝像機即可恢復運動。同時考慮了由視覺里程計的漂移引起的點雲畸變，即我們將漂移建模為短時間（1s）內的線性運動，並在掃描匹配期間使用線性運動模型校正畸變。
坐標系統和任務
本文的任務是估計相機和雷達系統的運動，並利用估計的位姿構建環境的地圖。假設相機是通過中央相機模型來建模的參考文檔。利用這個模型系統可以利用常規的或者是魚眼相機來定位。假設相機內參已知、相機和激光雷達外參通過標定已得到，這就允許我們利用一個坐標系來表示兩個傳感器。為了簡化計算，我們選擇相機坐標系作為機體坐標–所有激光點都投影到相機坐標系中。
- {S}表示相坐標系：x->left、 y ->upwards、z -> forward
- 世界坐標系{W}和{S}在0時刻是對齊的。
本文的問題可以表述：利用在{S}中得到的相機獲得的圖像和激光雷達得到的點雲，確定{S}和{W}的位姿變換關系，在{W}系中構建地圖。
系統概述
系被分為兩部分：
- 視覺里程計：按圖像的幀率利用圖像和激光點雲估計系統的運動。在這個部分特征跟蹤模塊在連續的圖像幀中檢測和匹配視覺特征點。深度圖配准模塊把局部的深度圖和點雲對齊並得到視覺特征點的深度。幀到幀的運動估計模塊利用特征點計算機體運動。
- 激光里程計：激光雷達的一次完整的掃描定義為sweep。在這個系統中,一個sweep是1s。激光里程計一個sweep執行一次來處理在sweep中接收到的點雲。首先sweep到sweep的refinement模塊匹配連續幀的點雲來優化運動估計並消除點雲中的畸變。然后sweep到map的配准模塊匹配和對齊當前點雲和地圖，並發布相對於地圖的傳感器的位姿。
最后位姿的輸出是集成了這兩部分，然后按照圖像的幀率進行輸出。
視覺里程計
利用激光雷達的點雲和視覺里程計得到的位姿對齊和維護了一個深度圖。計算運動的時候，按照深度來源划分可以得到三種特征點：
- 來自深度圖的深度
- 利用先前的位姿三角化得到的深度
- 沒有深度的特征點
記號
最后的運動模型可以定義為:
對於k幀和k-1幀都沒有深度的點:
所以一個點如果有深度另外一個沒有就用公式2，兩個點都沒有深度就用公式3。在求解運動時利於所有的方程用六個未知數來表示運動估計問題。這個問題由LM解決。運動估計采用一個魯棒的擬合框架來解決特征跟蹤誤差。利用上邊兩個公式得到每個點的權重，殘差越大權重越小，當殘差大於某個閾值認為是外點需要剔除。設置其權重為0；如果找到收斂或滿足最大迭代次數，則優化將終止。維護深度圖的時候，只添加相機前邊的雷達點，那些在一定時間前的點就被丟棄。深度圖會進行降采樣保證常數的密度，還要把這些點投影到已經得到位姿的k-1幀。使用距離和兩個角度在球面坐標上表示深度圖上的點。這些點基於兩個角坐標存儲在2D KD樹中。當深度圖中的點關聯到特征點的時候，在深度圖中剛找到3個和特征點最近的深度點組成一個平面的patch，利用相機中心到平面的視線插值得到特征點的深度。如果多次跟蹤特征點在深度圖中始終得不到深度，就用三角化得到深度。
雷達里程計
從視覺得到的幀到幀的運動估計可以利用激光里程計做優化。從粗到細的處理點雲有兩個重要的步驟:
- 匹配連續的sweep的點雲來優化位姿估計
- sweep to map配准來對准點雲到地圖上
上圖表示的是sweep-to-sweep的優化過程。橘色的線表示視覺里程計估計的傳感器的非線性的運動。漂移經常被看作慢運動，所以我們在一個sweep把模型構建為恆速的(藍色的線表示)。當利用視覺恢復的運動來對齊點雲，漂移會導致點雲畸變。sweep到sweep的優化過程代表一個線性的運動模型來匹配點雲，去除畸變。
把線特征和平面特征存在兩個容器中，利用m-1幀的特征和m幀的特征匹配，匹配之后m-1幀點雲的畸變就被去除。所以對於當前的sweep我們只需要用線性的運動模型。定義T'表示第m個sweep的運動漂移，對於當前幀點i(線特征或者面特征)的運動漂移，可以定義為:
在m-1幀的線特征找兩個和當前幀線特征最近的點，在m-1幀的平面特征中找三個和當前特征最近的點，這里用兩個3D的KD-tree，一個存儲線特征點一個存儲平面特征點。我們利用下式來計算特征點的距離:
參考文獻
[1] Zhang J , Singh S . Visual-lidar odometry and mapping: low-drift, robust, and fast[C]// IEEE International Conference on Robotics & Automation. IEEE, 2015.
[2] Zhang J , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014.
本文僅做學術分享，如有侵權，請聯系刪文。下載1在「3D視覺工坊」公眾號后台回復：3D視覺，即可下載 3D視覺相關資料干貨，涉及相機標定、三維重建、立體視覺、SLAM、深度學習、點雲后處理、多視圖幾何等方向。
下載2在「3D視覺工坊」公眾號后台回復：3D視覺github資源匯總，即可下載包括結構光、標定源碼、缺陷檢測源碼、深度估計與深度補全源碼、點雲處理相關源碼、立體匹配源碼、單目、雙目3D檢測、基於點雲的3D檢測、6D姿態估計匯總等。
下載3在「3D視覺工坊」公眾號后台回復：相機標定，即可下載獨家相機標定學習課件與視頻網址；后台回復：立體匹配，即可下載獨家立體匹配學習課件與視頻網址。