弱紋理環境下融合點線特征的雙目視覺同步定位與建圖

龔 坤，徐 鑫，陳小慶，許悅雷*，張兆祥

（1.西北工業大學 無人系統技術研究院，陜西 西安 710072；2.中國人民解放軍軍事科學院 國防科技創新研究院，北京 100850）

1 引言

目前，自主移動機器人在完全未知且拒止的環境中，需要依靠對周圍環境的感知來確定自身位置以實現自主導航。這種方法被稱為同步建圖與定位（Simultaneous Localization and Mapping，SLAM）［1］。SLAM 主要分為激光SLAM與視覺SLAM［2-3］。在室外環境中，由于光線的干擾，一般采用激光SLAM 為移動機器人提供高精度導航信息，如自動駕駛車輛等。而在室內環境下，掃地機器人等已開始采用視覺SLAM［4］。目前，視覺SLAM 作為一種低成本的定位方案，引起了廣泛的關注［5-6］。

視覺SLAM 依據所采用的圖像特征不同，可分為基于特征點的方法，如ORBSLAM［7-9］，VINS［10］等，以及基于直接法或者半直接法，如DSO［11］，SVO［12］等。其中，直接法以及半直接法需要依賴灰度不變假設，因此對光線的影響較為敏感［13］；基于特征點的方法（例如ORB 特征點）能夠提取具有尺度不變性和旋轉不變性的特征點，但對環境紋理的要求較高，如果特征點較少或者移動速度較快，特征點容易丟失［14］。2017年，PL-SLAM 系統［15］被提出，相比基于ORBSLAM 算法，它是第一個使用點特征和線特征進行融合的雙目SLAM 系統，且在光束（Bundle Ajustment，BA）優化階段，同時考慮兩種特征約束來優化相機位姿，從而提高了算法的精度和穩定性。然而，由于特征數量的增加，特征提取、匹配和優化部分的計算時間增加，導致PL-SLAM系統的實時性較差。

Company-Corcoles 等［16］提出了一種基于RGB-D 的視覺里程計稱為MSC-VO。該方法結合ORB 特征點和LSD 線特征［17］，用于估計相機的姿態。MSC-VO 通過估計空間中的曼哈頓軸以及點和線的重投影誤差，利用局部地圖進一步優化相機的姿態。這種綜合特征的使用和局部地圖的優化有助于提高相機姿態估計的準確性和穩定性。賀一家博士團隊［18］在VINS-mono［10］的基礎上，通過添加LSD 線特征跟蹤提出了一種PL-SLAM。他們結合VINS-mono 的單目視覺慣性里程計和LSD 線特征跟蹤，以實現更準確和穩定的SLAM。這種改進在保持VINS-mono 優勢的同時，通過引入線特征跟蹤提高了系統的穩定性。

在弱紋理環境下，尤其是白墻較多的走廊，盡管更多的特征可以提高算法的穩定性，但也會限制算法的實時性。原本對于前端點特征的提取已經很耗時，再加上線特征的提取和匹配，SLAM 系統的實時性難以保證。針對此問題，Gomez 等［19］在SVO［12］的基礎上提出了PL-SVO，引入了一種快速跟蹤線段的方法。將半直接方法擴展到線特征的情況下，通過作為稀疏-直接運動估計的隱式結果來執行快速線段跟蹤，并利用這種稀疏結構來消除線段檢測。吳毅紅團隊［20］提出了一種基于IMU-KLT 預測的高效線段跟蹤方法，通過IMU 的預測以及光流信息對線段位置進行預測，加快匹配速度，線段的跟蹤時間僅需要3.7 ms。然而，以上方法僅針對單目線段跟蹤展開研究，并未探索線段提取的質量問題。此外，在雙目情況下如何實現快速的線段匹配和三角化的研究仍然較少，目前主要依賴于傳統的LSD+LBD［21］方法，但該方法的實時性能較差。

