Hierarchical Segment-based Optimization for SLAM

作者：Yuxin Tian, Yujie Wang, Ming Ouyang, Xuesong Shi

來(lái)源：IROS 2021

論文地址：https://arxiv.org/pdf/2111.04228.pdf

作者：chaochaoSEU|來(lái)源微信公眾號(hào)：3D視覺(jué)工坊

摘要： 本文提出了一種用于SLAM系統(tǒng)的分層分段優(yōu)化方法。首先，我們提出了一種可靠的軌跡分割方法，可用于提高后端優(yōu)化的效率。然后，我們首次提出了一種緩沖機(jī)制來(lái)提高分割的魯棒性。在優(yōu)化過(guò)程中，我們利用全局信息對(duì)誤差較大的幀進(jìn)行優(yōu)化，用插值代替優(yōu)化更新估計(jì)好的幀，根據(jù)每一幀的誤差分層分配計(jì)算量。與基準(zhǔn)上的對(duì)比實(shí)驗(yàn)表明，我們的方法大大提高了優(yōu)化效率，且?guī)缀鯖](méi)有精度下降，大大優(yōu)于現(xiàn)有的高效優(yōu)化方法。

1 引言

隨著各種相機(jī)的廣泛使用和三維計(jì)算機(jī)視覺(jué)的發(fā)展，SLAM和SfM系統(tǒng)在過(guò)去的幾十年中得到了廣泛的研究。為了提高建圖和定位的精度，使用后端算法優(yōu)化3D位姿信息是必不可少的環(huán)節(jié)。

Bundle Adjustment (BA) 是一種常用于 SfM 和 SLAM 系統(tǒng)的優(yōu)化方法。同時(shí)，位姿圖是 SLAM 系統(tǒng)中常用的另一種優(yōu)化方法。與同時(shí)優(yōu)化位姿和地標(biāo)（地標(biāo)表示特征點(diǎn)在世界坐標(biāo)中的 3D 位置）的 BA 不同，位姿圖僅優(yōu)化位姿。此外，BA 使用重投影誤差作為優(yōu)化目標(biāo)，而位姿圖使用相對(duì)位姿誤差。

然而，傳統(tǒng)的BA和位姿圖計(jì)算量大，迭代次數(shù)多，消耗大量計(jì)算資源和時(shí)間，阻礙了許多實(shí)時(shí)、大規(guī)模、復(fù)雜的多機(jī)器人應(yīng)用。為了提高系統(tǒng)的效率，許多論文對(duì)BA方法提出了各種改進(jìn)，而很少考慮對(duì)位姿圖的改進(jìn)。此外，與 SfM 相比，旨在提高 SLAM 系統(tǒng)效率的同時(shí)保持位姿估計(jì)精度的工作相對(duì)較少。

到目前為止，可以提高BA效率的方法主要有兩類，即Incremental BA和Hierarchical BA。Incremental BA 主要遵循增加當(dāng)前地圖的思想，通過(guò)增量合并新的測(cè)量來(lái)減少計(jì)算量。但是這種方法主要是為了提高SLAM系統(tǒng)中前端局部?jī)?yōu)化的效率，由于缺乏全局優(yōu)化，估計(jì)位姿容易漂移。

Hierarchical BA 通常采用將整個(gè)軌跡分成幾段的方法。然后在每個(gè)段內(nèi)和段之間使用不同的優(yōu)化方法來(lái)提高計(jì)算效率。許多論文從各個(gè)方面研究了分層 BA。然而，目前的方法有一些缺點(diǎn)。一些方法[1][2][3]在不降低計(jì)算復(fù)雜度的情況下減少了迭代次數(shù)，導(dǎo)致效率提升有限。其他方法 [4] [5] [6] [7] [8] 使用每個(gè)段內(nèi)的局部?jī)?yōu)化方法和不同段之間的對(duì)齊來(lái)代替全局優(yōu)化，但由于缺少全局誤差分布，系統(tǒng)精度不理想。還有一種方法[7]，主要關(guān)注不同段之間連接部分的誤差，而忽略了段內(nèi)的優(yōu)化。但是這種方法對(duì)分割的精度要求很高，而且容易出現(xiàn)段之間的連接不平滑的情況。

