基于圖的SLAM點(diǎn)稀疏化方法

特征點(diǎn)法的VSLAM系統(tǒng)中，特征點(diǎn)并不是越多越好；相反， 少量 并且 質(zhì)量高 的點(diǎn)會讓這個(gè)SLAM系統(tǒng)位姿精度更高，且消耗更少的存儲資源和計(jì)算資源。本文基于上述情況，提出了一種在SLAM系統(tǒng)中實(shí)現(xiàn)地圖點(diǎn)稀疏化的有效的圖優(yōu)化方法，進(jìn)而減少一般特征點(diǎn)SLAM系統(tǒng)中的3D點(diǎn)和關(guān)鍵幀，減少了計(jì)算和存儲資源，同時(shí)提高了位姿估計(jì)的精度。

Abstract

當(dāng)將SLAM用于真實(shí)世界中實(shí)際的問題（例如自動駕駛、無人機(jī)和AR設(shè)備）上時(shí)，其內(nèi)存占用和計(jì)算成本是限制性能和應(yīng)用范圍的兩個(gè)主要的因素；在離散的特征點(diǎn)SLAM算法中，解決這個(gè)問題的一個(gè)有效的方式是通過local BA 和global BA刪除點(diǎn)進(jìn)而限制地圖點(diǎn)的規(guī)模；本項(xiàng)研究中提出了一種在SLAM系統(tǒng)中 實(shí)現(xiàn)地圖點(diǎn)稀疏化的有效的圖優(yōu)化方法，具體上作者將最大位姿可見性和最大空間多樣性問題表示成一個(gè)最小成本最大流量圖優(yōu)化問題。作者所提出的方法可以在現(xiàn)存的SLAM系統(tǒng)中作為額外的一個(gè)步驟，因此它可以用于傳統(tǒng) 和學(xué)習(xí)的SLAM系統(tǒng)；通過大量的實(shí)驗(yàn)評估證明所提出的方法用 大約1/3的地圖點(diǎn) 和 1/2的計(jì)算 實(shí)現(xiàn)了更精確的相機(jī)位姿。

Introduction

Motivation：基于特征點(diǎn)法的VSLAM系統(tǒng)具有較好的處理速度和計(jì)算效率從而達(dá)到實(shí)時(shí)的性能，但是VSLAM系統(tǒng)最大的缺陷之一是隨著地圖大小的增長，內(nèi)存大小和計(jì)算成本會呈二次增長，解決這個(gè)日益增長的資源需求有兩個(gè)方向：一個(gè)是高效地求解優(yōu)化問題[40]，比如許多算法都試圖利用地圖拓?fù)浠騿栴}結(jié)構(gòu)來降低計(jì)算成本；另一個(gè)是減小整個(gè)SLAM系統(tǒng)的大小，包括特征/幀選擇、關(guān)鍵幀/3D點(diǎn)抽取等，它們大多集中于減少數(shù)據(jù)量或計(jì)算成本，同時(shí)略微犧牲位姿精度；在現(xiàn)有的SLAM系統(tǒng)中，為了在保持位姿定位精度的同時(shí)減少地圖大小和計(jì)算成本，作者引入了一種有效的點(diǎn)稀疏化算法，該算法可以直接集成到任何特征點(diǎn)的視覺SLAM系統(tǒng)中。

Contribution：

提出了一個(gè) 相機(jī)位姿對 和 3D點(diǎn)最大的點(diǎn)可見性 的 圖表示 方法；

提出了一種新的cost用于最大化圖像空間上二維特征的空間多樣性；

提出了一種 基于最小代價(jià)最大流量（minimum-cost maximum-flow）點(diǎn)稀疏化算法來控制剩余點(diǎn)的數(shù)量；

提供了詳細(xì)的位姿準(zhǔn)確性、點(diǎn)減少和各種室內(nèi)/室外公共數(shù)據(jù)集的改進(jìn)速度比較；

據(jù)作者所知，本文的工作是第一次將關(guān)于特征和幀關(guān)系的多個(gè)屬性同時(shí)集成到稀疏化特征圖中，也是第一次為稀疏化地圖的本地化性能的維護(hù)提供了驗(yàn)證。

圖1 上方圖：ORB-SLAM2生成的地圖點(diǎn)；下方圖：稀疏化圖；在構(gòu)建地圖和在地圖上進(jìn)行定位時(shí)，估計(jì)位姿精度都有一定的提高；與上方圖相比，下方圖的地圖包含39%的點(diǎn)

