爆改ORB-SLAM2，能否實現(xiàn)更高魯棒性的多攝像頭SLAM系統(tǒng)？

作者：

Han Song， Cong Liu， Huafeng Dai

01 ? 摘要

多攝像頭SLAM系統(tǒng)提供了許多優(yōu)勢，主要源于其能夠整合更廣泛視野的信息，從而提高魯棒性和定位精度。在這項研究中，我們對現(xiàn)有的立體SLAM系統(tǒng)ORB-SLAM2進行了重大擴展和改進，旨在實現(xiàn)更高的精度。為了實現(xiàn)這一目標，我們首先將所有攝像頭的測量映射到一個名為BundledFrame的虛擬相機上。這個虛擬相機經(jīng)過精心設計，可以無縫適應多攝像頭配置，有助于有效地融合來自多個攝像頭的數(shù)據(jù)。此外利用捆綁調整（BA）過程中的外參，實現(xiàn)精確的軌跡估計。此外對多攝像頭場景中捆綁調整（BA）的作用進行了廣泛的分析，探討了其對跟蹤、局部建圖和全局優(yōu)化的影響。我們的實驗評估涉及對比地面真實數(shù)據(jù)和最先進的SLAM系統(tǒng)。為了嚴格評估系統(tǒng)的性能，使用了EuRoC數(shù)據(jù)集。我們評估的結果一致表明，與現(xiàn)有方法相比，該系統(tǒng)具有更高的準確性。

02 ? 主要貢獻

本文的主要目標是通過擴展ORB-SLAM2的功能來增強準確性，從多個攝像頭中的姿態(tài)估計和地圖重用開始。所有這些多攝像頭的圖像特征將被合并到跟蹤模塊中進行特征匹配，以及在閉環(huán)檢測期間進行位置識別。此外通過最小化涉及多個攝像頭的成本函數(shù)來實現(xiàn)姿態(tài)更新和優(yōu)化。在將視覺SLAM系統(tǒng)視為虛擬傳感器方面，我們受到了Wang等人的工作的啟發(fā)。我們首先將所有攝像頭的測量映射到一個稱為BundledFrame的虛擬相機上。這個虛擬相機被設計成可以無縫適應多攝像頭設置，使我們能夠高效地合并來自多個攝像頭的數(shù)據(jù)，然后使用外部參數(shù)進行捆綁調整，以優(yōu)化多攝像頭SLAM系統(tǒng)中的姿態(tài)。我們的多攝像頭系統(tǒng)的主要貢獻可以總結如下：?

全面性：我們?yōu)槎鄶z像頭提供了一個完整的SLAM系統(tǒng)，包括閉環(huán)檢測和地圖重用。?

可擴展性：通過利用一種稱為“Bundled”的高效數(shù)據(jù)結構，我們將多個攝像頭的數(shù)據(jù)合并到一個“BundledFrame”或“BundledKeyframe”中。這形成了所有系統(tǒng)操作的基礎，包括跟蹤、位置識別和優(yōu)化。我們的系統(tǒng)通過在攝像頭之間實現(xiàn)外部參數(shù)的捆綁調整，可以輕松適應額外的攝像頭。

03 ? 內容概述

A. 系統(tǒng)概述?

圖1:BundledSLAM的方案流程

圖1展示了我們多攝像頭SLAM系統(tǒng)的流程，我們的系統(tǒng)分為三個主要的并行線程：跟蹤、局部建圖和閉環(huán)檢測。跟蹤模塊負責通過在局部BundledMap中識別特征匹配來估計增量運動，并使用我們的僅運動Bundle調整（BA）來最小化重投影誤差。它還確定當前幀是否符合新的BundledKeyframe，隨后將其整合到局部建圖線程中。局部建圖線程管理新的BundledKeyframes，涉及一致性連接更新、新地圖點的創(chuàng)建和冗余數(shù)據(jù)的刪除。通過實現(xiàn)我們的局部Bundle調整（BA），它優(yōu)化了局部BundledMap。閉環(huán)檢測線程的主要目標是檢測重要的環(huán)，并執(zhí)行位姿圖優(yōu)化。此外，該線程啟動另一個線程來進行全局Bundle調整（BA），以糾正累積的誤差。與先前基于特征的SLAM系統(tǒng)類似，BundledSLAM首先對輸入圖像進行預處理，以在顯著的關鍵點位置提取特征。一旦特征提取完成，原始輸入圖像將被丟棄，所有后續(xù)的系統(tǒng)操作都依賴于這些特征，用于跟蹤、位置識別和優(yōu)化等任務。