我們的工作遵循Hierarchical BA 的路線，并將思想擴(kuò)展到位姿圖優(yōu)化。在本文中，我們提出了一種適用于SLAM的基于分段的優(yōu)化方法。與傳統(tǒng)的以SfM為主的高效優(yōu)化方法不同，我們充分利用SLAM系統(tǒng)有序豐富的先驗(yàn)知識(shí)來(lái)提高優(yōu)化算法的效率。我們使用SLAM系統(tǒng)中跟蹤過(guò)程中產(chǎn)生的速度信息和重投影誤差信息進(jìn)行分割。這種方法試圖為段內(nèi)的位姿提供更準(zhǔn)確的結(jié)果，而為段間的位姿提供一個(gè)不太理想的結(jié)果。這樣，在進(jìn)行全局優(yōu)化時(shí)，會(huì)更多地關(guān)注精度較差的區(qū)域，因此我們可以在提高效率的同時(shí)獲得更好的優(yōu)化結(jié)果。

同時(shí)，我們?cè)O(shè)置了分割時(shí)的緩沖機(jī)制。我們添加多個(gè)單獨(dú)的幀作為兩個(gè)相鄰段之間的緩沖區(qū)。例如，圖 1 顯示了分割結(jié)果的示例。我們?cè)趫?zhí)行全局優(yōu)化時(shí)優(yōu)化緩沖區(qū)內(nèi)的所有幀。這種方法充分利用全局信息來(lái)減少不同段之間的誤差，以達(dá)到更高的精度結(jié)果。此外，緩沖區(qū)的使用還可以緩解分割精度的問(wèn)題。這樣，我們不再需要過(guò)多關(guān)注分裂點(diǎn)的準(zhǔn)確位置，而只需關(guān)注一個(gè)近似位置，可以有效提高魯棒性。

圖1 上圖分別為KITTI的00和05序列的分割結(jié)果。彩色區(qū)域代表段，黑色區(qū)域代表緩沖區(qū)。在下半部分，我們展示了不同高效優(yōu)化算法對(duì)每個(gè) KITTI 序列（每個(gè)點(diǎn)代表一個(gè)序列）的準(zhǔn)確率（RMSE）和效率（時(shí)間）性能。請(qǐng)注意，這些值除以軌跡的長(zhǎng)度。

最后，在進(jìn)行全局優(yōu)化時(shí)，對(duì)每個(gè)段的前幾幀和最后幾幀以及緩沖區(qū)中的幀進(jìn)行全局優(yōu)化。然后將每個(gè)段中參與全局優(yōu)化階段的那些幀用于插值以更新段內(nèi)的位姿信息。具體算法在第三節(jié)詳細(xì)描述。我們將該方法應(yīng)用于開(kāi)源SLAM數(shù)據(jù)集，包括KITTI [9]、TUM RGB-D [10]和EuRoC [11]，結(jié)果表明我們的方法在效率和準(zhǔn)確性方面明顯優(yōu)于現(xiàn)有方法，如圖1（C）。

總之，本文的貢獻(xiàn)包括：

? 我們提出了一種混合軌跡分割和緩沖的方法，可用于SLAM 系統(tǒng)的后端優(yōu)化，以提高計(jì)算效率，同時(shí)緩解對(duì)分割精度要求過(guò)高的問(wèn)題。

? 我們率先引入基于插值的方法來(lái)替代大量?jī)?yōu)化以有效計(jì)算內(nèi)段位姿，同時(shí)確保幾乎沒(méi)有精度損失。

? 我們?yōu)?SLAM 中的位姿圖和全局 BA 提出了一種基于分段的統(tǒng)一優(yōu)化方法，我們的系統(tǒng)在效率和準(zhǔn)確性方面都顯著優(yōu)于以前的方法。

II．相關(guān)工作

有多種方法旨在解決在光束調(diào)整等優(yōu)化階段由于幀數(shù)增加而導(dǎo)致的效率問(wèn)題，尤其是在大規(guī)模環(huán)境中。早期的工作[12]利用了BA中Hessian矩陣的稀疏模式，提出先使用Schur補(bǔ)計(jì)算相機(jī)位姿，然后使用后代替代地標(biāo)。它顯著降低了BA的復(fù)雜度，后來(lái)被引入到g2o等常見(jiàn)求解器中[ 13]。

一項(xiàng)研究 [14] [15] [16] [17] [18] [19] [20] 使用增量 BA 的方法，其中新幀增量合并到現(xiàn)有地圖。然而，系統(tǒng)的性能受到初始化差和累積錯(cuò)誤的影響，特別是對(duì)于長(zhǎng)序列。

