特征法視覺SLAM逆深度濾波的三維重建

摘要：針對現有特征法視覺SLAM只能重建稀疏點云、非關鍵幀對地圖點深度估計無貢獻等問題，本文提出一種特征法視覺SLAM逆深度濾波的三維重建方法，可利用視頻序列影像實時、增量式地構建相對稠密的場景結構。具體來說，設計了一種基于運動模型的關鍵幀追蹤流程，能夠提供精確的相對位姿關系；采用一種基于概率分布的逆深度濾波器，地圖點通過多幀信息累積、更新得到，而不再由兩幀三角化直接獲取；提出一種基于特征法與直接法的后端混合優化框架，以及基于平差約束的地圖點篩選策略，可以準確、高效解算相機位姿與場景結構。試驗結果表明，與現有方法相比，本文方法具有更高的計算效率和位姿估計精度，而且能夠重建出全局一致的較稠密點云地圖。

3D reconstruction with inverse depth filter of feature-based visual SLAM

ZHANG Yi,JIANG Ting,JIANG Gangwu,YU Anzhu,YU Ying

Institute of Surveying and Mapping, Information Engineering University, Zhengzhou 450001, China

Foundation support: The National Natural Science Foundation of China (Nos. 41501482; 41471387; 41801388)

First author: ZHANG Yi (1989—), male, PhD candidate, majors in digital photogrammetry and visual SLAM. E-mail:276690308@qq.com.

Abstract: Aiming at the problem that the current feature-based visual SLAM can only reconstruct a sparse point cloud and the ordinary frame does not contribute to point depth estimation, a novel 3D reconstruction method with inverse depth filter of feature-based visual SLAM is proposed, which utilizes video sequence to incrementally build a denser scene structure in real-time. Specifically, a motion model based keyframe tracking approach is designed to provide accurate relative pose relationship. The map point is no longer calculated directly by two-frame-triangulation, instead it is accumulated and updated by information of several frames with an inverse depth filter based on probability distribution. A back-end hybrid optimization framework composed of feature and direct method is introduced, as well as an adjustment constraint based point screening strategy, which can precisely and efficiently solve camera pose and structure. The experimental results demonstrate the superiority of proposed method on computational speed and pose estimation accuracy compared with existing methods. Meanwhile, it is shown that our method can reconstruct a denser globally consistent point cloud map.

Key words:visual simultaneous localization and mapping3D reconstructioninverse depth filtermotion modelback-end hybrid optimization framework

視覺即時定位與地圖構建(visual simultaneous localization and mapping, VSLAM)技術，以視頻序列影像為輸入，能夠在恢復相機運動軌跡的同時，實時重建未知場景的三維結構，可用于無人機、車輛、機器人等平臺的智能環境感知、自動駕駛與導航[1]，也可用于應急測繪、災害與突發事件監測、虛擬與增強現實等場景，具有廣闊的應用前景與市場潛力[2-3]。

目前視覺SLAM算法主要采用基于關鍵幀優化的特征法[4-6]進行實現，其基本流程是：首先從每幅影像中提取出具有可重復性與顯著區分性的點、線特征；使用不變性描述符在后續幀中對特征進行匹配，利用多視圖幾何原理恢復相機位姿與結構，然后通過光束法平差[7]進行聯合優化。

在諸多特征法SLAM中最具代表性的是ORB-SLAM[8-9]，它采用統一的ORB[10](oriented FAST and rotated BRIEF)特征進行計算，在室內外環境中均能穩健、實時運行，可在大視角差異下完成重定位與閉環檢測，并且由像點匹配累積的共視[6]信息可用于構建強壯的平差與圖優化[11]網絡，有利于得到全局最優解。ORB-SLAM的主要缺陷在于，地圖點通過兩關鍵幀三角化直接得到，對于短基線視頻影像，其深度不確定性較大，為避免粗差必須采取嚴格的篩選策略，導致所構點云地圖十分稀疏。此外，ORB-SLAM的非關鍵幀在追蹤完后即被拋棄，其對地圖點深度信息無貢獻，造成了資源浪費。

