視覺同時定位與建圖方法綜述*

朱東林，徐光輝，周 捷，狄恩彪，李明燦

（陸軍工程大學，江蘇 南京 210007）

0 引言

機器人領域作為如今熱門的研究領域之一受到了研究者們的廣泛關注，但是機器人的研究始終伴隨著如何使機器人能夠在陌生的環境中自主探索和行動這個關鍵性問題。SLAM 先前由Smith Self 和Cheeseman 提出，至今已有30 多年的研究歷程，大致分為傳統時期、算法分析時期和魯棒性-預測性時期3 個時間段。

對于未知環境，為了能夠自主移動和探索，機器人需要依靠傳感器獲取環境數據。機器人首先需要解決定位問題即明確自身位置，隨后測量自身周圍的環境信息并建立地圖，同時保存已建的地圖以備后期在該地區內直接進行定位和導航。2007 年，MonoSLAM[1]是首個利用純視覺恢復相機的移動路徑，使用概率框架在線創建稀疏并持久的自然地標地圖，同時由先驗信息輔助系統完成初始化，由擴展卡爾曼濾波法實時更新狀態。此外，相機采用恒速度運動模型，相機狀態的誤差用不確定性程度Qv衡量。2008 年，PTAM[2]實現了跟蹤和建圖用不同線程，并可在地圖中顯示AR 虛擬物體。前端采用FAST 角點檢測算法且引入關鍵幀技術，利用極線約束法匹配特征點計算旋轉矩陣，后端則采用非線性優化的BA（Bundle Adjustment）方法。2014 年，SVO[3]為了在無人機高速移動狀態下估計準確位姿，采用特征點法和直接法相結合的方法實現高頻高精度里程計。直接法中摒棄傳統算法對圖像中少量的大補丁圖像塊的提取，轉而提取大量小補丁圖像塊。RTAB-MAP 針對大場景稠密建圖，并提出內存管理方法來解決大場景長時間稠密建圖存在的回環檢測和實時性問題。2018 年，VINS-Mono[4]設計了一個完備的運動狀態估計器。它是由單目和IMU組裝成的一個小巧裝置，用于度量六自由度，可以廣泛應用移動機器人、無人機和移動電子設備等SLAM 系統。DSO[5]依靠圖像上的采樣像素生成稀疏地圖，所用模型涉及光度校正來消除曝光時間和鏡頭漸暈等影響。ORB-SLAM 在PTAM 基礎上添加了回環檢測線程，是第一個具備跟蹤、回環檢測和建圖環節的完整實例，最終準確實現了相機的運動軌跡和地圖中軌跡的一致。2019 年，基于模型的視覺慣性里程計VIMO[6]正逐步取代傳統的視覺慣性里程計VIO，關注點放在作用于系統的外力上，把動力學和外力結合在殘差中作為約束產生了一種精度較高的狀態估計器。2020 年，DeepFactors[7]建立在CodeSLAM[8]的基礎上，結合深度學習重新表示已學習的緊湊性深度圖對光度誤差、重投影誤差和幾何誤差，實現單目相機構建稠密地圖。一個無需配置相機參數，便可較靈活地選擇多個數量的相機組合使用，完成自適應初始化的SLAM 系統[9]。語義NodeSLAM[10]根據VAE 和概率渲染引擎判別物體形狀及重構多視圖物體。Vid2Curve[11]系統單憑RGB 視頻流也可實現細線條物體三維重建，摒棄傳統基于深度和圖像紋理，而采用基于2D 曲線特征的方法抽取出骨骼曲線，沿曲線按物體寬度融合相應半徑圓餅反映物體線條。AVP-SLAM[12]借助對視角和光照具有魯棒性的語義特征構建地下停車場地圖，完成了自動泊車任務。在任一亮環境下，基于物理的深度網絡考慮反射、折射和內部全反射對物體表面法線重建，進一步實現透明物體的三維重建[13]。ESVO[14]是SLAM 領域首次用雙目事件相機構建的視覺里程計，在保證每幀圖像時空一致性前提下，逆深度估計附加時間戳，并采用了IRLS 非線性優化和前向合成Lucas-Kanade 方法。除此之外，目前開源并且框架完備的算法有ORB-SLAM[15]、DTAM[16]、RGBD-SLAM[17-18]、改善大場景下回環檢測性能的算法[19-20]。綜上所述，對大量視覺算法進行分析歸納出視覺SLAM框架涵蓋傳感器數據輸入、前端VO、后端優化、回環檢測和建圖5 個主體。圖1 為視覺SLAM 的算法流程。