此外，Hierarchical BA有增長(zhǎng)趨勢(shì)，它將優(yōu)化分為幾個(gè)階段，每個(gè)階段只優(yōu)化子節(jié)點(diǎn)組[21] [1] [2] [3] [4] [5] [6] ] [7]。選擇優(yōu)化節(jié)點(diǎn)的子組，最常用的方法是將軌跡劃分為若干段，其中每段內(nèi)的誤差較小，段間的誤差較大。這樣，我們可以從解決更容易和更快收斂的小子問(wèn)題開(kāi)始，然后設(shè)計(jì)不同的段合并策略將段綁定在一起。從而可以提高BA的整體效率，優(yōu)化結(jié)果更加穩(wěn)定。在[1]中，首先基于相似性度量對(duì)圖像進(jìn)行聚類，并進(jìn)行局部BA優(yōu)化簇內(nèi)的位姿，然后使用全局BA合并簇。[2] 在概念上是相似的，除了使用成對(duì)對(duì)齊的綁定片段。[3] [4] 嘗試對(duì)集群進(jìn)行并行計(jì)算，從而更好地提高效率。[6] [7] 在優(yōu)化期間以自適應(yīng)方式在迭代之間改變分區(qū)方式，這引入了額外的復(fù)雜性成本。但這些方法主要針對(duì)解決離線SfM問(wèn)題。

