基于ORBSLAM2 改進的單線程雙目SLAM 系統

王劭靖

（北方工業大學 信息學院，北京 100144）

0 引言

視 覺 SLAM（Simultaneous Localization And Mapping）是計算機視覺和機器人技術界的一個熱門研究課題。目前視覺SLAM 技術廣泛應用于航天機器人、AR／VR，和自動駕駛等領域。

視覺SLAM 系統主要分成前端和后端兩大部分。前端又被稱為視覺里程計，主要分為基于直接法和基于特征點法兩種；后端主要分為基于濾波優化和基于非線性優化兩種［1］。近些年來，基于直接法的代表性研究成果有SVO 和DSO，但容易受光照的影響導致位姿估計失敗，并且沒有閉環檢測功能，使得估計的相機軌跡存在較大漂移，在跟蹤丟失后不容易重定位；而基于特征點法的視覺SLAM 系統對光照和動態物體不敏感并且穩定，還可以利用基于描述子的閉環算法消除相機軌跡的漂移，減少累計誤差，因此基于特征點法的視覺SLAM 系統是目前主流且成熟的視覺方案，并且大多使用基于非線性優化的后端。PTAM 是首個基于特征點法的實時視覺SLAM 系統，并且首創了前端和后端并行的雙線程結構［2］。隨后出現的許多優秀的視覺SLAM 系統都是基于PTAM 進行改進的，其中ORBSLAM2 被業界人士認為是當前最穩定的視覺SLAM 系統，可應用于多種場景，在視覺SLAM 領域具有里程碑的意義。

ORBSLAM2 使用了四線程結構。跟蹤線程主要對提取的特征點利用描述子進行匹配，從而構造數據關聯，并根據數據關聯關系對相機位姿實時地進行估計。在當前幀確定為關鍵幀后，建圖線程使用局部BA 優化共視圖（Co-visibility graph）中的關鍵幀和三維空間點。閉環線程中優秀的閉環檢測算法和基于本質圖（Essential graph）的位姿圖優化保證了ORBSLAM2 能夠有效地減少累計誤差。此外，全局BA 單獨占用一個線程同時優化所有的關鍵幀和三維空間點。因此，ORBSLAM2 比其他現有視覺SLAM 系統具有更好的準確性和魯棒性，但其四線程結構給CPU 帶來了較重的負擔，很難將其移植到嵌入式設備上。此外，航天設備上需要多種技術共同協作，SLAM 的資源占用率的減少不僅能夠降低硬件成本，還能滿足其他技術對資源的需求。因此，本文將對ORBSLAM2 進行改進，使其能夠以單線程的形式在CPU 上實時運行并且準確地估計相機位姿。

1 相關工作

視覺SLAM 系統前端主要負責數據關聯與位姿估計。良好的位姿初值是建立準確數據關聯的前提，有效的運動模型能夠提供一個良好的位姿初值，從而加速特征匹配的速度和精度，并且加快位姿估計的速度。常見的運動模型通常在相機相鄰兩幀之間應用平滑的運動約束［3］，即假設前后兩幀的運動速度相似，DP-PTAM、DT-SLAM 和ORBSLAM2 都利用恒速運動模型假設相機前后兩幀的運動速度相同，進而預測相機的位姿初值。SVO 和LSD-SLAM直接令相機當前時刻的位姿等于上一時刻的位姿，即假設當前幀的運動速度為0。但在實際應用中，相機的運動速度是經常變化的，不符合上述的假設，因此本文將相機的運動假設為速度不斷變化的加速運動，將平滑運動約束的思想應用到前后幀的加速度上，假設前后幀的加速度相同來計算當前時刻的速度，最終預測出當前時刻的位姿初值，以提高數據關聯的效率和正確性。

由于BA 同時優化大量的三維空間點坐標和相機位姿，因此計算復雜度高導致運算時間過長，很難實現快速而準確的收斂［4］，通常需要開啟多個線程來分擔計算量以滿足實時性的要求。Chatterjee［5］等人提出了一種基于L1范數的旋轉平均（Rotation averaging）的方法能夠魯棒、快速地單獨優化相機的朝向。近些年來許多研究都證實了基于旋轉平均的解耦合優化方法比BA 的效率和精度更高，能夠快速、精確地優化出相機的位姿。Fan［6］等人提出了將三維空間點的位置、相機的朝向和平移進行解耦合優化的方法來提高優化的效率和精確性，并且假設噪聲服從拉普拉斯分布，以提高對存在大噪聲數據優化的魯棒性。本文采用基于平均的解耦合優化方法代替ORBSLAM2 中的Local BA，以提高優化的效率、魯棒性和精確性，并減少SLAM 的線程數量。