從應用層面看，稀疏點云僅可用于傳感器的自身定位，而較高級的視覺導航、避障、虛擬增強現實、模型重建、語義識別等應用，都需要更加稠密的地圖表達。為此，直接法相關技術獲得了更多關注，它根據像素亮度差異直接構建優化問題求解相機位姿，并采用一種基于概率分布的深度濾波器模型[12-14]，利用多幀影像更新地圖點深度信息。早期的直接法多根據稠密深度圖進行計算，以最大化利用影像信息并增加穩健性[15-17]，這類算法一般需要GPU加速來滿足計算要求。文獻[14, 18]使用具有顯著梯度的像素，在CPU上完成了較稠密的實時重建；文獻[13, 19]提出了僅需少量像素的稀疏直接法，使算法具有更強的適用性。

直接法不必進行特征提取與匹配，因而具備了更高的計算效率，但它易受相機曝光、環境光源等因素影響，當場景亮度變化劇烈時可能導致求解失敗，穩健性不足[2]。另外，直接法優化基于像素本身進行操作，該過程需要完整的影像數據，對于大規模場景來說，由于設備存儲空間有限，直接法難以進行全局質量控制。為此文獻[20]將概率分布構圖方法應用于ORB-SLAM中，使得其在保留特征法優點的同時，能夠重建出精確的稠密場景結構。但由于其構圖模塊獨立于ORB-SLAM系統并具有若干關鍵幀的延遲，嚴格意義上并不屬于在線處理，并且構圖結果無法用于后續幀的位姿追蹤，作用有限。

在總結前文算法的基礎上，本文提出一種特征法視覺SLAM逆深度濾波的三維重建方法，無須GPU加速即可實時、增量式地重建出較為稠密的點云地圖。在前端中，設計了一種基于運動模型的參考關鍵幀追蹤流程，能夠有效利用視頻影像先驗約束獲取精確的相對位姿關系；建模并分析了深度估計誤差，采用一種基于概率分布的逆深度濾波器構圖方法，可得到更加稠密的地圖點。在后端中，提出一種基于特征法與直接法混合的優化框架以及基于平差約束的點位篩選策略，可以準確、高效地恢復相機位姿與場景結構。

1 總體流程與關鍵技術1.1 總體流程

本文核心思路是：地圖點不再由兩幀三角化直接得到，而是先構造為種子點，經多幀信息融合直至深度收斂后再插入地圖。為此設計總體流程如圖 1所示，系統以ORB-SLAM為基礎框架，仍采用前后端結合的三線程結構，主要改動集中于追蹤與局部構圖線程，以粗體表示。

圖 1本文方法總體流程Fig. 1Flowchart of the proposed algorithm

圖選項

對于輸入的每一幀視頻序列影像，首先在前端追蹤線程中提取ORB特征，利用運動模型提供的幀間先驗約束，將參考關鍵幀的地圖點投影至當前幀，進行特征匹配與位姿估計；然后在逆深度濾波器中對參考關鍵幀的種子點進行極線匹配并更新其深度；最后判斷該當前幀是否應作為新關鍵幀插入局部構圖線程。

后端局部構圖線程收到新關鍵幀后，首先將原參考關鍵幀中所有深度收斂的種子點激活為新的地圖點，并與鄰近關鍵幀的其他局部地圖點一起投影至新關鍵幀以建立更多匹配；隨后根據各地圖點的觀測情況，采用一種基于特征法與直接法的混合框架優化方法，聯合求解局部相機位姿與場景三維結構；在剔除掉地圖野點與重復關鍵幀后，將新關鍵幀作為后續影像的追蹤參考幀，其未匹配的特征點構造為新的種子點；最后，將新關鍵幀送入閉環線程以實現最優全局一致性。

1.2 運動模型追蹤參考關鍵幀

準確的極線幾何關系是本文逆深度濾波器順利更新種子點深度的前提，因此前端系統需提供當前幀Ic關于參考關鍵幀Ir的精確相對位姿Tcr∈SE(3)。然而ORB-SLAM采用逐幀追蹤的方式，當前幀并不直接與參考幀進行匹配，Tcr只能通過間接方式求出，由于存在誤差累積等因素，該方法用于逆深度濾波器的效果較差。為此本文設計了一種新的追蹤方式，可直接計算當前幀關于參考關鍵幀的相對位姿Tcr，基本流程如圖 2所示。

圖 2運動模型追蹤參考關鍵幀流程Fig. 2Flowchart of tracking reference keyframe with motion model