本文在前人研究的基礎上，提出一種融合點線特征的雙目視覺SLAM 系統，重點改進了線特征的提取和匹配過程?？紤]到高質量和穩定性的線特征對SLAM 系統的貢獻更大，本文對LSD 線特征提取方法進行改進，通過長度和梯度抑制策略以及短線合并等方法進一步提高線段提取的質量。另外，本文還引入了一種基于幾何約束的快速線段三角化方法，通過引入區域約束、鄰域約束和平行約束構建代價函數，并將線段匹配問題轉化為優化問題。利用L1 范數的稀疏解求得最優匹配，并通過線特征的重投影誤差進行線段的非線性優化，從而提高線段三角化精度。在公開數據集和自建數據集上驗證了本文所提方法的先進性，并將它應用于實際的雙目視覺SLAM 系統中。實驗結果表明，該方法的定位精度優于現有算法，驗證了其在實際應用中的有效性。

2 原 理

2.1 雙目視覺SLAM 系統框架

本文所提出的融合點線特征的雙目視覺SLAM 系統框架主要分為跟蹤線程和局部建圖線程兩個部分，其框架示意圖如圖1 所示。

圖1 融合點線特征的雙目視覺SLAM 系統框架Fig.1 Framework of binocular vision odometer system integrating point and line features

在跟蹤線程中，雙目圖像作為輸入，首先對其利用多線程的方式分別對左右目圖像提取點特征與線特征，并利用特征匹配進行點線特征的三角化；在位姿估計中，通過對點線特征的持續跟蹤，構建點線特征的聯合重投影殘差，并利用非線性優化求解位姿，如下：

局部建圖線程通過具有共視關系的多個關鍵幀來維護一個共視窗口進行后端位姿圖的局部優化，主要通過將地圖中的點特征以及線特征的重投影誤差Epi，Elj作為相機位姿約束，以及曼哈頓軸約束進行聯合BA（光束平差法），對跟蹤線程得到的相機位姿進行進一步優化，從而消除累積誤差，如式（2）所示［16］：

其中：Mj表示對地圖線特征估計所得的曼哈頓軸，E||j，Mj代表曼哈頓軸Mj∈M相對于局部地圖中線特征j的誤差項。

2.2 基于改進LSD 的線特征提取方法

本文平衡考慮精度和速度，采用LSD 線特征提取方法。然而，由于LSD 算法提取的無效線特征數量較多，提取和匹配的時間成本較高。此外，高質量的特征更容易穩定地跟蹤，并對定位算法做出更大的貢獻。如果直接將LSD 算法應用于視覺SLAM 系統中，仍然會面臨一些問題需要解決。因此，本文提出了一種改進的LSD 線特征提取方法，通過對線特征的梯度和長度進行抑制，抑制了一些梯度較小且長度較短的線特征的產生。同時，在局部優化過程中考慮平行和垂直約束以及曼哈頓軸的估計，保留了相互平行和垂直的線特征。這樣做有利于提高線特征的質量，并為后續的線特征匹配和定位過程提供更好的約束條件。

對于一些短線特征可以通過判斷是否滿足短線合并條件從而進行線段的合并，從而成為質量更高的線特征。這種合并操作不僅可以減少線特征的數量，還能加快匹配和定位的速度，并降低定位算法的漂移風險。

假設Ln，Lm是需要合并的兩條線特征向量，故其合并條件為在圖像坐標系下滿足平行約束，且兩者左端點和右端點存在一定距離，由于斜率可能不存在，故采用向量夾角的余弦值作為判定條件，合并條件即：

其中：λmin為平行判定閾值（根據多次平行特征余弦值計算實驗。由于特征提取誤差導致其余弦值往往無法達到1，一般在0.999x以下的數量級，故確定為0.999），distance（）代表兩直線端點的最短距離，Ln.length 代表Ln的長度。綜上所述，基于改進LSD 的線特征提取算法的偽代碼描述如下：

在第3，4，5 步中，將梯度不明顯的像素和長度較小的矩形區域進行剔除，并將剩下的未存在于矩形區域的每個像素分配到最近的直線區域，這有助于抑制一些梯度較小且長度較短的線特征的產生。在第6 步中，對長度小于閾值的線特征，只保留相互平行或垂直的線特征，并判斷是否滿足合并條件，從而將短線特征合并成質量更高的線特征。