位姿圖優化是SLAM 進行全局優化的重要手段，能夠有效減少相機軌跡的漂移，從而減少累積誤差。但隨著相機的不斷探索，位姿圖的規模會越來越大，使得優化效率受限。Kretzschmar［7］等人提出了基于信息論的方法，減少位姿圖優化的規模，通過計算互信息和信息增益的期望挑選出需要邊緣化的節點，并提出了一種基于Chow-Liu 樹的近似邊緣化方法保證位姿圖的稀疏性，但理論復雜且實現較困難；Johannsson［8］、Zeng［9］等人認為當連續幀連續檢測到閉環時，說明相機正重新經過之前探索過的區域，就將連續檢測到閉環的多個幀視為冗余，并將多個冗余幀和回環幀之間的約束進行合并，直接將冗余節點去除，不進行邊緣化操作，但不適用于大場景；Latif［10］等人認為在二維平面上運動且不能原地旋轉的機器人在做直線運動時朝向是幾乎不變的，因此直線運動中的約束對位姿圖優化提供的是線性約束，于是就將直線運動中的多個約束合并為一個約束，只對位姿圖中朝向改變較大的位姿節點進行優化，但對相機的運動條件限制嚴格，不適用于三維場景中能夠自由轉動的相機；車聯網領域，Keogh［11］等人提出的開放窗口算法是一種快速壓縮軌跡數據的簡單方法，該算法將車輛軌跡根據彎曲程度用多個線段子軌跡表示。本文將車聯網領域中軌跡壓縮的思想應用到SLAM 中，并對Latif 等人的方法進行改進，以此只優化朝向變化較大的位姿節點，減少位姿圖優化的規模，并且適用于在三維場景中自由轉動的相機。

2 具體改進

本文提出的SLAM 系統是基于ORBSLAM2 的框架改進的，本文SLAM 系統的流程圖如圖1 所示，主要改進：

圖1 本文SLAM 系統的流程圖Fig.1 The flow chart diagram of our SLAM system

（1）使用加速運動模型預測相機的位姿，并結合環形匹配策略提高特征匹配的精度和速度；

（2）在當前幀確定為關鍵幀后，利用基于開放窗口的關鍵幀聚類算法對其進行聚類，減少位姿圖優化的規模，加速位姿圖優化的速度；

（3）在噪聲服從拉普拉斯分布假設的前提下，使用解耦合優化三維點空間、相機的朝向和平移的方法代替Local BA，以此提高優化的速度和精度。

2.1 基于平滑運動約束的加速運動模型

良好的位姿初值不僅僅能提高數據關聯的正確性，還可以減少優化的迭代次數，加速收斂速度。本文考慮到相機的運動大部分情況是速度不斷變化的加速運動，勻速運動的情況較少，并且勻速運動和減速運動也分別是一種加速度為0 和加速度為負數的特殊加速運動。因此本文利用加速運動模型估計相機位姿的初值，并將平滑運動約束的思想應用到運動模型的加速度上，假設相鄰兩幀的加速度相同，則有式（1）：

其中，Tt-1、Tt-2、Tt-3分別代表t-1 幀和t-2 幀和t -3 幀的位姿。

因此t幀的速度和預測的位姿可由以上公式推導而出，式（4）和式（5）：

由此能夠根據t幀的預測位姿將t -1 幀觀測到的三維空間點投影至t幀進行匹配。為了使特征匹配更可靠，本文加入了環形匹配策略。在ORBSLAM2 中，每一幀左右圖像的特征匹配關系是在每一幀初始化時確定的，因此在通過投影得到前后幀左圖像中特征點的對應關系后，可直接得到前后幀右圖像中特征點的對應關系，利用描述子驗證環形是否閉合，最終確定匹配成功的特征點，如圖2所示。特征匹配結果對比如圖3 所示，可以看出本文的特征匹配數量相比于ORBSLAM2 少，但準確度更高，進一步剔除了更多的誤匹配。

圖2 環形匹配示意圖Fig.2 Schematic diagram of circle matching

圖3 特征匹配結果對比示意圖Fig.3 Schematic diagram of feature matching results comparison

2.2 基于開放窗口的關鍵幀聚類算法

為了挑選出運動過程中朝向變化較大的位姿節點，本文提出了基于開放窗口的關鍵幀聚類算法。該算法首先對位姿圖中的位姿節點在?3空間中聚類，以此將位姿圖劃分為多個線段子軌跡，每個線段子軌跡都是一段直線運動，每個直線運動的尾結點和下一個直線運動的首節點之間會發生較大的朝向變化。此外，由于相機在直線運動過程中也會出現朝向變化較大的情況，因此每一段在?3空間中確定的子軌跡需要繼續在SO（3）空間中進行聚類，以此有效挑選出朝向變化較大的節點進行優化，減小位姿圖優化規模。