圖2：Bundled的示例，包括唯一特征ID、單眼點和匹配點。唯一特征數(shù) = 單眼特征數(shù) + 匹配特征數(shù)

為了促進來自不同攝像頭的圖像特征的集成，我們實現(xiàn)了跨多個攝像頭的特征匹配，為每個特征點分配一個唯一的特征ID。這些唯一特征ID與它們對應的攝像頭ID（每個觀察到的攝像頭）之間的關聯(lián)存儲在我們的專用數(shù)據(jù)結構'Bundled'中，如圖2所示。只由單個攝像頭觀察到的特征點稱為“單眼特征點”，而其他特征點則稱為“匹配點”。Bundled提供了比ORB-SLAM2更廣泛的特征搜索范圍，后者僅考慮左-左攝像頭之間的匹配，以找到準確的圖像特征和地圖點。

B.BundledFrame、BundledKeyframe和BundledMap

圖3：BundledFrame 的總體概覽

BundledFrame（見圖3）包含來自不同攝像頭的所有圖像信息。此外，來自不同攝像頭的幀分別劃分為 64×48 大小的網(wǎng)格，并根據(jù)位置將特征分配到相應的網(wǎng)格中，以降低匹配的時間復雜度。BundledKeyframe 在跟蹤的最后一步?jīng)Q定當前 BundledFrame 是否被選為新的 BundledKeyframe。同時，BundledKeyframe 是本地映射和本地閉合的基本操作單元。共視性表示為無向加權圖。如果兩個 BundledKeyframes 共享相同地圖點的觀測，則存在一個共視邊。接下來的本地 BA 和閉環(huán)依賴于這些共視信息。BundledMap 由所有 BundledKeyframes 和被 BundledKeyframes 觀察到的所有地圖點的集合組成。

C.多相機 SLAM 系統(tǒng)中相機投影模型

相機狀態(tài)向量: 在時間步 k，第 i 個攝像頭的狀態(tài)向量包括相對于世界坐標系的傳感器方向和位置，表示為 cki = [Rkiw, tk_iw; 0, 1]。其中，Rkiw 是旋轉矩陣，表示世界坐標系到第 i 個攝像頭坐標系的轉換，而 tk_iw 是位置向量，表示相機在世界坐標系中的位置。

相機投影模型: 使用與已有研究相同的校準相機投影模型，描述特征點 j 的觀測。這個投影模型將特征點的三維位置向量 Pj 投影到第 i 個攝像頭的圖像平面上，并考慮了測量噪聲。投影后的像素位置表示為 zji，其中 h(·) 是相機的投影函數(shù)，nji 是測量噪聲。

姿態(tài)更新: 對于特定的時間步 k，通過測量來自所有攝像頭的數(shù)據(jù)，計算第一個攝像頭 Ck1 的姿態(tài)更新。然后，通過相對于第一個攝像頭的估計恒定轉換來更新其他攝像頭的姿態(tài)。這個相對轉換 Ti1 是固定的，因為它是不可觀測的，避免了整個系統(tǒng)由于攝像頭和轉換的自由運動而導致的漂移。

D.多相機的運動估計和束調整

我們的目標是準確地估計每個時間步的相機姿態(tài)，以便構建出準確的地圖。首先介紹了我們的運動估計方法，為每個時間步的第一個相機姿態(tài)賦予初始值，如果上一幀中的跟蹤成功，我們就將初始值設為前一相對運動，假設了一個恒定速度運動模型。然后，我們對相機姿態(tài)進行更新和優(yōu)化，這個過程分為三個部分：只考慮運動的束調整（Motion-only Bundle Adjustment，簡稱 Motion-only BA）、局部束調整（Local BA）和全局束調整（Global BA）。