Related Work

針對VSLAM優(yōu)化問題的快速計(jì)算和低計(jì)算量為目標(biāo)的問題空間縮減方法可以大致分為兩個(gè)部分的工作；

第一個(gè)部分的工作研究了基于圖優(yōu)化的快速位姿優(yōu)化方法。[35]和[36]通過使用因子下降法和非循環(huán)因子下降法減少KLD (Kullback-Leibler Divergence)的迭代優(yōu)化，提高了SLAM的準(zhǔn)確性和速度；[29]提出了節(jié)點(diǎn)選擇問題相對于在結(jié)果稀疏化中付出的代價(jià)；[22]通過邊緣化舊節(jié)點(diǎn)的稀疏化節(jié)點(diǎn)，同時(shí)保持剩余節(jié)點(diǎn)的所有信息，并給出一個(gè)標(biāo)準(zhǔn)化的最小化問題，以保持圖組合的稀疏性；[39]設(shè)計(jì)了一種動態(tài)變量重新排序的方法，為快速增量Cholesky分解在增量更新和批量更新之間做出決定減少了與逆排列相關(guān)的工作量，為增量SLAM算法提供了節(jié)省計(jì)算量的方法；[26]提出了一種啟發(fā)式方法，通過消除復(fù)雜性度量來彌合圖結(jié)構(gòu)及其計(jì)算之間的分析差距，從而減少批量優(yōu)化和增量優(yōu)化的計(jì)算量；[21]提出固定滯后方法，邊緣化SLAM問題中的變量，最小化圖稀疏化過程中的信息損失；[6]使用一種基于信息的方法和最小化問題的增量版本來高效地稀疏地標(biāo)和姿態(tài)的數(shù)量，而不影響估計(jì)軌跡的準(zhǔn)確性。

另一個(gè)部分的工作對SLAM中的圖幾何進(jìn)行了縮減，以最小的信息損失對特征、點(diǎn)、幀進(jìn)行了抽取。[2]提出了一種自適應(yīng)的非最大抑制(ANMS)方法，可以快速均勻地重新分割圖像中的關(guān)鍵點(diǎn)，該算法通過對搜索范圍的平方逼近來抑制無關(guān)點(diǎn)，降低了計(jì)算復(fù)雜度，并根據(jù)圖像維數(shù)初始化搜索范圍，收斂速度更快；[15]通過通過 SDC (suppression via disk covering)算法高效地選擇一個(gè)空間分布的關(guān)鍵點(diǎn)集，該算法基于逼近最近鄰和貪婪方法對關(guān)鍵點(diǎn)進(jìn)行聚類；[37]提出了一種通過SLAM系統(tǒng)跟蹤，利用連續(xù)幀之間的時(shí)間相關(guān)性和加權(quán)特征，通過多幀引用提取有用特征的策略。

Methods

作者在ORB-SLAM2上融合了本文所提出的方法；一旦通過n > 1幀的三角化估計(jì)的地圖點(diǎn)之間的連通性和相機(jī)姿勢被表示為一個(gè)具有流量和成本的圖結(jié)構(gòu)，作者提出了一個(gè)點(diǎn)稀疏化的圖表示的解決方案。

A. ORB-SLAM2 Overview

ORB-SLAM2中影響內(nèi)存和計(jì)算需求的關(guān)鍵因素之一是地圖點(diǎn)的數(shù)量和與地圖點(diǎn)相關(guān)的局部興趣點(diǎn)特征，隨著BA的增長，local和global BA的規(guī)模呈二次增長，這反過來又需要顯著增加的計(jì)算成本；所以主要研究方向有兩個(gè)：

（1）只提取相關(guān)的局部特征和點(diǎn)

（2）剔除這些對位姿優(yōu)化貢獻(xiàn)不大的特征或點(diǎn)；在提出的方法中，在提取到足夠的特征點(diǎn)和臨時(shí)生成地圖點(diǎn)之后，作者著重于抽取點(diǎn)來建立一個(gè)更簡化的BA問題，并且通過大量的實(shí)驗(yàn)驗(yàn)證了該方法的有效性。

B. Graph Representation for Point Sparsification