?3空間中關鍵幀聚類示意如圖4 所示，在當前幀確定為關鍵幀時，令其為xcurrent，窗口中的位姿節點為xstart和xend之間的所有位姿節點，根據xstart和xend可確定直線L，由此可以計算窗口內所有位姿節點相對直線L的偏移量，即位姿節點到直線L的垂直距離，即式（6）：

圖4 ? 3空間中關鍵幀聚類示意圖Fig.4 Schematic diagram of keyframe clustering in ? 3

其中，t∈?3，代表位姿節點的平移部分。

當偏移距離最大的xi的偏移距離大于閾值時，將xstart和xi之間的所有節點聚為一類，表示在?3空間中的聚類結果，用xstart和xi代表這段子軌跡，并更新窗口，即令xstart為xi＋1，令xend為xcurrent。

每一段在?3空間中確定的子軌跡再分為兩步在SO（3）空間中進行聚類：

第一步，在xstart與xi構成的子軌跡中判斷相鄰位姿節點之間的朝向變化，當xj和xj＋1朝向變化大于閾值時，就將這段子軌跡繼續劃分，即xstart和xj之間的所有位姿節點聚為一段子軌跡，并繼續判斷后面相鄰位姿節點之間的朝向變化，如圖5 所示；

圖5 SO（3）空間中基于相鄰關鍵幀朝向變化的關鍵幀聚類示意圖Fig.5 Schematic diagram of keyframe clustering based on the orientation change of adjacent keyframes in SO（3）

第二步，考慮到存在相鄰幀之間朝向變化不大而不相鄰兩幀之間朝向變化較大的情況，本文將第一步中的子軌跡在SO（3）空間中進行第二次聚類，即當xstart與xk之間朝向變化大于閾值時，就將xstart和xj這段子軌跡繼續劃分，即xstart和xk-1之間的所有位姿節點聚為一段子軌跡，并繼續判斷后面不相鄰位姿節點之間的朝向變化，如圖6 所示。具體的基于開放窗口的關鍵幀聚類算法如算法1 所示，其中Dthreshold代表在?3空間中進行聚類的偏移距離閾值，thetaThreshold代表在SO（3）空間中進行聚類的角度閾值。

圖6 SO（3）空間中基于不相鄰關鍵幀朝向變化的關鍵幀聚類示意圖Fig.6 Schematic diagram of keyframe clustering based on the orientation change of non-adjacent keyframes in SO（3）

在KITTI 07 序列上應用該聚類算法的結果示意圖如圖7 所示。圖7 中的大節點是朝向變化較大的節點，即需要優化的位姿節點，可以看出本文提出的基于開放窗口的關鍵幀聚類算法能夠有效減少位姿圖優化的規模。針對其余未進行位姿圖優化的節點位姿，本文使用位姿傳播的方式更新。

圖7 基于開放窗口的關鍵聚類算法在KITTI07 上的聚類結果示意圖Fig.7 Schematic diagram of the clustering results of the keyframe clustering algorithm based on the open window on KITTI07

2.3 解耦合優化方法

為了在SLAM 后端避免復雜的BA 優化，本文在假設噪聲服從拉普拉斯分布的前提下構建L1范數誤差，使用解耦合優化三維空間點、相機朝向和平移的方法代替Local BA。

得益于根據數據關聯建立的共視圖，一個三維空間點在多個關鍵幀中的觀測能夠快速被找到。因此，三維空間點在多個關鍵幀的觀測根據雙目視差能夠計算出多個坐標，如圖8 所示。本文采用?3上的IRLS（Iterative Reweighted Least Squares）算法優化三維空間點坐標為

圖8 利用雙目視差多次重建三維空間點示意圖Fig.8 Schematic diagrams of 3D space points are reconstructed multiple times according to binocular disparity

對于相機朝向的優化，本文利用數據關聯關系快速得到當前關鍵幀與多個共視關鍵幀的特征匹配關系，從而使用五點法求解出當前關鍵幀和多個共視關鍵幀之間本質矩陣（Essential matrix），因此可得到當前關鍵幀的多個朝向，使用SO（3）上的IRLS（Iteration Reweight Least Square）算法優化相機的朝向為

其中，代表相機朝向的初值，本文取多個朝向中的任意一個朝向作為初值，R*代表優化后的相機朝向。

最后利用優化后的三維空間點和相機的朝向根據重投影誤差優化相機的平移為

其中，ui代表三維空間點Pi在當前關鍵幀上的二維觀測點坐標，π（R Pi ＋t）代表三維空間點Pi對應在當前幀上的投影。

3 實驗結果與分析