圖選項

由于視頻影像通常具有較高幀率，相機運動一般比較平穩，具備豐富的先驗信息，能夠為追蹤系統提供良好的初值。常速運動模型假設短時間內相機幀間相對位姿保持不變，即

?(1)

式中,Tv∈SE(3)即為常速運動模型。由于參考幀Ir位姿Trw已知，上一幀Ic－1的相對位姿Tc-1,r=Tc-1,w·Trw-1也為已知，于是當前幀Ic與參考幀Ir的相對位姿初值就可以按照式(2)計算

?(2)

由此就得到了一個的初值假設，考慮到相機不規則運動的可能性，還需構建多種其他假設，包括雙倍運動假設、零運動假設等。算法首先采用常速假設，將參考關鍵幀的地圖點投影至當前幀，得到預測的像點坐標，然后在一定半徑范圍內搜索最優特征匹配，如果匹配數量不足就擴大搜索范圍重新搜索。當取得足夠數量的匹配后，進行位姿優化并采用自由度為2、置信度95%的χ2測試剔除野點。如果上述過程失敗，就采用其他假設繼續嘗試。該方法能夠準確、穩健地恢復當前幀關于參考幀的相對位姿Tcr，同時能夠充分利用先驗信息加速匹配過程。

1.3 逆深度濾波器

由于視頻序列影像幀間基線較短，直接通過兩幀計算的像點深度誤差往往較大，深度濾波器的基本思想是通過多次觀測融合更新像素深度。假設像點P的深度dp服從均值為μ，方差為δ2的正態分布，且系統每追蹤一幀，都可以通過極線搜索尋找該點在新幀上的匹配點，進而觀測到該點的深度μobs并估算方差δobs2，假設這次觀測仍服從正態分布，進行高斯融合可以得到

?(3)

由式(3)可知，觀測融合后深度估計的方差會減小，當其小于某一閾值并趨于穩定時，就認為該點的深度值收斂。本文采用文獻[13]提出的方法對深度估計誤差進行建模。如圖 3所示，設左影像Ir為參考關鍵幀，右影像Ic為已追蹤的當前幀，O1、O2分別為兩幀相機光心位置，基線長b，P為同名像點p1p2經三角化得到的空間點，其在左影像中的深度值為dp，P、O1、O23點的夾角分別記為α、β、γ，l2為p1在當前幀Ic中對應的極線。

圖 3深度估計誤差模型Fig. 3Error model of depth estimation

圖選項

假設在Ic上經過極線匹配得到的同名點p2存在一個像素的誤差，相應的深度值及夾角變為d′p、β′與γ′。此時P點深度dp的誤差δp為

(4)

理論上，深度濾波器只需不斷將當前幀得到的深度估值與方差作為新的觀測，與已有數據進行融合即可。然而實際過程中，由于位姿估計不夠準確、匹配錯誤等諸多原因，觀測值可能存在粗差，將會對融合結果產生較大影響，因此必須對該過程進行優化。

首先，文獻[21]發現逆深度(即深度值的倒數)的統計直方圖更接近正態分布，因此在計算出P點深度值dp后，將其換算為逆深度ρp=1/dp，相應的誤差計算公式為

(5)

其次，考慮到單像素的亮度與梯度沒有明顯的可區分性，易受噪聲影響，本文采用文獻[19]提出的8像素-圖像塊模型，用各像素的梯度均值作為中心像素的梯度，并按照距離平方和(sum of squared distance，SSD)測度進行極線搜索匹配，能夠保證較高的準確度并兼顧效率，如圖 4所示，其中p1為待匹配點，pmin與pmax分別是按照最大與最小逆深度值(ρp±3δρ)計算的空間點P投影到當前幀的像點，兩點連線即為極線段向量l2。

圖 4圖像塊模型與極線搜索Fig. 4Patch model and epipolar line search

圖選項

此外，匹配點像素梯度與極線的夾角也對深度濾波器有重要影響，隨著像素梯度與極線段夾角的增加，極線匹配的不確定性也會增加[22]。由于視頻影像幀率較高，幀間相對旋轉較小，可將參考幀Ir上p1點的圖像塊平均梯度gp作為當前幀Ic匹配點p2梯度的近似，于是由像素梯度與極線夾角造成的誤差可被定義為