1 前 端

前端視覺里程計部分實現對位姿的計算。計算方法大致有特征點法、直接法、光流法和深度學習與幀間估計4 種。如圖2 所示，視覺里程計要計算估計出相機各幀位姿(R,t)，從初始位置C0（可以自定義）起，當前位置Ck通過(R,t)和上一時刻的位置Ck-1來計算，公式為Ck=RCk-1+t。其中，(R,t)為K和K+1 時刻之間的相機平移和旋轉變化，可根據圖像變換的基本矩陣、單應矩陣和SVD 分解本質矩陣計算獲得，從而恢復相機的運動軌跡。

1.1 特征點法

特征點法是在相鄰幀之間選取特征點并進行匹配，然后使用RANSAC、M 估計或最小中值估計等方法，盡可能濾除錯誤匹配[21]的噪聲點，最后求得位姿。介于對精度和時間的考慮，關鍵點檢測器和描述符組合[22]搭配使用。檢測器算法有ORB、SIFT 以及SURF 等，描述符算法有BRIEF、FREAK等。由于深度圖像的取舍，前端將面臨2D-2D、3D-2D 和3D-3D 共3 種情形，故使用特征點法時最主要考慮的問題是算法選擇問題和計算(R,t)方法的選擇問題。

（1）SIFT/SURF。SIFT[23]/SURF[24]算法使用基于梯度方法的HOG 作為描述符，通過物體在局部區域中的強度梯度分布來描述物體的結構。

圖1 視覺SLAM 算法流程

圖2 相機運動軌跡

（2）RISK/BRIEF/ORB/KAZE/FREAK。上述算法使用二進制Binary 描述符，僅依靠圖像自身強度信息，將關鍵點周圍信息編碼成一串二進制數。BRISK 算法[25]采用長距離對和短距離對，其中長距離對描述圖像補丁的梯度方向，而短距離對用于組成二進制字符串來描述梯度大小。

長距離對搜索函數為：

短距離對搜索函數為：

長對計算關鍵點方向向量公式為：

短距離對組裝成二進制串公式為：

上面使用相同數量圖片及同一工程對所有可能的檢測器和描述符組合進行性能比較實驗。由表1 各種檢測器和描述符組合的總運行時間結果可以看出，FAST+BRIEF 速度最快，FAST+ORB 次之。表1 中×表示當前檢測器和描述符不能搭配使用，相應的實驗結果也將不存在。表2 為各種檢測器和描述符組合的匹配點數目結果，可以看出BRISK+BRIEF 匹配點數目最多，BRISK+SIFT 和BRISK+FREAK 次之。對于所有類型的幾何變換來說，SIFT 和BRISK 的總體精度最高。

1.2 光流法

光流法[26]是為了應對圖像中的特征點難提取、紋理簡單且單一的情況提出的。它是基于亮度不變的假設I(x+dx,y+dy,t+dt,)=I(x,y,t)，即圖像中某一位置的像素其亮度在短時間內不會變化的思想來選取圖像中某些像素點進行跟蹤，最后計算出位姿。

表1 各種檢測器和描述符組合的總運行時間（單位：ms）

表2 各種檢測器和描述符組合的匹配點數目（單位：個）

不變性假設關于時間t進行求導，得：

整理為矩陣形式，為：

式中：Ix代表該點x方向梯度，u代表該點x方向速度；同理，Iy、v分別代表y方向梯度和速度：It表示圖像隨時間的變化量。

1.3 直接法