對(duì)于段劃分標(biāo)準(zhǔn)，co-visibility [5] [6] 經(jīng)常被用作標(biāo)準(zhǔn)。例如，[5] 為 SLAM 系統(tǒng)設(shè)計(jì)了一種混合方法，它利用空間和時(shí)間的共同可見(jiàn)性來(lái)確定當(dāng)前幀與附近幀的連接性，并相應(yīng)地分割軌跡。然而，共同可見(jiàn)性能否可靠地反映誤差度量是值得懷疑的。例如，在共可見(jiàn)度較大的地方，誤差不一定很小，因?yàn)槲覀儫o(wú)法保證地標(biāo)的 3D 位置的準(zhǔn)確性。[7]使用連續(xù)幀之間最陡下降方向的角度作為分割軌跡的標(biāo)準(zhǔn)，只考慮局部變化。此外，[7]聲稱雖然重投影誤差可以作為一種解決方案，但它可能會(huì)將軌跡分成太多段，因?yàn)榇笳`差往往分布在選擇分裂點(diǎn)的區(qū)域。然而，我們認(rèn)為這個(gè)問(wèn)題可以通過(guò)在段之間引入緩沖區(qū)來(lái)解決，緩沖區(qū)內(nèi)的所有幀都將被視為需要后續(xù)優(yōu)化的大錯(cuò)誤區(qū)域。除了重投影誤差，我們還使用速度信息來(lái)更好地識(shí)別分裂點(diǎn)，并且系統(tǒng)能夠可靠地運(yùn)行。

要將段合并在一起，自然要在每個(gè)段的邊緣使用幀進(jìn)行對(duì)齊或優(yōu)化。例如，[5] 使用局部 BA 來(lái)優(yōu)化段內(nèi)的位姿，并使用基于運(yùn)動(dòng)平均的對(duì)齊方法來(lái)計(jì)算段之間的相對(duì)變換并將它們合并在一起。由于僅估計(jì)成對(duì)變換，因此很容易導(dǎo)致累積誤差。因此，在提高 BA 過(guò)程的效率的同時(shí)，結(jié)果的準(zhǔn)確性受到顯著影響。相反，[7] 提出通過(guò)分配單個(gè) 7-DoF 相似變換將每個(gè)段視為剛體，并應(yīng)用全局 BA 進(jìn)行僅優(yōu)化重疊幀，從而避免漂移誤差。段內(nèi)幀的位姿通過(guò)優(yōu)化前后的相對(duì)位姿變化來(lái)更新。但是，對(duì)齊方式很容易造成段之間的不一致。為了解決這個(gè)問(wèn)題，劃分和優(yōu)化過(guò)程是分層進(jìn)行的，很繁瑣。此外，我們認(rèn)為在段的頭部和尾部分配兩個(gè) 7-DoF 相似變換可以輕松解決不一致問(wèn)題。段內(nèi)幀的位姿可以通過(guò)類似于[22]的插值方法獲得，系統(tǒng)的精度幾乎保持不變。

III.提出的方法

在目前的SLAM框架中，后端的優(yōu)化主要有兩部分，一是利用相機(jī)位姿信息的位姿圖優(yōu)化，二是利用重投影誤差的BA優(yōu)化。我們提出的分割方法可以有效提高兩個(gè)優(yōu)化模塊的計(jì)算效率。具體來(lái)說(shuō)，我們將我們的方法分為以下五個(gè)部分進(jìn)行詳細(xì)描述。

第 III-A 節(jié)展示了我們?nèi)绾问褂?SLAM 系統(tǒng)的先驗(yàn)信息進(jìn)行分割。第 III-B 節(jié)詳細(xì)描述了提議的緩沖機(jī)制。第 III-C 節(jié)和第 III-D 節(jié)描述了將所提出的方法分別納入位姿圖優(yōu)化和BA優(yōu)化模塊的方法。最后，第 III-E 節(jié)描述了全局優(yōu)化后段內(nèi)相機(jī)位姿的插值方法。我們方法的流程如圖2所示。

圖2.所提方法的流程。上半部分展示了現(xiàn)有 SLAM 系統(tǒng)的整體流程，下半部分展示了我們基于分段的方法對(duì)每個(gè)相應(yīng)流程的主要思想。

A.分割方法

為了提高系統(tǒng)的效率，全局優(yōu)化應(yīng)該主要優(yōu)化精度較低的部分，而較少關(guān)注精度較好的部分。因此，理想的幀分割應(yīng)該遵循：

? 對(duì)于每個(gè)片段，幀的姿態(tài)估計(jì)和片段內(nèi)的地標(biāo)相對(duì)準(zhǔn)確。

? 段之間連接部分的位姿估計(jì)有較大的誤差。

為了達(dá)到目的，我們分析了SLAM系統(tǒng)中位姿圖優(yōu)化（1）和BA優(yōu)化（2）的數(shù)學(xué)形式。

圖優(yōu)化中最好的分割點(diǎn)應(yīng)該在誤差較大的邊附近。根據(jù)（1），圖中的分割點(diǎn)應(yīng)該是位姿圖優(yōu)化中相鄰位姿變換估計(jì)較差的幀。并且根據(jù)（2），優(yōu)化圖中的段點(diǎn)應(yīng)該是BA整優(yōu)化中具有較大重投影誤差的幀。因此，分裂點(diǎn)應(yīng)該是重投影誤差大或相對(duì)位姿誤差大的幀。

在SLAM系統(tǒng)的跟蹤過(guò)程中，由于測(cè)量噪聲不可避免地會(huì)出現(xiàn)估計(jì)誤差，但這些誤差一般很小。然而，環(huán)境中的一些劇烈變化會(huì)導(dǎo)致位姿估計(jì)誤差的突然增加，例如碰撞、漂移或特征點(diǎn)檢測(cè)和匹配誤差。這些條件在 SLAM 系統(tǒng)中反映為速度的快速變化或增加的重投影誤差。分裂點(diǎn)的位置應(yīng)該反映這些意外的變化。

因此，我們以相鄰幀的位姿變換速度和每幀的重投影誤差作為分割的標(biāo)準(zhǔn)。等式（3）顯示了該準(zhǔn)則的數(shù)學(xué)形式。有關(guān)緩沖區(qū)和等式 (4) 的詳細(xì)信息將在第 III-B 節(jié)中討論。

與之前主要使用公共視圖地標(biāo)的數(shù)量作為分割標(biāo)準(zhǔn)的分割方法[2][5][6]相比，我們的方法與優(yōu)化方程的關(guān)系更緊密，更量化，與最終定位準(zhǔn)確性更相關(guān)。不同分割方法見(jiàn)表III(a)。

B. 緩沖機(jī)制

對(duì)于傳統(tǒng)的分割方法，兩個(gè)相鄰的段直接相連，如圖3（a）所示。因此，分割時(shí)需要仔細(xì)處理分裂點(diǎn)的具體位置，因?yàn)椴煌姆指顣?huì)產(chǎn)生不同的輸出，這會(huì)影響優(yōu)化器的魯棒性。另外，在SLAM系統(tǒng)中，通常會(huì)看到多個(gè)連續(xù)幀的位姿估計(jì)都不準(zhǔn)確。將這些幀視為一個(gè)片段是不合理的。

為了解決這個(gè)問(wèn)題，我們?cè)O(shè)計(jì)了一個(gè)緩沖機(jī)制。在劃分的兩個(gè)段之間，放置一個(gè)緩沖區(qū)，如圖3（b）所示。在切分過(guò)程中，首先使用A段的方法判斷當(dāng)前段是否要結(jié)束。然后將后面的幀放入緩沖區(qū)，直到速度變化和重投影誤差都恢復(fù)到一個(gè)相對(duì)穩(wěn)定的水平。最后，創(chuàng)建一個(gè)新段并重復(fù)上述過(guò)程。緩沖區(qū)選擇結(jié)束的判據(jù)如(4)所示。

圖3 緩沖機(jī)制示意圖。段由彩色幀組成，緩沖區(qū)由黑色幀組成。

同時(shí)，為了方便后續(xù)的優(yōu)化，我們將每個(gè)段按照segment中幀的順序分為head、tail、inside三個(gè)部分。在實(shí)驗(yàn)中，我們使用前兩幀作為頭部，后兩幀作為尾部。最終，SLAM 系統(tǒng)中的每一幀都屬于以下四類之一：頭部、尾部、內(nèi)部和緩沖區(qū)。分割結(jié)果的定義如圖 4 所示。

圖4. 分割結(jié)果的定義。紅框、藍(lán)框和綠框分別代表該段的頭部、內(nèi)部和尾部。每個(gè)段由這三個(gè)部分組成。緩沖區(qū)中的幀顯示為黑色。

緩沖機(jī)制有效緩解了傳統(tǒng)分割方法對(duì)分割精度的過(guò)高要求，提高了優(yōu)化器的魯棒性。緩沖機(jī)制的設(shè)計(jì)也可以靈活處理連續(xù)不準(zhǔn)確的位姿估計(jì)。同時(shí)，緩沖區(qū)的使用幾乎不會(huì)對(duì)系統(tǒng)產(chǎn)生額外的計(jì)算成本。與傳統(tǒng)的分割方法相比，它犧牲很少的效率來(lái)?yè)Q取魯棒性和準(zhǔn)確性的提高。

C. 位姿圖優(yōu)化

SLAM系統(tǒng)中的位姿圖優(yōu)化主要是利用幀的相對(duì)位姿變換來(lái)優(yōu)化。優(yōu)化圖中的節(jié)點(diǎn)代表幀在世界坐標(biāo)系中的位姿，節(jié)點(diǎn)之間的邊代表相對(duì)位姿變換。傳統(tǒng)位姿圖優(yōu)化中的優(yōu)化圖如圖5(a)所示。

圖5 位姿圖方法。實(shí)線方塊代表優(yōu)化圖中的位姿節(jié)點(diǎn)。虛線方塊代表需要通過(guò)插值更新的位姿節(jié)點(diǎn)。并且連接線表示在優(yōu)化過(guò)程中用作邊緣的相對(duì)位姿變換。

在我們的方法中，我們通過(guò)分割信息改變優(yōu)化方程以提高優(yōu)化效率。我們將整個(gè)優(yōu)化過(guò)程分為全局優(yōu)化和segment內(nèi)姿態(tài)更新兩個(gè)步驟。在接下來(lái)的部分中，我們遵循圖4所示的四類segmentation的定義。

在全局優(yōu)化階段，我們利用分割信息來(lái)減少位姿圖中的節(jié)點(diǎn)和邊的數(shù)量，從而減少優(yōu)化器的計(jì)算量。我們刪除段內(nèi)部的所有節(jié)點(diǎn)及其相關(guān)邊。但是為了在全局優(yōu)化時(shí)保證位姿圖的連通性，我們將段頭節(jié)點(diǎn)和段尾節(jié)點(diǎn)之間的邊連接起來(lái)，以近似刪除的邊。最后，需要優(yōu)化的位姿圖的節(jié)點(diǎn)和邊圖如圖5（b）所示。由于位姿圖中的節(jié)點(diǎn)和邊發(fā)生了變化，優(yōu)化方程也發(fā)生了變化，如（5）所示。通過(guò)減少參與優(yōu)化的節(jié)點(diǎn)和邊的數(shù)量，我們大大提高了優(yōu)化的速度。

經(jīng)過(guò)位姿圖優(yōu)化后，進(jìn)入segment內(nèi)位姿更新階段。從上一步中，我們只能得到head、tail和buffer中節(jié)點(diǎn)的優(yōu)化結(jié)果，因此我們需要使用另一種方法來(lái)獲取segment內(nèi)部幀的更新結(jié)果。對(duì)于每一段，我們固定頭部和尾部的幀，并使用更有效的插值而不是優(yōu)化來(lái)更新內(nèi)部的節(jié)點(diǎn)。插值方法將在第 III-E 節(jié)中詳細(xì)描述。這樣，SLAM系統(tǒng)中每一幀的位姿就有了最終的優(yōu)化結(jié)果。

D. BA優(yōu)化

SLAM系統(tǒng)中的BA優(yōu)化使用了landmarks位置和幀位姿之間的重投影誤差來(lái)優(yōu)化。優(yōu)化圖中的節(jié)點(diǎn)表示幀的位姿和地標(biāo)的 3D 坐標(biāo)，邊表示從地標(biāo)到幀的重投影誤差的估計(jì)。

與位姿圖優(yōu)化方法類似，整個(gè)優(yōu)化過(guò)程也分為兩個(gè)步驟：全局位姿優(yōu)化和段內(nèi)位姿更新。

在全局優(yōu)化階段，首先簡(jiǎn)化節(jié)點(diǎn)和邊。與位姿圖方法類似，我們保留位于頭部、尾部和緩沖區(qū)中的幀節(jié)點(diǎn)。此外，為了保證連通性，我們選擇了最少數(shù)量的互連幀作為每個(gè)段的頭部和尾部之間的連接幀。在實(shí)際操作中，我們從與頭部區(qū)域最后一幀的共視度大于30的幀中，按時(shí)間順序選取最后一幀加入到連接幀中。新的連接框架。重復(fù)上述過(guò)程，直到最后選擇的幀與尾部區(qū)域的第一幀的covisibility超過(guò)30。這樣，我們選擇最少的連接幀來(lái)保持用于束調(diào)整的幀的連通性。最后，我們執(zhí)行全局優(yōu)化的幀節(jié)點(diǎn)包括head、tail、buffer和連接幀中的幀。對(duì)于地標(biāo)節(jié)點(diǎn)，僅保留將邊連接到框架節(jié)點(diǎn)的節(jié)點(diǎn)。BA 的原點(diǎn)優(yōu)化圖如圖 6(a) 所示，我們提出的原點(diǎn)優(yōu)化圖如圖 6(b) 所示。隨著優(yōu)化圖中節(jié)點(diǎn)和邊的變化，優(yōu)化方程也發(fā)生變化，如式（6）所示。

圖6. BA方法。實(shí)線方塊代表優(yōu)化圖中的幀節(jié)點(diǎn)，虛線方塊代表需要通過(guò)插值更新的位姿節(jié)點(diǎn)。特別是，線段內(nèi)部的實(shí)線方塊代表連接框架。綠點(diǎn)代表地標(biāo)節(jié)點(diǎn)，連接線代表重投影誤差邊。

經(jīng)過(guò)全局BA優(yōu)化后，進(jìn)入segment內(nèi)位姿更新階段。與位姿圖模塊類似，對(duì)于每個(gè)段，我們固定頭部、尾部和連接幀中的幀。然后使用插值而不是優(yōu)化來(lái)更新段內(nèi)部的其他節(jié)點(diǎn)，然后對(duì)齊相對(duì)地標(biāo)位置。這樣，SLAM系統(tǒng)中的每一幀姿態(tài)和地標(biāo)坐標(biāo)都會(huì)得到最終的優(yōu)化結(jié)果。

E. 段內(nèi)插值

在本節(jié)中，我們將介紹插值方法。我們使用這種方法根據(jù)全局優(yōu)化階段后的優(yōu)化幀獲得未優(yōu)化幀的近似結(jié)果。

[22]表明，當(dāng)需要估計(jì)的幀兩側(cè)都有通過(guò)優(yōu)化得到的準(zhǔn)確位姿結(jié)果和幀之間位姿變換關(guān)系的準(zhǔn)確估計(jì)時(shí)，我們可以使用插值代替優(yōu)化來(lái)得到近似的優(yōu)化結(jié)果?？梢员苊夥敝氐挠?jì)算。因此，我們使用改進(jìn)的插值方法代替優(yōu)化方法來(lái)有效地計(jì)算結(jié)果。

在位姿圖優(yōu)化模塊中，段的頭部和尾部的幀將參與全局優(yōu)化并獲得更準(zhǔn)確的位姿結(jié)果。此外，根據(jù)A段的分割策略，該段內(nèi)幀間位姿變換的精度也比較高。我們使用頭部和尾部的幀位姿作為參考。而對(duì)于segment內(nèi)部不參與全局優(yōu)化的每一幀，我們利用當(dāng)前幀與參考結(jié)果的變換關(guān)系來(lái)完成插值。

插值算法的數(shù)學(xué)形式如(7)所示，其中R、t和s分別代表旋轉(zhuǎn)矩陣、平移向量和尺度。

我們使用相對(duì)速度作為插值因子。然后我們使用相對(duì)位姿變換分別根據(jù)頭部和尾部的結(jié)果計(jì)算當(dāng)前幀的旋轉(zhuǎn)矩陣。最后，使用球面線性插值獲得最終的旋轉(zhuǎn)結(jié)果。并且平移的插值結(jié)果也是用類似的方法計(jì)算出來(lái)的。

對(duì)于BA模塊，連接幀和段的頭尾部分的幀都將參與全局優(yōu)化。因此，用于插值的參考幀除了包含片段頭部和尾部的幀之外，還將包括連接的幀。其余的與位姿圖模塊中的相同。完成幀位姿的插值后，未參與BA的地標(biāo)將根據(jù)其與參考幀的位置關(guān)系進(jìn)行更新。

由于全局優(yōu)化的估計(jì)精度高，使用插值代替優(yōu)化可以獲得精度損失很小的結(jié)果，但可以節(jié)省大量時(shí)間。同時(shí)，與[7]等直接不對(duì)段內(nèi)誤差進(jìn)行優(yōu)化的方法相比，插值可以有效地消除誤差。我們的方法可以在效率和準(zhǔn)確性之間取得更好的平衡。

IV. 評(píng)估

在本節(jié)中，我們對(duì)不同的優(yōu)化算法進(jìn)行了實(shí)驗(yàn)，主要比較了方法的效率和準(zhǔn)確性。本文提出的分割方法主要適用于SLAM系統(tǒng)。因此，我們的實(shí)驗(yàn)也在 SLAM 數(shù)據(jù)集中進(jìn)行，包括 KITTI [9]、TUM RGB-D [10] 和 EuRoC [11]。

我們已經(jīng)基于開(kāi)源 SLAM 系統(tǒng) OpenVSLAM 實(shí)現(xiàn)了我們的優(yōu)化算法。因此，首先我們將我們的優(yōu)化方法與 OpenVSLAM [23] 中使用的傳統(tǒng)方法進(jìn)行比較，以驗(yàn)證我們的改進(jìn)。此外，我們還將我們的算法與其他高效優(yōu)化算法進(jìn)行了比較，以反映我們方法在效率和準(zhǔn)確性方面的性能。最后，我們進(jìn)行ablation實(shí)驗(yàn)以驗(yàn)證我們方法中每個(gè)模塊的效果。

A. 基線比較

在與基線的對(duì)比實(shí)驗(yàn)中，我們選擇 SLAM 數(shù)據(jù)集中有回環(huán)的序列進(jìn)行實(shí)驗(yàn)。我們分別記錄了SLAM系統(tǒng)在每個(gè)序列中檢測(cè)回環(huán)時(shí)位姿圖優(yōu)化和BA的優(yōu)化時(shí)間，以比較效率。此外，我們記錄每一幀的最終位姿估計(jì)結(jié)果，并計(jì)算軌跡估計(jì)RMSE作為精度的比較。

最終的實(shí)驗(yàn)結(jié)果如表I所示。與傳統(tǒng)的OpenVSLAM優(yōu)化算法相比，我們的方法在精度方面幾乎沒(méi)有損失，但效率大大提高。我們觀察到我們的方法在某些序列中的準(zhǔn)確性有意外的輕微提高。當(dāng)優(yōu)化結(jié)果陷入局部最優(yōu)時(shí)，可能會(huì)發(fā)生這種情況，因?yàn)閮?yōu)化中涉及的幀過(guò)多。需要對(duì)根本原因進(jìn)行更多調(diào)查。

B. 高效優(yōu)化實(shí)驗(yàn)

在高效優(yōu)化算法的對(duì)比實(shí)驗(yàn)中，使用的數(shù)據(jù)集是KITTI數(shù)據(jù)集的00-10序列。我們刪除了 01 序列，因?yàn)橐曈X(jué) SLAM 系統(tǒng)會(huì)在跟蹤階段丟失。在實(shí)驗(yàn)中，我們的方法與傳統(tǒng)的SLAM算法ORBSLAM2[24]，以及兩種最好的層次優(yōu)化算法ENFT SfM[7]和Hybrid BA[5]進(jìn)行了比較。

由于有適合SfM的優(yōu)化算法，為了保證實(shí)驗(yàn)的公平性，我們對(duì)SLAM系統(tǒng)中的后端優(yōu)化做了自適應(yīng)修改。SLAM系統(tǒng)中所有幀的跟蹤完成后，我們將再次執(zhí)行全局BA。我們記錄最終的全局BA時(shí)間作為效率的評(píng)估。同時(shí)，將最終優(yōu)化結(jié)果的絕對(duì)軌跡誤差（ATE）作為準(zhǔn)確度的評(píng)價(jià)。實(shí)驗(yàn)結(jié)果如表II所示。結(jié)果表明，我們的方法在基于段的優(yōu)化算法中在準(zhǔn)確性和效率方面都達(dá)到了最佳性能。

C. Ablation實(shí)驗(yàn)

為了驗(yàn)證我們提出的方法中每個(gè)模塊的效果，我們進(jìn)行了Ablation實(shí)驗(yàn)。實(shí)驗(yàn)方法類似于第 IV-B 節(jié)中的方法。在 KITTI 的 00-10 序列上執(zhí)行比較實(shí)驗(yàn)。記錄最終全局BA的效率和準(zhǔn)確性以供比較。結(jié)果示于表III中。

Ablation實(shí)驗(yàn)分為三個(gè)部分，分別用于驗(yàn)證我們的分割方法、緩沖機(jī)制和插值方法的效果。在第一個(gè)實(shí)驗(yàn)中，為了驗(yàn)證我們的分割方法的優(yōu)越性，我們將所提出的分割方法與幾種傳統(tǒng)的分割方法進(jìn)行了比較，包括共同視點(diǎn)的數(shù)量、重投影誤差、速度和定長(zhǎng)分割。經(jīng)驗(yàn)證，我們的分割方法在優(yōu)化的準(zhǔn)確性和效率方面都具有更好的性能。

然后為了驗(yàn)證緩沖區(qū)的效果，我們?nèi)サ袅司彌_區(qū)，改為段間直接連接，其他改進(jìn)保留。實(shí)驗(yàn)結(jié)果表明，刪除緩沖區(qū)后，優(yōu)化的準(zhǔn)確率和效率都有所下降。

最后，我們將我們的插值方法與使用局部 BA 和線性插值的方法進(jìn)行比較，以驗(yàn)證我們的插值方法的有效性。結(jié)果還表明我們的插值方法表現(xiàn)最好。

V 結(jié)論

在本文中，我們提出了一種基于分層分段的 SLAM 系統(tǒng)優(yōu)化方法。我們提出了一種可靠的軌跡分割方法，可用于提高后端優(yōu)化的效率。然后，我們首次提出了一種緩沖機(jī)制來(lái)提高分割的魯棒性。此外，我們使用全局信息來(lái)優(yōu)化估計(jì)不佳的幀，并使用插值代替優(yōu)化來(lái)更新估計(jì)好的幀。這種分層優(yōu)化方法可以在保證精度幾乎沒(méi)有損失的同時(shí)，大大提高效率，優(yōu)于以往的高效優(yōu)化方法。對(duì)比實(shí)驗(yàn)也驗(yàn)證了我們方法的有效性。然而，分層分割優(yōu)化仍有一定的改進(jìn)空間，包括使用更多的理論方法來(lái)代替當(dāng)前的分割方法或提出分割和優(yōu)化的緊耦合方法以獲得更好的結(jié)果。

備注：作者也是我們「3D視覺(jué)從入門到精通」特邀嘉賓：一個(gè)超干貨的3D視覺(jué)學(xué)習(xí)社區(qū)

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。

更多干貨

歡迎加入【3D視覺(jué)工坊】交流群，方向涉及3D視覺(jué)、計(jì)算機(jī)視覺(jué)、深度學(xué)習(xí)、vSLAM、激光SLAM、立體視覺(jué)、自動(dòng)駕駛、點(diǎn)云處理、三維重建、多視圖幾何、結(jié)構(gòu)光、多傳感器融合、VR/AR、學(xué)術(shù)交流、求職交流等。工坊致力于干貨輸出，為3D領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長(zhǎng)~

添加小助手微信：CV_LAB，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進(jìn)步。