在 Motion-only BA 中，我們的目標是找到使得在當前時間步的相機姿態(tài)估計與地圖點的投影誤差最小化的最佳相機姿態(tài)。我們定義了一個成本函數(shù)，將所有相機在當前時間步觀察到的地圖點與其對應的特征點之間的重投影誤差加權求和，并利用迭代方法來最小化這個非線性成本函數(shù)。接著，我們介紹了我們系統(tǒng)中多相機的投影模型。我們使用了單目投影模型，而不是 ORB-SLAM2 中的矯正立體投影，以適應多相機系統(tǒng)。我們給出了單目投影的數(shù)學表示，并說明了它是如何用于姿態(tài)更新的。

在 Local BA 中優(yōu)化了一組共視的束關鍵幀和所有這些關鍵幀中觀察到的地圖點。為了防止變量收斂到零空間，我們使用了與 ORB-SLAM2 相同的策略。對于不屬于當前局部束調整的關鍵幀，但是觀察到了局部地圖點的其他關鍵幀，則會在優(yōu)化過程中保持不變。最后介紹了全局 BA，它對所有的束關鍵幀和地圖點進行了優(yōu)化，除了原點束關鍵幀。與局部 BA 類似，全局 BA 也使用了相同的優(yōu)化策略來最小化成本函數(shù)，以進一步提高系統(tǒng)的精度和魯棒性。

圖4：具有多個同步攝像頭的視覺里程計,目標是估計BundledFrame或BundledKeyframe中第一個相機Ck1在世界坐標的每一時刻的相對運動。

E.回環(huán)閉合

回環(huán)閉合包含三個關鍵步驟：回環(huán)檢測、相似性變換計算和回環(huán)融合。在 BundledSLAM 中，回環(huán)檢測依賴于數(shù)據(jù)庫查詢，采用基于 DBoW2 的詞袋模型地點識別模塊。利用從大量圖像數(shù)據(jù)集中提取的 ORB 描述符創(chuàng)建了一個視覺詞匯，以確保在具有相同詞匯的不同環(huán)境中獲得魯棒的性能。我們系統(tǒng)中的每個唯一特征描述子都被分配給詞匯表中的特定視覺詞。與 ORB-SLAM2 不同，BundledSLAM 根據(jù) BundledKeyframe 信息增量構建數(shù)據(jù)庫，包括一個反向索引。該索引跟蹤了每個視覺詞匯在詞匯表中被哪些 BundledKeyframes 觀察到。因此BundledKeyframe 數(shù)據(jù)庫為回環(huán)候選項提供了更廣泛的搜索范圍，比 ORB-SLAM2 實現(xiàn)的回環(huán)閉合更準確。此外在查詢識別數(shù)據(jù)庫時，計算當前 BundledKeyframe Bi 和其在共視圖中的所有鄰居的詞袋向量之間的相似性，并設置一個閾值得分 smin。BundledSLAM 在回環(huán)候選項檢測方面比 ORB-SLAM2 更為嚴格，因為它丟棄了所有得分低于 smin 的 BundledKeyframes，這種嚴格的方法提高了系統(tǒng)中回環(huán)閉合的精度。

04 ? 實驗

BundledSLAM 系統(tǒng)在配備有 Intel Core i7-8700（四核 @ 3.2GHz）處理器和16GB內存的 PC 上運行。為了評估我們系統(tǒng)的性能，使用了 EuRoc 數(shù)據(jù)集進行了評估，將系統(tǒng)的性能與當前最先進的 SLAM 系統(tǒng)，即 ORB-SLAM2 和 VINS-Stereo 進行了比較，以突出我們系統(tǒng)的準確性?？紤]到多線程系統(tǒng)的非確定性以及所涉及的固有隨機性，對每個序列執(zhí)行了五次。這種方法不僅可以展示最佳結果，還可以提供中位數(shù)結果，全面展示了在估計軌跡方面實現(xiàn)的準確性。

