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

作者：

Han Song， Cong Liu， Huafeng Dai

01 ? 摘要

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

02 ? 主要貢獻(xiàn)

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

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

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

03 ? 內(nèi)容概述

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

圖1:BundledSLAM的方案流程

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

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

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

B.BundledFrame、BundledKeyframe和BundledMap

圖3：BundledFrame 的總體概覽

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

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

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

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

姿態(tài)更新: 對(duì)于特定的時(shí)間步 k，通過(guò)測(cè)量來(lái)自所有攝像頭的數(shù)據(jù)，計(jì)算第一個(gè)攝像頭 Ck1 的姿態(tài)更新。然后，通過(guò)相對(duì)于第一個(gè)攝像頭的估計(jì)恒定轉(zhuǎn)換來(lái)更新其他攝像頭的姿態(tài)。這個(gè)相對(duì)轉(zhuǎn)換 Ti1 是固定的，因?yàn)樗遣豢捎^測(cè)的，避免了整個(gè)系統(tǒng)由于攝像頭和轉(zhuǎn)換的自由運(yùn)動(dòng)而導(dǎo)致的漂移。

D.多相機(jī)的運(yùn)動(dòng)估計(jì)和束調(diào)整

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

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

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

圖4：具有多個(gè)同步攝像頭的視覺(jué)里程計(jì),目標(biāo)是估計(jì)BundledFrame或BundledKeyframe中第一個(gè)相機(jī)Ck1在世界坐標(biāo)的每一時(shí)刻的相對(duì)運(yùn)動(dòng)。

E.回環(huán)閉合

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

04 ? 實(shí)驗(yàn)

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

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

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

B. 精度評(píng)估?

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

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

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

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

05 ? 總結(jié)

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

審核編輯：黃飛

?