本文的實驗環境為Ubuntu 18.04、配置為Inter Core i7-5500 CPU、主頻2.4 GHz、8 G RAM。為了檢測本文SLAM 算法的性能，本文從精度和速度兩個方面在KITTI 數據集以及New Tsukuba 數據集上進行測試評估。KITTI 數據集采集了大型室外場景下的視頻序列，并提供了真實軌跡的數據；New Tsukuba 數據集是由計算機合成的靜態室內小場景。本文選擇與目前開源的Viso2 和ORBSLAM2 算法進行對比，使用均方根誤差（RMSE）來衡量相機真實位姿與估計的位姿之間的誤差。本文在算法1中使用的偏移距離閾值為0.5，角度閾值為0.2 rad。

Viso2、ORBSLAM2 和本文方法在KITTI 數據集上和New Tsukuba 數據集的測試結果見表1，可以看出本文方法的精度比ORBSLAM2 的精度與Viso2 的精度高，并且由于Viso2 沒有閉環檢測功能，在具有閉環的序列中表現差距更加明顯。

表1 Viso2、ORBSLAM2 和本文方法在KITTI 和New Tsukuba 數據集上的RMSE（m）對比Tab.1 RMSE（m）comparison of Viso2，ORBSLAM2 and our method on KITTI and New Tsukuba

本文算法在精度方面的優勢主要得益于加速運動模型以及環形匹配策略提高了數據關聯的準確性，盡管在數據關聯過程中錯誤的數據關聯并不能完全消除，但基于構建L1范數誤差的解耦合優化方法提高了對存在異常值數據進行的優化魯棒性。

ORBSLAM2 與本文方法在New Tsukuba 數據集上處理每一幀的時間對比圖如圖9 所示，其中細折線代表ORBSLAM2 處理每一幀的時間，粗折線代表本文方法處理每一幀的時間，可以看出本文方法處理每一幀的時間整體上要低于ORBSLAM2 處理每一幀的時間。本文的SLAM 算法與ORBSLAM2 在New Tsukuba 數據集上的計算效率和線程數量對比見表2，可以看出本文方法的計算效率高于ORBSLAM2，并且能夠以單線程的形式運行。

圖9 ORBSLAM2 與本文方法在New Tsukuba 數據集上處理每一幀的時間對比圖Fig.9 Time comparison of ORBSLAM2 and our method on the New Tsukuba dataset

表2 本文算法與ORBSLAM2 在New Tsukuba 數據集上的計算效率和線程數量對比Tab.2 Comparison of computational efficiency and number of threads between our algorithm and ORBSLAM2 on the New Tsukuba dataset

本文算法對比于ORBSLAM2 在計算效率和線程數量方面的優勢主要歸因于如下幾點：

（1）加速運動模型預測的良好位姿初值和較為準確的數據關聯使得位姿估計過程中的僅運動BA（motion-only BA）計算的梯度更為準確，能夠使位姿估計過程更快地收斂；

（2）解耦合的優化方法計算復雜度比BA 低，由此減少了優化時間；

（3）基于開放窗口的關鍵幀聚類算法能夠明顯減少位姿圖優化的規模，使得全局優化的效率得以提升；

（4）由于位姿估計過程并不需要太多數量的三維空間點，過多的三維空間點對位姿估計的精度提升并不明顯，還會增加計算的復雜性。因此本文只利用當前幀的共視關鍵幀所觀測到的三維空間點對當前幀進行位姿估計，相比ORBSLAM2 利用當前幀的共視關鍵幀以及共視關鍵幀的共視關鍵幀所觀測到的三維空間點對當前幀進行位姿估計的方法，能夠有效減少不必要的計算量，并且保證位姿估計的精度。

在KITTI 數據集和New Tsukuba 數據集上本文的方法、Viso2、ORBSLAM2 的軌跡和真實軌跡對比如圖10 所示，其中粗實線為真實軌跡，細實線為本文方法估計的軌跡，粗虛線為ORBSLAM2 估計的軌跡，細虛線為Viso2 估計的軌跡，可以看出本文方法估計出的軌跡更接近于真實軌跡。

圖10 KITTI 數據集和New Tsukuba 數據集上本文的方法、Viso2、ORBSLAM2 的軌跡和真實軌跡對比圖Fig.10 Comparison of trajectories and real trajectories of our method、Viso2、ORBSLAM2 on the KITTI dataset and the New Tsukuba dataset

4 結束語

本文主要針對ORBSLAM2 進行3 方面改進：

（1）使用加速運動模型預測相機的位姿，并結合環形匹配策略提高了數據關聯的準確性和位姿優化的收斂速度；

（2）基于開放窗口算法提出了一種簡單有效的關鍵幀聚類算法，從而減少位姿圖優化的規模；

（3）使用基于拉普拉斯分布的解耦合優化方法代替Local BA 提高位姿優化的速度和精度。

在KITTI 和New Tsukuba 數據集上的實驗結果表明，本文改進后的SLAM 系統能夠以單線程的形式運行，并且運行速度和精度都高于ORBSLAM2。