A. EuRoC數(shù)據(jù)集?

我們對IMU-PARSAC在手工場景和公共數(shù)據(jù)集ADVIO上進行了定性和定量評估。我們的手工場景包括靜態(tài)背景和前景的移動物體，有些物體偶爾會遮擋視野的大部分區(qū)域，以測試IMU-PARSAC的能力。圖7比較了兩種不同場景下幾種異常值移除方案的效果，包括傳統(tǒng)的魯棒估計器RANSAC、動態(tài)對象分布先驗估計器PARSAC以及我們提出的IMU-PARSAC。我們還比較了使用IMU預積分預測姿勢來消除視覺觀測中的異常值的能力。我們對當前幀中可見的2D點和地圖中的3D點執(zhí)行PnP幾何估計，以確定觀察到的2D點是否對應于移動物體。

B. 精度評估?

表I提供了BundledSLAM在所有序列中的絕對平移誤差（均方根誤差 - RMSE）的概述，與ORB-SLAM2和VINS-Stereo進行了比較。在將估計的軌跡與地面真實數(shù)據(jù)對齊后，計算了這些誤差。值得注意的是，我們在處理序列MH_05_difficult、V1_02_medium、V1_03_difficult、V2_01_easy和V2_02_medium時，為我們的系統(tǒng)和ORB-SLAM2都激活了具有全局束調整（BA）的回環(huán)閉合模塊。然而，在序列V2_03_difficult的某些部分由于高運動速度，兩種方法都遇到了困難。通過加入額外的傳感器，如慣性測量單元（IMU），這一挑戰(zhàn)有可能得到緩解。作為一個多相機系統(tǒng)，BundledSLAM在精度上表現(xiàn)出比最先進系統(tǒng)ORB-SLAM2更好的優(yōu)勢。值得注意的是，BundledSLAM和ORB-SLAM2在所有序列中始終優(yōu)于VINS-Stereo。每個序列的表現(xiàn)最佳方法已用粗體突出顯示。?

圖5：與ORB-SLAM2和VINS Stereo相比，EuRoC數(shù)據(jù)集中的軌跡。

圖5比較了ORB-SLAM2、VINS-Stereo和地面真實數(shù)據(jù)。顯而易見的是，我們的多相機系統(tǒng)在EuRoC數(shù)據(jù)集上測試時始終提供更準確的估計。此外，我們還對ORB-SLAM2、VINS-Stereo和我們的BundledSLAM系統(tǒng)之間的絕對姿態(tài)誤差（APE）進行了比較，結果見圖6。這些結果清楚地表明，我們提出的系統(tǒng)BundledSLAM始終優(yōu)于其他系統(tǒng)，在每個序列中都表現(xiàn)出最小的APE。

圖6：EuRoC數(shù)據(jù)集中絕對姿態(tài)誤差（APE）的比較。

05 ? 總結

本文介紹的BundledSLAM是一個旨在利用多相機功能的視覺SLAM系統(tǒng)。該系統(tǒng)將來自各種相機的數(shù)據(jù)集成到統(tǒng)一的“捆綁幀”結構中，從而實現(xiàn)了實時姿態(tài)跟蹤、用于姿態(tài)和地圖點優(yōu)化的局部建圖，以及閉環(huán)以確保全局一致性。我們使用EuRoC數(shù)據(jù)集進行的評估突顯了我們的系統(tǒng)始終優(yōu)于原始系統(tǒng)，展示了其在最佳結果和平均結果中的異常準確性。為了提高系統(tǒng)的魯棒性，特別是在運動模糊或紋理特征有限的情況下，我們打算探索傳感器融合，可能會引入慣性測量單元（IMU）。然而，我們意識到額外傳感器可能引入的計算復雜性。作為我們未來研究的一部分，我們將優(yōu)先考慮采用策略來減少這種復雜性，同時保持甚至提高系統(tǒng)性能。

審核編輯：黃飛

?