直接法依據灰度不變性I1(x)=I2(x′)假設，利用圖像中像素灰度值的變化來估計相機的運動方向。只要環境中有亮度并使得相機運動時圖像像素點有亮度的變化，直接法就能工作。因為直接法只關注亮度變化，不關注特征信息，故效率較高。常見的塊匹配算法有NCC[27]、SSD 等。

DVO[28]在假設條件下用高斯牛頓迭代法計算相機的運動位姿(R,t)，預測場景點深度值和深度圖提供的實際深度值之間的差異預估運動。

LSD-SLAM[29]是繼特征點法后將直接法應用于SLAM 的典型算法，省略了特征點提取環節，從而提高了算法效率，將圖像中梯度較大的像素點還原出對應3D 地圖坐標來盡量反映場景物體的輪廓信息，即建立了半稠密的地圖。

跟蹤線程中以歸一化的方差光度誤差最小化為目標，對上一關鍵幀的姿態值左乘這兩個關鍵幀之間的剛體變換值δξ(n)，從而得到當前關鍵幀的位姿。

2 后 端

后端優化主要對前端的位姿值進行優化，盡量減少累積誤差。前端當前建立的帶有累積誤差的局部地圖，后端會優化當前的地圖，避免地圖誤差越來越多。目前，最主要的后端算法包括基于濾波理論的擴展卡爾曼濾波法和基于優化理論的BA 和圖優化法、位姿圖法。除此之外，基于濾波理論的算法還有UKF[30]、EIF、SEIF 以及PF 等，基于優化理論的算法還有非線性優化、TORO 以及G2O 等。

2.1 濾波法

卡爾曼濾波器包含預測和更新兩個任務，被看作兩步驟程序。

步驟1：使用上一狀態值估計下一狀態及不確定性：

步驟2：使用當前的測量值透過加權平均來更新當前的狀態估計值：

式中，Kk表示卡爾曼增益，Hk表示觀測模型，表示殘差。

MonoSLAM 的后端采用擴展卡爾曼濾波法，相機采用恒速度運動模型，受瞬間外力對相機的影響，需要再更新相機運動狀態量。未知力度對相機瞬時作用的影響用噪聲向量n表示，其中n包含短時間內外力的線加速度和角加速度產生的線速度和角速度。

2.2 優化法

BA（Bundle Adjustment）目標函數中，相機位姿變量xc=[ξ1,ξ2,…,ξm]T∈R6m和路標點變量xp=[p1,p2,…,pm]T∈R3n組成自變量，作為所有待優化的變量x=[ξ1,…,ξm,p1,…,pn]T。變量發生變化時對應的增量方程為：

式中，F、E分別表示關于相機位姿和路標點位置的偏導數。無論使用G-N 方法還是L-M 方法，最后都將面對增量線性方程：

以G-N 為例，則H矩陣為：

認識到矩陣H的稀疏結構，并發現該結構可以自然地用圖優化來表示，此處引入G2O。若一個場景內有5 個相機位姿(C1,C2,C3,C4,C5)和2 個路標(P1,P2)。關系結構圖如圖3 所示，以最小化下列函數為目標，優化所有圖中的變量：

式中，Pj表示第j個路標點，表示j=N個路標點分別在第i個相機下的像素坐標，si表示各路標點到各相機平面的深度值。

圖3 關系結構

2.3 位姿圖法

從圖優化知，隨著相機運動需加入大量路標節點。路標節點數量遠大于位姿節點的數量，將嚴重降低計算效率。因此，舍棄路標節點變量的優化，只關注相機位姿變量xc=[ξ1,ξ2,…,ξm]T∈R6m的優化，如圖4 所示。

圖4 關系結構

首先根據兩幀已記錄（前端計算）的位姿得到相對位姿變換：

重新根據i、j兩幀圖像利用對極幾何得到實際位姿增量Tij，優化的目標為：

若所有位姿節點之間的邊集合記做κ，則總體目標函數為：

3 三維地圖構建