2.3 基于幾何約束的快速線段三角化

傳統方法下，幀與幀之間的線特征的三角化方法主要為首先提取兩幀之間LSD 線特征然后利用LBD 描述子進行匹配，從而得到歸一化平面上線對的直線方程，若已知相機中心C1，C2在世界坐標系中的位置，便可得到線與相機中心的兩個平面方程，通過平面方程的交線便可求出線特征的世界坐標［16］，示意圖如圖2 所示。

圖2 線特征三角化方法Fig.2 Line feature triangulation method

本文針對雙目視覺的線特征三角化問題，提出一種基于幾何約束的線特征三角化方法。針對線特征的匹配問題，傳統的基于LBD 描述子的匹配方式應用于雙目線特征匹配，計算描述子復雜且匹配時間較長，不適用于弱紋理環境。雙目平行線特征匹配和三角化方法卻能提高線特征的匹配效率，其示意圖如圖3 所示。

圖3 基于幾何約束的雙目線特征三角化方法示意圖Fig.3 Schematic diagram of binocular line feature triangulation method based on geometric constraints

此外，雙目圖像經過立體校正和去畸變處理后，雙目圖像中的像素點在對應的行上具有嚴格的一一對應關系。利用極線約束，兩幅圖像中的對應線在同一水平線上，因此線對應滿足區域約束。如圖3 所示，區域約束的應用過濾掉大部分無關的線特征，從而加快了匹配速度。右目匹配線段應處于區域內或與區域相交，故定義區域約束Dij，將線段lk={sk，ek}，sk和ek分別為線段的兩端點，故區域約束Dij如下：

鑒于平行約束與區域約束僅針對線特征本身，并未利用其鄰域信息，本文基于傳統的NCC匹配方法提出一種線特征的鄰域約束，即取左目線段的中心構建3×3 鄰域匹配窗口Wp，并通過建立目標函數來對右目線特征的匹配窗口進行度量相關性，如式（7）所示：

其中：d為左右目匹配中心區域的橫坐標之差，Ii(x，y)代表左目鄰域窗口Wp中的像素灰度值，Ij(x+d，y)代表右目匹配窗口Wp中的像素灰度值。若NCC=-1，表示兩個匹配窗口完全不相關；若NCC=1，則表示兩個匹配窗口相關程度非常高。令Nij=(1-NCCij)/2，故可定義左右目線對的匹配代價向量Qij為：

其中Uij為線段的長度約束，設兩線段長度的比值為u，故Uij=|u-1|。若定義在左目區域約束內的所有線段的代價向量為Ai={Qi0，Qi1…，Qin}，由于雙目線特征匹配問題具有奇異性，即左右目線段都是唯一對應的，若設其匹配向量xi={xi0，xi1…，xin}，其中xij表示線對匹配成功則為1，反之則為0。進一步建立線性約束Ai xi=b，其中b=[0 0 0 0]代表匹配誤差，保證其奇異性，且匹配向量xi歸一化后所有分量之和為1。因此，在上述約束條件下，將線段匹配問題轉化為優化問題，利用L1 范數得到最優解的稀疏表示［23］：

初始值為x0={1，0…，0}，采用牛頓高斯法求得其數值最優解后，將xi進行歸一化，xi中的最大分量xim代表兩線段的相似性最大，即為查詢線段的最佳匹配。

在雙目線段匹配成功后，本文采用一種簡單而有效的方法進行線特征的三角化。通過左目線段端點縱坐標的橫線與右目匹配線段的交點，可以確定右目匹配線段的端點，從而完成線特征的三角化。此外，本文還借鑒文獻［16］中RGBD相機的線特征優化方法，對雙目三角化得到的線特征進行了非線性優化，利用線特征的重投影誤差求得最優解。其優化公式為：

其中：Ek，o為歸一化線與平行關聯線段之間的夾角的正弦，ρ為減少異常值影響的Huber損失函數。

3 實 驗

3.1 線特征提取與匹配