圖2 （a）四個(gè)關(guān)鍵幀(A、B、C和D)共享3D空間中的三個(gè)點(diǎn)的事例；（b）由(a)構(gòu)造二分圖；一共有4層頂點(diǎn):兩個(gè)特殊頂點(diǎn)，一組點(diǎn)頂點(diǎn)，一組幀對頂點(diǎn)；在每個(gè)邊的上面，為示例寫了容量/成本值

圖2顯示了要在 BA 中優(yōu)化的簡化局部地圖結(jié)構(gòu)的示例，這里作者建立一個(gè)有向流圖結(jié)構(gòu)來解釋幀和它們所看到的點(diǎn)之間的關(guān)系，該方法解決的核心問題是如何在local 和 global BA 問題中選擇結(jié)構(gòu)變化最小的點(diǎn)子集，它相當(dāng)于如何選擇這樣的點(diǎn)，使BA問題中的約束數(shù)量最大化，同時(shí)使點(diǎn)的數(shù)量最小化；此外，在調(diào)整每幀6DOF位姿時(shí)，圖像空間上的殘差構(gòu)成了每個(gè)相機(jī)位姿的誤差協(xié)方差矩陣，殘差均勻分布有助于使協(xié)方差得到很好的正則化，相反，如果所有的特征點(diǎn)都聚集在圖像空間的一個(gè)角落附近，姿態(tài)調(diào)整問題空間的邊緣都很陡峭，導(dǎo)致很難有效地解決；同樣，兩幀之間的幀baseline越寬，各點(diǎn)約束的雅可比矩陣值也就越相似。直觀上看，如果一個(gè)點(diǎn)的兩幀基線角度接近于零，深度估計(jì)會變得困難，點(diǎn)調(diào)整也會變得困難。

有以上三點(diǎn)觀察結(jié)果可以概括為選點(diǎn)的三個(gè)目標(biāo)：

最大點(diǎn)可見性:最大限度地共享一個(gè)點(diǎn)的幀數(shù)（選擇在多幀之間觀測到多次的點(diǎn)）；

最大空間多樣性:特征點(diǎn)在圖像空間上的分布是多樣化的（要求點(diǎn)分布均勻）；

最大幀baseline長度:使任意兩幀之間的相機(jī)中心距離最大化（這個(gè)是關(guān)鍵幀選擇）。

為了集成一個(gè)算法解決上述問題，作者提出一種基于有向圖表示的新方法，即節(jié)點(diǎn)對應(yīng)于點(diǎn)和位姿對；在此圖中，利用節(jié)點(diǎn)之間的成本和容量能力，將節(jié)點(diǎn)的實(shí)際點(diǎn)可見性、空間多樣性和基線轉(zhuǎn)化為最小成本最大流量的二部分圖。

1）Point Connectivity：首先考慮一個(gè)點(diǎn)和共享該點(diǎn)的特征點(diǎn)的frames之間的連通性，連通性高的點(diǎn)表示高可見性，具有魯棒的局部特征，這種高度可見的點(diǎn)容易被選擇，因?yàn)樗鼈冊谖蛔藞D上的多個(gè)位姿提供了強(qiáng)約束；將Vso 與點(diǎn)vpi 之間的邊的代價(jià)函數(shù)cc 定義為具有高連通性的vpi 的較低的代價(jià)值：

2）Spatial Diversity of Interest Points：ORB- slam2在ORB特征提取步驟中包含了空間均勻分布的興趣點(diǎn)選擇過程，但是興趣點(diǎn)的初始均勻分布并不能保證在下面的步驟(包括點(diǎn)稀疏化)中得到相似的興趣點(diǎn)分布；這里定義了邊緣e(vP, vF)的空間代價(jià)cs，以確保在點(diǎn)稀疏過程中特征分布保持甚至改善：

3）Frame Pair Baseline：這里考慮了每一幀對的基線距離；對幀對中觀測到的超過一定距離的點(diǎn)進(jìn)行優(yōu)化可以更可靠地進(jìn)行，并可能對補(bǔ)償關(guān)鍵幀之間累積的漂移誤差有用；基線代價(jià)cb 應(yīng)用于vF 和vsi 之間的邊：

C. Minimum Cost Maximum Flow Graph Optimization

上面 B. Graph Representation for Point Sparsification 將整個(gè)有向圖構(gòu)建出來了，本節(jié)描述如何計(jì)算這個(gè)有向圖。作者使用最小代價(jià)最大流算法[11]來解決上述圖問題去計(jì)算從 Vso 到 Vsi 的最大流量，進(jìn)而使總的代價(jià)最?。?/p>