三維地圖的表達方式有深度圖（Depth）、點云（Point Cloud）、體素（Voxel）和網格（Mesh）共4 種。圖5 為4 類三維建圖方式樣例，深度圖中每個像素表示物體距離相機平面的值。點云是大量點組成的數據集，每個點包含位置、顏色等信息。體素是由一個個矩形小方塊組成，類似于三維空間中的像素。網格由許多三角形面拼接成的多邊形以接近現實物體表面，每個網格包含三角形的頂點、邊和面。

圖5 4 類三維建圖方式樣例

3.1 深度圖

Shao 等人[31]提出了一種立體視覺和陰影形狀（Shape From Shading，SFS）算法，能夠同時生成深度圖和方向圖，但很難作用在顏色和紋理不均一的場景。強度圖像與對應的深度信息進行隱馬爾可夫模型（Hidden Markov Model，HMM）建模，應用單張圖像的3D 人臉或手的重構系統[32]。Saxena 等人[33]將圖像劃分出多個區域塊，使用馬爾可夫隨機場（Markov Random Field，MRF）對逐個像素塊計算出相應參數，以描述每塊所指的3D 平面位置與方向，而各個平面共同組成最后的3D 場景。Liu等人[34]使用有監督的學習方法對非結構化場景圖進行深度預測，其中模型采用經過區分訓練的馬爾可夫隨機場（MRF）。受生物學復雜單元[35]的啟發，對圖像像素之間相關性進行編碼，從數據中學習深度和運動線索實現3-D 運動分析。

最近，Karsch 等人[36]使用基于SIFT Flow[37]的KNN 傳輸機制從單個圖像估計靜態背景的深度，并利用運動信息對其進行增強，以更好地估計視頻中的運動前景對象。Eigen 等人[38]使用兩個深度網絡分別進行圖像粗略預測和局部細化預測，實現了對單張圖像的深度估計，并提出了一個尺度不變的損失函數。Ladicky 等人[39]提出了將語義對象標簽與單眼深度特征集成在一起的方法，以提高性能，但它們依靠手動添加的方式并使用超像素來分割圖像。Laina 等人[40]建議使用基于ResNet 的網絡結構來預測深度。Xu 等人[41]建議使用CNN 結構構建用于深度預測的多尺度條件隨機場CRF。BA-Net[42]輸入圖像使用預先學習（端到端）的深度圖生成器產生多個基礎深度圖，并通過特征量度BA 將這些深度圖線性組合出最終深度。

3.2 點 云

Fan 等人[43]實現了對單圖像中單個物體以點云形式進行三維重建，開創了點云生成的先例。從一張2D 圖像恢復一個3D 物體可能出現多個可能的較好結果，使得此問題不適用經典回歸或分類方法解決，轉而研究基于點云表示的3D 幾何生成網絡。網絡由輸入圖像和推斷的視點位置確定3D 點云中的點位置。點集生成網絡恢復出多個可能的結果，再經損失函數確立最終點云結果。圖6 為系統的整體結構圖。

圖6 系統結構

2D 圖恢復所有可能的3D 形狀：

損失函數：

ElasticFusion[44]算法側重構建清晰稠密地圖。對RGB-D 相機獲取的深度圖進行操作，相關信息融合到初始的surfel 數學模型中，一步步優化模型外形，最后使用此模型描畫場景。圖7 展示了ElasticFusion 算法的整體實現流程。

建圖線程的兩個步驟。一是surfel 模型初始化采用Deformation 圖使用均勻抽樣初始化。它由許多node 組成，每個node 帶有等參數來影響surfel 模型進行表面變形。二是RGB-D 相機獲得深度圖像，使用內參將深度圖轉為點云。點云和RGB彩色圖像使用跟蹤線程計算的相機幾何位姿和光度位姿，并利用OPENGL 融合到surfel 模型中。

3.3 體 素

3D-R2N2[45]沒有在觀察對象之前需先匹配合適的3D 形狀，以盡可能迎合2D 圖像中所描述的物體。而受早期使用機器學習來學習2D 到3D 映射以進行場景理解的工作啟發[33,46]，引入深度卷積神經網絡。網絡建立在LSTM 和GRU 的基礎上，實現了單視圖和多視圖的3D 重建。