?(6)

式中,n2表示極線段l2的法向量。由式(6)可知，當l2與gp平行時，Ep取得最小值，反之當l2與gp垂直時Ep取得最大值。在實際計算過程中，若Ep小于某一閾值，并且極線搜索的最優匹配SSD小于次優匹配的一半以上，就認為該點的深度觀測有效，可以對其進行高斯融合。

1.4 后端混合局部優化框架

后端優化是維持SLAM系統一致性的關鍵，在ORB-SLAM中該問題可以通過局部光束法平差解決，但由于本文方法地圖點是通過深度濾波算法構造的，在初始化時僅能夠被當前關鍵幀觀測，無法按照重投影誤差最小化原理進行優化。為此本文提出一種基于特征法與直接法的后端混合局部優化框架。

首先，對于觀測數大于等于3的地圖點，采用光束法平差聯合求解相機位姿與地圖點位置。然后，對每個觀測數小于3的地圖點，將其投影到其他關鍵幀，按照直接法輻射誤差最小化原理，對該點深度值進行優化。該過程可以用因子圖的形式表達，如圖 5所示，MP1與MP2表示待優化的地圖點，HostKF表示構造這兩個地圖點的主幀，LocalKF表示與主幀存在共視連接的局部關鍵幀，Ob表示地圖點投影到目標關鍵幀形成了一次觀測(觀測的主幀與目標幀分別用紅線和藍線表示)。

圖 5直接法深度優化因子圖Fig. 5Factor graph of direct depth optimization

圖選項

假設圖像塊模型中的各像素具有相同的逆深度值，以MP1為例，其深度優化目標函數為

(7)

以及對應的誤差項

?(8)

式中，優化目標ρ1為地圖點MP1在主幀HostKF中的逆深度；IH表示主幀影像灰度函數；Ij表示第j個觀測目標幀的影像灰度函數；pi與p′i分別表示MP1投影在IH和Ij上的8像素-圖像塊的第i個像素。建立誤差方程式并線性化后，可按照最小二乘原理迭代求解。

采用該優化框架的好處非常明顯：首先，觀測數較多的地圖點往往具有較好的穩健性，平差系統能夠減少野點風險，準確恢復相機位姿與場景結構[23]，如圖 6(a)；其次，這類地圖點數量稀疏，平差負擔小；而經直接法優化的地圖點，如圖 6(b)，盡管比較稠密，但未知數僅有一個，且各點解算過程相互獨立，可采用并行策略進行加速，從而提高優化效率。

圖 6不同類型地圖點Fig. 6Different types of map points

圖選項

由于誤匹配等原因，系統在運行過程中不可避免會出現地圖野點，需予以剔除。對于新構建的地圖點，ORB-SLAM根據其在后續若干關鍵幀中的匹配情況進行篩選，該策略無法用于本文直接法優化的地圖點(匹配數不足，總是會被剔除)。為此，提出一種基于平差約束的篩選策略，每次后端優化完成后，檢查所有新構建的地圖點，如果滿足以下任一條件，就會被標記為野點并剔除。

(1) 經局部光束法平差優化后，重投影誤差未通過χ2測試被標記為野點的；

(2) 經直接法優化后，能量函數超過閾值被標記為野點的；

此外，如果一個地圖點被標記為野點，并且其主幀是最新的兩個關鍵幀之一，就重新將該點構造為種子點進行深度濾波。本文策略能夠在排除野點的同時，有效增加點云密度。

2 試驗與分析2.1 試驗數據與評價方法

利用3組數據進行驗證，如圖 7所示，第1組數據選自開源TUM RGB-D數據集[24]，采用移動機器人搭載Kinect相機拍攝，拍攝環境為室內，本文僅利用其中的RGB數據，數據集提供了完整的相機參數以及由高精度IMU獲取的位姿數據作為評價真值。

圖 7試驗數據Fig. 7Data examples

圖選項

第2、3組數據由大疆精靈4無人機相機獲取，其中第2組采用手持方式，拍攝對象為室內機房，第3組采用航拍方式，拍攝對象為室外建筑，相機預先經過標定并對原始影像進行了幾何畸變校正，3組數據均包含完整閉合回路，基本信息見表 1。依據本文方法，在Ubuntu16.04系統下開發了驗證算法ZY-SLAM，所用PC配置為:Inter Core i7 2.6 GHz、DDR3 16 GB，未采用GPU加速。