其中 f(e) 是邊 e 上的流量；通過計(jì)算在容量約束下成本最小的最優(yōu)流量，能測量點(diǎn)和幀之間邊上的流量，與上述定義的三個(gè)理想條件的滿足程度有關(guān)；計(jì)算流量后，只取e邊( Vso, Vpi)上流量大于預(yù)先設(shè)定閾值θf的點(diǎn)pi；Goldberg算法[17]保證最差情況下的時(shí)間復(fù)雜度限制在:

其中n是頂點(diǎn)總數(shù)，m是邊總數(shù)，C是最大的輸入成本。

Experiment

實(shí)驗(yàn)部分先是給出了所提出的方法的實(shí)現(xiàn)細(xì)節(jié)，然后將該方法在各種數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)并對結(jié)果進(jìn)行分析。

A. Implementation Details

本文的系統(tǒng)建立在ORB-SLAM2上。具體的，本文的方法在ORB-SLAM2 的 local mapping部分每個(gè) local BA 之前執(zhí)行；作者使用兩種方式修改了ORB-SLAM2，首先是將現(xiàn)有的多線程處理改為單線程處理，目的如下：

（1）對總運(yùn)行時(shí)間的客觀評估

（2）確定效果評估

（3）由于處理延遲，在優(yōu)化步驟中禁用幀丟失；第二點(diǎn)是因?yàn)镺RB-SLAM2中確定關(guān)鍵幀的條件與來自local mapping線程的跟蹤點(diǎn)數(shù)量和 local mapping線程的狀態(tài)有關(guān)，因此當(dāng)應(yīng)用單線程處理或點(diǎn)稀疏化時(shí)，local BA執(zhí)行的數(shù)量會發(fā)生變化，所以這里作者使用基于平移和旋轉(zhuǎn)變化量的確定性關(guān)鍵幀插入準(zhǔn)則用來衡量所提出的點(diǎn)稀疏化的影響。

整個(gè)實(shí)驗(yàn)中，作者設(shè)置 θf 為邊緣容量的一半，同時(shí)使用Google Optimization Research Tools解決最小成本最大流圖問題。

B. Performance Evaluation

作者在EuRoC、TUM、ScanNet、ICL-NUIM和KITTI數(shù)據(jù)集上進(jìn)行了評估，使用RMS ATE度量位姿精度，通過計(jì)算估計(jì)位姿與gt位姿之間的絕對距離來表示估計(jì)軌跡的全局一致性；下面是在各個(gè)數(shù)據(jù)集上的實(shí)驗(yàn)情況及說明。

表1 在EuRoC序列上選擇點(diǎn)和關(guān)鍵幀的比例

圖3 EuRoC上的總運(yùn)行時(shí)間和RMS ATE與表I相關(guān)

表 1 和圖 3 為雙目模式下EuRoC數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果，參數(shù) M 代表；特征點(diǎn)減少的同時(shí)，關(guān)鍵幀也會減少，因?yàn)樗鼈冇羞B接的映射點(diǎn)的減少會導(dǎo)致連接低于閾值的關(guān)鍵幀自動退出，總體處理時(shí)間也大大減少，因?yàn)橥ㄟ^減少約束的數(shù)量，BA和跟蹤過程；圖 3 顯示了表I中實(shí)驗(yàn)的總運(yùn)行時(shí)間和RMS ATE的圖，結(jié)果顯示所提出的方法減少了 1/3 的運(yùn)行時(shí)間并且性能上幾乎保持甚至提高，ATE差值最多在4cm以內(nèi)；參數(shù)M不同 ，證明所提出的方法不平衡屬于一個(gè)位姿的約束成功地刪除了只對優(yōu)化有負(fù)面影響的點(diǎn)；

表2 通過多個(gè)不同的M值對所提出的方法進(jìn)行比較；序列依次為TUM，ICL-NUIM和ScanNet

作者將所提方法在TUM，ICL-NUIM和SCANNET數(shù)據(jù)集上也進(jìn)行了實(shí)驗(yàn)對比；因?yàn)榕cEuRoC數(shù)據(jù)集相比，空間比例尺不同，生成的地圖點(diǎn)更少，所以M的取值范圍不同；圖1是ScanNet在scene0000上保存的地圖點(diǎn)的可視化，以及根據(jù)估計(jì)軌跡得到的場景的三維重建環(huán)境。