網絡框架包含Encoder、3D Convolutional LSTM和Decoder 共3 大部分，如圖8 所示。Encoder 是一種CNN 結構對輸入圖像進行編碼；Decoder 是解碼過程；3D Convolutional LSTM 是由許多LSTM 單元組成的正立方體網格結構，每個單元負責一個已編碼的部分并恢復出相應體素，所有體素格組成3D物體。損失函數使用二分類交叉熵函數，決定相應體素格狀態是否占有。

圖7 ElasticFusion 算法框架流程

3.4 網 格

Pixel2Mesh[47]在基于圖的卷積神經網絡中輸出3D 網格圖形。任一單圖像輸入之前，系統默認同步初始化一橢圓球作為初始的物體形狀。全卷積神經網絡一步一步提取2D 圖像中的細節特征，同時圖卷積神經網絡受圖像特征的約束將細節附加于初始的橢球體上，隨著特征數量的不斷增加致使橢球不斷形變（粗糙到細密過程），最終得到最后物體。圖9 為雙網絡協作流程。

圖9 雙網絡協作流程

4 種約束網格形變的損失函數為：

圖卷積神經網絡中包含與Mesh 網格對應的頂點（V）、邊（E）和特征向量（F），定義函數如下：

4 發展趨勢

當前，視覺SLAM 的發展趨勢主要涵蓋以下3個方面。

4.1 面對特殊材質的透明物體的重建

透明物體的重建受光反射、折射和內部全反射等因素影響。透明物體成像的物理基礎是界面處的折射遵循斯涅爾定律，反射由菲涅爾方程確定。由高折射率介質進入較低折射率介質界面處的入射角大于臨界角時，就會發生全內反射。文獻[13]中利用了深層網絡學習和模擬圖像形成的物理基礎知識。網絡內的渲染層可對最多兩次反射的折射光路進行建模，以建立對應于物體正反兩面投影光線的曲面法線，可在任意環境下使用不受約束的圖像來重建透明形狀。圖10 為透明物體樣例。

圖10 透明物體樣例

4.2 面對海面以下復雜場景的重建

由于無線電信號被海水吸收，因此無法使用GPS 系統定位水下機器人。Ferrera 等人[48]致力于在水下海床進行同步定位和制圖，使用配備單目、IMU 和壓力傳感器的設備，采集制定了3 個不同環境下的數據集。對于每個數據集還使用SFM 離線計算了真實軌跡，以便能夠使后期和實時定位建圖方法進行比較。圖11 為SFM 對水下場景的重建結果，曲線代表計算的水下機器人移動軌跡。此外，Ferrera 等人[49]提出一新型的單眼UW-VO（水下視覺測距法）算法，克服了由于介質特定屬性導致的視覺退化。

圖11 重建的水下場景結果

4.3 面對纖細復雜物體的重建

纖細物體（如樹枝、電線和鐵絲等）通常表面積小且缺乏紋理特征和點特征，故無法使用傳統的基于圖像特征點的重建方法獲取3D 模型。Vid2Curve 使用曲線特征的新方法，鎖定連續圖像中同一物體的變化動態計算位姿，以一種迭代結構優化策略逐步添加和處理所有圖像幀，并改進估計的物體曲線結構。圖12 為鐵絲物體的重建結果。Usumezbas 等人[50]實現將圖像中大量2D 曲線拓撲連接為3D 曲線集合，最后生成一個3D 曲線圖。

圖12 重建的鐵絲物體結果圖

5 結語

本文分析了SLAM 算法近20 年的最新進展情況，歸納出視覺SLAM 算法的整體框架，并闡述前端和后端包含的各種方法和相關理論，實例分析三維建圖的各種方式，突出介紹了苛刻環境下的地圖構建。視覺SLAM 是未來立體視覺感知頗具潛力的發展方向，其中視覺傳感器具有成本低和普及度高的優勢，極大地促進了SLAM 的研究及未來在電子設施上的應用。