表 1試驗數據基本信息Tab. 1Data description of test images

測試數據	影像大小/像素	幀率/(幀/s)	影像幀數	平面范圍/m2	拍攝環境
TUM_Desk	640×480	30	2964	5×6	室內
UAV_Lab	1280×720	20	3231	18×12	室內
UAV_Building	1920×1080	10	1840	160×120	室外

由于本文方法基于ORB-SLAM框架實現，故以其作為主要比較對象，考慮到ORB-SLAM只能得到稀疏點云，而較為稠密的三維結構表達又是直接法視覺里程計(visual odometry, VO)的研究熱點，因此采用當前具有代表性的DSO[19](后文稱DSO-VO，區別于SLAM)共同比較，按如下3種方法分析本文方法性能：

(1) 通過比較系統處理影像的整體及關鍵環節運行時間，驗證算法的計算效率；

(2) 通過計算軌跡誤差，分析算法的相機位姿估計精度；

(3) 通過目視檢查，評價算法的重建效果。

2.2 計算效率分析

表 2顯示了采用本文方法與ORB-SLAM對TUM_Desk數據進行處理的系統運行時間對比(由于DSO計算流程存在較大差異，這里未進行對比)。本文方法的總體效率優于ORB-SLAM，每幀平均處理時間約36 ms，幀率28 fps，基本滿足實時要求。

表 2系統總體運行時間對比Tab. 2Comparison of system overall runtime

	ORB-SLAM		ZY-SLAM
主要操作	平均耗時/ms		主要操作	平均耗時/ms
追蹤線程	ORB特征提取	25.6		ORB特征提取	25.6
初始位姿估計	3.6	運動追蹤	3.4
追蹤局部地圖	15.6	深度濾波	2.9
局部構圖線程	插入關鍵幀	17.7	種子點激活	21.6
地圖點篩選	0.1	追蹤局部地圖	18.2
地圖點構造	22.7	局部光束法平差	184.6
匹配融合	122.2	直接法深度優化	56.9
局部光束法平差	229.8	重復幀與野點剔除	11.5
重復幀剔除	11.1	構造新種子點	20.1
逐幀處理時間/ms	45			36
地圖點數量/萬	0.6			14

考慮到本文方法地圖點數量超過ORB-SLAM的20倍，綜合效率提升非常顯著，這主要得益于：①追蹤線程中，本文方法采用追蹤參考關鍵幀的方式進行位姿估計，不再逐幀追蹤局部地圖，節省了大量資源；②局部構圖線程中，影響計算效率的主要是局部平差。本文算法采用的混合優化框架，能夠在準確恢復相機位姿與場景結構的同時，有效降低平差規模，不僅可以減少局部平差優化被中斷的次數，也可以更好地平衡追蹤與局部構圖這兩個并行線程的關系，使系統更加流暢。

上述試驗結果也證明由深度濾波構造地圖點的方式更適合SLAM系統。從效率角度看，深度濾波利用若干相鄰幀影像更新深度，由于基線較短，極線搜索范圍小，匹配成功率高，計算負擔低，系統運行更平滑，更符合增量式重建的特點，而ORB-SLAM為保證深度估計的可靠性，只在關鍵幀上進行極線匹配，雖然基線較長，但搜索范圍大，耗時較長，同時由于影像函數的高度非凸性，可能存在若干接近最優匹配的“次優匹配”結果。從實用性角度看，深度濾波過程中，倘若某次觀測出現粗差，只需跳過該次觀測，系統仍可利用后續觀測準確估計深度，而在ORB-SLAM中，地圖點由兩關鍵幀直接計算，為避免粗差就需采用多種組合策略進行約束，實際能夠通過測試的點對非常少，這也是ORB-SLAM只能輸出稀疏點云的主要原因。

2.3 相機軌跡誤差分析