線特征提取與匹配實驗設備如圖4 所示，弱紋理圖像采集設備采用小覓雙目相機（S1030），分辨率為1 344×376，焦距為2.1 mm，基線長度為12 cm，幀率為30 Hz，自建數據集場景為弱紋理室內走廊環境，場景中僅存在白墻和門窗，如圖5 和圖6（a）所示，距離全長為91 m 左右，錄制時長為216 s，圖像數量為2×6 684 張。

圖5 線特征提取算法對比Fig.5 Comparison of line feature extraction algorithms

圖6 雙目匹配算法在弱紋理數據下的對比Fig.6 Comparison of binocular matching algorithm under weak texture data

本文將所提出的線特征提取與匹配方法與目前已有方法進行對比，線特征提取算法與原本的LSD 算法進行對比，結果如圖5 所示。

由圖5 可知，原始LSD 算法提取的線段分布較為集中，短小線段居多，且大部分梯度不明顯；相比之下，改進LSD 線特征提取算法提取的線段質量明顯高于原LSD 算法，且線特征大多為水平和豎直線段，為曼哈頓軸估計和基于線特征的位姿約束提供了更強的約束信息，更容易被跟蹤。

線特征匹配實驗中，將本文所提基于幾何約束的線特征匹配算法與傳統的基于LBD 描述子方法進行對比。為了評估算法性能，本文使用的雙目匹配數據集為公開雙目數據集（室內Euroc數據集［24］、室外KITTI 數據集［25］）與自建弱紋理數據集。圖6 展示了在弱紋理圖像數據上進行的線特征匹配結果（彩圖見期刊電子版）。

在線特征提取方面，如圖6（a）和6（c）中紅色圓圈所示，LSD 算法提取的線特征大多具有較短的長度和較小的梯度，無法進行有效的匹配，因此被認為是無效特征。相比之下，本文提出的改進線特征檢測算法（如圖6（b）和6（d）所示）明顯提高了線特征的質量，無效線特征的數量較少。在線特征匹配方面，本文所提方法基本無錯誤匹配，而基于LBD 描述子的方法存在多處錯誤匹配，如圖6（a）和6（c）中紅色線段所示，匹配精度明顯高于LSD+LBD 算法。為了定量評價雙目線特征匹配算法的精度，一般認為投影誤差小于5 個像素為正確匹配。實驗中，使用正確匹配的數量與匹配總數量之比來評估算法平均實驗的匹配精度［20］。

本文所用實驗平臺CPU 為i5-4690，系統為ubuntu18.04；共進行20 組實驗，每次實驗取數據中跨度較大的實驗場景，平均實驗匹配精度與匹配時間如表1 所示。

表1 該方法與基于LBD 的線特征匹配方法對比Tab.1 Comparison between proposed method and line segment matching method based on LBD

表1 展示了本文所提算法與現有算法在公開數據集以及自建弱紋理數據集上的性能。首先，在線特征檢測數量方面，LSD 算法的平均檢測數量最多為84，FLD 算法平均檢測數量為63。雖然本文所提算法最少為54，但在匹配精度方面，基于LBD 描述子的線特征匹配方法中，LSD 算法的平均匹配精度為71%，FLD 的平均匹配精度為55%，而本文所提方法的平均匹配精度為85%，優于其他兩種方法。這主要歸因于本文方法提取的線特征質量更高，并且通過雙目的幾何結構約束，每條線特征具有唯一的極大值解。在結構約束消融實驗的對比中，當缺少鄰域約束時，平均精度為78%，減少6%，而缺少區域約束為83%，減少2%，可看出鄰域約束的作用大于區域約束，也證明了兩種約束的有效性。

表2 比較了3 種不同方法的線特征平均提取時間以及匹配時間。其中，線特征提取時間是指兩張圖片的線特征提取時間，匹配時間則是指描述子的計算時間與匹配時間之和。在線特征提取方面，FLD 的提取速度最快，僅需18.5 ms，LSD 的線特征提取速度最慢，需要34.2 ms，本文所提出的算法的線特征提取速度居中為33.1 ms。在匹配時間方面，本文算法的線特征匹配時間僅需7.4 ms，而其他兩種算法的匹配時間分別需要17.5 ms 和15.3 ms。這表明基于幾何約束的線特征匹配方法比基于描述子的匹配方法具有更快的匹配速度。