表3 在雙目式下使用KITTI對室外環(huán)境的實(shí)驗(yàn)結(jié)果

作者使用KITTI數(shù)據(jù)集對室外環(huán)境進(jìn)行了驗(yàn)證，表 3 是M=100 和M=200的實(shí)驗(yàn)結(jié)果，與室內(nèi)環(huán)境不同的是，它在一個(gè)相對較小的空間尺度上采用了許多地圖點(diǎn)和多個(gè)連接，并有許多重新審視的點(diǎn)，關(guān)鍵幀丟失在室外環(huán)境中并不常見；時(shí)間的增加也不像在室內(nèi)那樣顯著；然而，當(dāng)?shù)貓D點(diǎn)選擇像室內(nèi)其他結(jié)果一樣保守執(zhí)行時(shí)，也可以觀察到性能的提高。

C. Comparisons

圖4 圖像上選定的關(guān)鍵點(diǎn)的可視化；從上到下，原來的方法，ANMS，作者的方法；右下圖顯示了關(guān)鍵點(diǎn)的覆蓋范圍和聚類情況

表4 與ANMS點(diǎn)選擇的比較包括原始結(jié)果；最低的RMS ATE用粗體表示

自適應(yīng)非最大抑制(ANMS)算法[2]通過高效計(jì)算選擇待均勻分布的圖像上檢測到的關(guān)鍵點(diǎn)，提高了SLAM和圖像配準(zhǔn)性能；表IV顯示了在點(diǎn)選擇比例最低的情況下應(yīng)用ANMS和根據(jù)ANMS產(chǎn)生的地圖點(diǎn)總數(shù)應(yīng)用作者的方法時(shí)的結(jié)果，作者的方法4例中有3例的結(jié)果優(yōu)于ANMS和原方法；圖4是通過ANMS和作者的方法對一幅圖像中選定的局部感興趣點(diǎn)的可視化。

D. Ablation Study

表5 在TUM數(shù)據(jù)集上以部分和全部成本降低姿態(tài)精度

該部分評估了三種代價(jià)的有效性；當(dāng)使用所有這三種成本時(shí)，最低的ATE是通過在大基線下更多地使用幀來實(shí)現(xiàn)的，同時(shí)最大限度地提高位姿連接和空間多樣性。

E. Localization Test

本節(jié)通過從相同場景中收集的兩組序列來檢驗(yàn)原始地圖和稀疏地圖的位姿定位精度；使用 EuRoC dataset的MH01 & MH02 和 ScanNet的scene0000 00 & scene0000 01 進(jìn)行測試；對MH01和scene0000 00分別采用原始和稀疏兩種模式構(gòu)建參考地圖，對MH02, scene0000 01進(jìn)行定位。

表6 定位精度評價(jià)

具體結(jié)果如上所示，MH01序列的稀疏化地圖相比原始地圖只包含了23.9%的點(diǎn)和34.4%的關(guān)鍵幀，同時(shí)計(jì)算時(shí)間和地圖大小有顯著性減小，但是位姿精度上升了；與原始地圖相比，scene0000只使用了38.6%的點(diǎn)和59.1%的關(guān)鍵幀，姿態(tài)誤差也減少了。

Conclusion

本文介紹了一種基于圖的SLAM點(diǎn)稀疏化方法，該方法在點(diǎn)稀疏化過程中同時(shí)實(shí)現(xiàn)了3個(gè)目標(biāo):最大點(diǎn)連通性（maximizing the point connectivity）、最大空間多樣性（maximizing the spatial diversity）和最大幀基線長度（ maximizing frame baseline length）；以O(shè)RBSLAM2為baseline，提出的方法在保持甚至提高局部映射過程中姿態(tài)跟蹤精度的同時(shí)，大大減小了地圖尺寸；提出的方法可以用于地圖創(chuàng)建后的后壓縮或global BA前的預(yù)處理，它通常適用于基于局部特征SLAM系統(tǒng)，包括多傳感器SLAM，并提供了有效的地圖抽取和加速，可用于其他具有計(jì)算挑戰(zhàn)性的環(huán)境，如可穿戴設(shè)備。

未來的研究方向包括邊緣圖優(yōu)化的快速優(yōu)化，以及考慮三維點(diǎn)的空間密度和二維特征多樣性。