位姿估計是SLAM系統的核心，其精度受地圖點精度的影響，并且反過來也直接影響所構建的地圖點。由于本文方法屬于單目視覺SLAM，存在尺度不確定性，且位姿估值可以被定義在任何坐標系下，無法直接與真值進行比較，本文采用軌跡誤差來衡量算法的位姿估計精度。其思路是，先利用最小二乘得到位姿估值序列與真值序列的相似變換S∈Sim(3)，將估值換算至同尺度真值坐標系下再計算各幀的相對變換。軌跡誤差一般只計算平移分量，這是因為它更加直觀，并且也潛在地受到角度差異的影響[24]。此外，對于第2、3組無人機影像數據，由于缺乏高精度POS實測的位姿真值，本文利用ContextCapture[25]軟件對影像數據進行三維建模并將其空三結果作為評價真值使用。結果如表 3所示。

表 3不同方法位姿估計軌跡誤差比較Tab. 3Trajectory error comparison of pose estimation among different methods

cm
Dataset	ORB-SLAM		ZY-SLAM		DSO-VO
RMS	Std	Max		RMS	Std	Max		RMS	Std	Max
TUM_Desk	0.81	0.31	1.52		0.72	0.28	1.61		6.14	3.06	11.07
UAV_Lab	8.78	4.26	16.08	6.20	2.64	12.11	63.34	27.53	124.97
UAV_Building	14.24	4.76	25.24		11.80	4.20	24.96		147.36	90.28	445.96

本文方法的位姿估計精度較ORB-SLAM有所提升，分析原因如下：①由深度濾波獲取的地圖點融合了多幀觀測結果，相比于直接通過兩幀三角化得到地圖點的方式，信息利用更加充分，三維位置更加準確，對位姿估計產生積極影響；②由于地圖點數量較多，后端光束法平差可以僅采用觀測數多的點估計相機位姿，有助于減小野點對最小二乘系統的影響；③特征點匹配率更高，一定程度上增加了多余觀測，能夠提高位姿估計的穩健性。圖 8顯示了不同數據集的關鍵幀特征點匹配率，本文方法在大部分情況下均高于ORB-SLAM。

圖 8關鍵幀特征點匹配率Fig. 8Matching ratio of keyframe feature points

圖選項

此外，本文方法與ORB-SLAM的位姿估計精度明顯高于DSO-VO，這是因為前兩種方法均可利用閉合回路約束減少累積誤差影響，而DSO-VO不具備此功能，軌跡誤差的累積效果在圖 9中尤為明顯。

圖 9相機平面軌跡與地面真值Fig. 9Camera plane trajectories and ground truth

圖選項

2.4 三維重建效果

在準確恢復相機位姿的前提下，本文方法無須GPU加速即可重建出較為稠密的場景三維結構，如圖 10所示，從而使虛擬現實、機器感知、語義識別等更高級別的地圖應用成為可能。這主要得益于，逆深度濾波器可以構造大量地圖點，且一個地圖點無論觀測情況如何，都可以通過后端混合框架進行優化。觀察可發現，本文方法點云地圖中包含許多非角點的物體輪廓、邊緣信息，這是ORB-SLAM無法實現的。

圖 10本文方法重建效果Fig. 10Reconstruction examples of proposed method

圖選項

與此同時，本文方法可以準確地識別并進行閉環改正[26]，得到全局一致的位姿與地圖信息，而DSO盡管也具備優異的局部重建性能，但由于其采用的直接法平差需利用完整影像數據，在資源受限的情況下難以進行全局質量控制，導致所構建地圖出現重影，如圖 11、圖 12所示，影響重建效果與準確性。

圖 11UAV_Lab數據局部重建細節對比Fig. 11Local reconstruction details of UAV_Lab data

圖選項

圖 12UAV_Building數據局部重建細節對比Fig. 12Local reconstruction details of UAV_Building data

圖選項

3 結論

本文提出一種特征法視覺SLAM逆深度濾波的三維重建方法，關鍵在于地圖點深度不再由兩幀三角化直接獲取，而是由基于概率分布的逆深度濾波器累計更新得出。除了計算效率與位姿估計精度方面的優勢，更重要的是，該方法能夠在保證全局一致性的前提下，顯著提升重建點云密度，準確、高效地恢復場景結構細節，為視覺SLAM在機器人智能感知、自動駕駛、應急測繪等領域的進一步應用提供了新思路。未來將針對點云的去噪策略與平滑約束、重建精度定量評價等問題繼續開展研究，同時也將采用更多類型的影像數據進行更加廣泛的測試。