表2 不同算法的時間性能比較Tab.2 Comparison of time consumption of different algorithms（ms）

綜上所述，本文提出的基于幾何約束的線特征匹配方法在精度和時間方面均表現出色，具有較高的實用性和效率。該方法能夠提供精準的線特征匹配結果并在較短的時間內完成匹配，在雙目視覺SLAM 系統中具有很大的應用潛力。

3.2 SLAM 建圖與定位實驗

本文將線特征檢測和線特征快速三角化方法到雙目視覺SLAM 系統中，提出一種點線特征融合的雙目視覺SLAM 系統。為了評估該系統的性能，選擇弱紋理的走廊和樓梯場景作為測試場景進行了對比實驗。將本文方法與基于點特征的雙目視覺SLAM 系統（ORBSLAM2 算法）進行對比，它們的建圖和定位效果如圖7 所示。

圖7 改進雙目點線融合視覺SLAM 與ORBSLAM2 算法建圖與定位效果對比Fig.7 Comparison of image construction and positioning effects between improved binocular point line fusion visual SLAM and ORBSLAM2 algorithms

從圖7（a）可以看出，基于點特征的ORBSLAM2［7］算法在轉彎時由于特征缺失發生了軌跡漂移導致最終與起點發生重大偏離，而在圖7（b）中則直接導致特征跟蹤失敗，算法失效。而點線融合雙目視覺SLAM 系統在走廊和樓梯場景下產生的軌跡依然平滑，不存在軌跡漂移以及跟蹤失敗的現象，且最終成功回到起點，進一步證明本文所提算法的抗干擾能力更強，且本文算法在CPU 為i5-4690，系統為ubuntu18.04 的電腦主機上的平均運行幀率達到18.1 frame/s，滿足實時性要求，具有更強的實用性能。為了進一步驗證算法的先進性，將它與已有的基于點線融合的PL-SLAM［15］以及先進的ORBSLAM3［9］進行對比，它們的軌跡如圖8 所示。

圖8 四種算法的軌跡對比Fig.8 Trajectories comparison of four algorithms

圖8 展示了4 種算法在具有挑戰性的長方形環形走廊場景下的定位軌跡.從圖8（a）可以觀察到，本文所提出的改進算法是唯一成功回到起點形成閉環的算法，其他兩種算法則無法完成閉環，而ORBSLAM2［8］，ORBSLAM3［9］由于特征缺失也未觸發回環。以回到起點的距離作為誤差判定，本文所提算法、ORBSLAM2［8］，ORBSLAM3［9］，PL-SLAM［15］的定位誤差分別為1.24，7.49，6.36 和3.67 m。實驗結果表明，該算法在弱紋理環境下的定位誤差較小，相對于ORBSLAM2［8］，ORBSLAM3［9］算法，其精度分別提高了6.04 倍和5.13 倍；相對于PLSLAM［15］算法，其精度提高了2.96 倍。因此，在具有挑戰性的弱紋理環境下，除了追求更多的特征之外，還需要確保特征的質量和穩定性，并提高線特征匹配的精度，以進一步提高定位的準確性。

4 結論

針對室內弱紋理環境下視覺SLAM 出現軌跡漂移等問題，本文提出了一種融合點線特征的雙目視覺SLAM 系統，針對雙目視覺SLAM中線特征提取與匹配問題進行研究，提出了一種基于改進的LSD 線特征提取方法，并在此基礎上提出了一種基于幾何約束的快速線特征三角化方法。實驗結果表明，該方法在多個數據集上的表現都優于傳統的LSD+LBD 方法，在室內弱紋理場景下的平均匹配精度達到91.67%，平均匹配時間為7.4 ms。雙目視覺SLAM 系統的定位誤差為1.24 m，其精度比ORBSLAM2，ORBSLAM3 算法高6.04 倍和5.13 倍，比PL-SLAM［算法高2.96 倍，具有很好的實用性和應用前景。未來，可以考慮更多的幾何約束條件來進一步提高線特征匹配的準確性，或者探索更高效的線特征三角化方法，以及引入深度學習技術以提高系統的穩定性和泛化能力。