VDO-SLAM ：一種動態目標感知的視覺SLAM系統

VDO-SLAM ：一種動態目標感知的視覺SLAM系統

（VDO-SLAM ：A Visual Dynamic Object-aware SLAM System）

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

代碼： https://github.com/halajun/vdo_slam

摘要：

場景剛性假設，也稱為靜態環境假設，經常被使用到SLAM算法中。如今大部分算法在復雜的動態環境下運行時，會考慮通過刪除移動物體或單獨跟蹤移動物體來簡化問題。這種強假設限制了自主移動機器人系統在涉及高度動態和非結構化環境等一系列重要現實場景中應用的發展。本文介紹了一種魯棒的動態目標感知SLAM系統（VDO-SLAM），該系統利用語義信息，可以在不具有目標形狀或運動模型先驗信息的條件下，對場景中的剛性物體進行運動估計。該方法將環境中的動態和靜態結構集成到一個統一的估計框架中，從而實現對於機器人相機和時空地圖的精確估計。我們提供了一種從運動目標的位姿變化中提取速度估計值的方法，來實現（機器人在）復雜動態環境下的導航功能。我們在大量的室內和室外真實數據集上演示了該系統的性能。結果顯示，與最先進的算法相比，我們的系統實現了持續且實質性的改進。本系統已經開放了源代碼。

關鍵詞：SLAM，動態場景，目標感知，速度，跟蹤，位姿變換

1.介紹

用於復雜環境下的高質量三維虛擬模型，在機器人、智能交通、監視、檢查、娛樂和電影制作、自然環境的勘探和監測等等一系列領域中的應用需求正在不斷增大。創建這些模型需要重復地感知環境，並使用例如同時定位和建圖(SLAM)或從運動中恢復結構(SfM)等估計技術將多種傳感器測量的結果融合成一致的表示方式。SLAM是一個在機器人領域成熟的技術，它可以同時估計機器人的狀態並構建其環境的模型。盡管存在許多精確和高效的解決方案，但目前的SLAM算法在高度動態的環境下依然很容易失效(Cadena et al. (2016))。SLAM中處理動態物體的傳統方法不是將任何與移動物體相關的傳感器數據作為異常值處理，並將其從估計過程中去除(Hahnel et al. (2002, 2003);Wolf and Sukhatme (2005); Zhao et al. (2008); Bescoset al. (2018))，就是使用傳統的多目標跟蹤方法檢測運動目標並單獨跟蹤它們(Wang et al. (2003); Miller and Campbell (2007); Rogerset al. (2010); Kundu et al. (2011))。前者排除了場景中動態物體的信息，只生成靜態地圖。后者的精度取決於相機的位姿估計，然而在復雜的動態環境中這種方法很容易失效，因為（在這種場景下）都不一定存在可靠的靜態結構。隨着越來越多的自主系統被應用到動態環境中，作為目前大多數開源SLAM算法和視覺里程計（VO）算法基礎的場景剛性假設（或者稱為靜態環境假設）不斷地遭受着挑戰。

一個典型的SLAM系統由處理來自傳感器原始數據的前端模塊，和將獲取的信息(原始的和隱式的高級信息)集成到概率估計框架中的后端模塊組成。簡單原語，例如特征的三維位置等，通常被用來描述環境（信息）。這主要是由於在SLAM估計問題中這些（特征）點很容易被檢測、跟蹤和集成。其他原語，如線條與平面 (de la Puente and Rodr´ıguez-Losada (2014);Kaess (2015); Henein et al. (2017); Hsiao et al. (2017))甚至是目標 (Mu et al. (2016); Salas-Moreno et al.(2013); Yang and Scherer (2019))都被考慮過用於表達更豐富的地圖信息。語義信息和目標分割可以為動態目標的識別提供重要的場景先驗信息 (Wang et al. (2007); G´alvez-L´opez et al. (2016))。最新的深度學習技術已經幾乎可以實時檢測和分割目標類別了(Girshick et al. (2018); He et al. (2017))。盡管基於視覺的目標檢測和分割技術在最近有了發展，但視覺SLAM社區還沒有充分利用這些信息(Nicholson et al. (2018))。（如果）要將這些信息合並到現有的幾何SLAM算法中，場景中每個目標的三維模型數據集必須是可用的(Salas-Moreno et al. (2013);G´alvez-L´opez et al. (2016)) 或者前端必須顯式地提供除了檢測和分割之外的目標位姿信息 (Milan et al. (2016); Byravan and Fox (2017);Wohlhart and Lepetit (2015)) （這些要求）給這個問題增加了一層復雜性。對三維模型的精確性要求嚴重限制了它的潛在應用領域，而就我們所知，多目標跟蹤和三維位姿估計仍然是一個具有挑戰性的技術。這顯然需要一種算法，可以利用當前深度學習算法強大的檢測和分割能力，而不依賴於額外的位姿估計或目標模型先驗。

在本文中，我們提出了VDO-SLAM，一種新穎的基於特征的雙目/RGB-D動態SLAM系統，利用基於圖像的語義信息來同時定位機器人，繪制靜態和動態結構，以及跟蹤場景中剛體的運動。總之，本論文的貢獻有:

• (提出)一種基於機器人位姿、靜態和動態三維點以及目標運動的估計的統一框架的動態場景模型。

• 本算法可以對動態目標的位姿變化\(SE（3）\)進行精確估計並估算出場景中目標的（移動）速度，效果優於目前最先進的算法。

• (提出)一種利用語義信息跟蹤運動目標的魯棒方法，能夠處理由於語義目標分割失敗而導致的間接遮擋問題。

• (提出)一個在復雜且有代表性的現實場景下能夠被演示證明的系統

具我們所知,這是第一個完整的能夠實現運動分割，動態目標追蹤，估算跟隨着靜態和動態結構相機的位姿，估算場景中的每一個剛體的完全SE(3)位姿變化，提取速度信息和可以在現實戶外場景中被演示證明的動態SLAM系統。我們證明了我們的算法在真實數據集中的性能，並展示了系統解決剛性目標運動估計的能力。本系統對目標運動和相機位姿估計的准確性都比當前最新算法在城市駕駛場景中（的結果）高出一個數量級的性能。

本文的其余部分結構如下，在下面的第2節中，我們將討論相關的工作。在第3節和第4節，我們會講解所提出的算法和系統。我們在第5節介紹實驗配置，以及結果和評估。我們將在第6部分進行總結並提出結論。

2.相關工作

為了可以自主地執行任務，一個機器人必須能夠對它所處的環境進行分析。在場景中建立機器人、靜止物體和移動物體之間的時空關系是（機器人）理解場景的基礎，同時定位、建圖和對移動物體進行跟蹤等問題是相互促進的 (Wang et al. (2007))。然而，在SLAM社區中，（研究人員認為）與靜止物體相關的信息被認為是積極的，而從移動物體提取到的信息則被視為會降低算法性能。最新的SLAM系統要么將從移動物體提取到的數據視為異常值 (Hahnel et al. (2002,2003); Wolf and Sukhatme (2005); Zhao et al. (2008);Bescos et al. (2018)) 要么使用多目標跟蹤技術單獨跟蹤它們 (Wang et al. (2003); Miller and Campbell(2007); Rogers et al. (2010); Kundu et al. (2011))很少有人會把靜態和動態物體的信息處理到一個框架中，來提高估計的准確性 (Bibby and Reid (2007); Judd et al. (2018);Yang and Scherer (2019)).

最早應用在動態環境中的SLAM系統之一是由Hahnel et al. (2003)提出的。他使用了最大期望(EM)算法來更新相對於靜態/動態物體的測量值概率估計，並在當它們對應於動態物體的估計值時將其刪去。Alcantarilla et al. (2012)引入了密集場景流方法用於動態目標檢測，並通過消除估計中對動態目標的“錯誤”測量，改進了定位和映射結果。 Tan et al. (2013)提出了在線關鍵幀更新方法，該方法能正確地檢測外觀和結構方面的變化特征，並在必要時丟棄它們。

Wang et al. (2007)開發了一種用於運動目標跟蹤(SLAMMOT）的SLAM理論。在他們最新版本的用於運動目標的檢測和跟蹤的SLAM算法中，估計問題被分解成兩個獨立的估計器(對於運動目標的和靜止物體的)， 並可以實時更新兩個過濾器。 Kunduet等人(Kundu et al. (2011)) 通過並行解決結構自運動(SfM)和運動目標跟蹤問題，將系統輸出統一為包含靜態和運動目標結構和軌跡的三維動態場景圖。Reddy等人(2015)利用了光流和深度計算語義運動分割。在使用語義約束改進三維重建算法之前，他們將靜態物體與運動物體分離並獨立重構。

Bibby and Reid’s SLAMIDE (Bibby and Reid (2007)使用了一種將可逆數據關聯（包括動態位姿）統一進單一SLAM框架的廣義EM算法來估計三維特征的狀態(靜止或動態)。Zou and Tan(2013)提出了一種多相機SLAM系統，該系統能夠跟蹤多個相機，並重建靜態背景和移動前景點的三維位置。他們的系統利用了運動目標上的點在同一時刻共享相關相機信息的想法，可以同時估計出所有的相機位姿。 Judd et al.(2018)通過應用多運動視覺測程(MVO)多模型擬合技術估計相機和場景中剛體的全部SE(3)運動。Yang and Scherer(2019)最近的一項工作提出了一種用於單圖像三維長方體檢測，以及用於靜態和動態環境的多視圖位姿SLAM方法。不過，他們的主要目標是相機位姿和目標檢測精度，沒有提供其目標位姿估計的精度評估。

在過去的二十年里，計算機視覺社區對物體運動分割和多目標跟蹤問題進行了大量的研究，這些都是動態場景估計（研究成果）的重要組成部分。當前最先進的運動分割算法試圖將基於仿射假設的方法(Vidal and Hartley(2004))和基於對極幾何的方法(Rao et al. (2010))結合成一個單一的框架，發揮出他們各自的優勢((Elhamifar and Vidal (2009); Lai et al. (2016))。在最近的一種方法中，Xuet al. (2018) 提出了一種仿射模型、一種單應性模型和一個基本矩陣聯合集成的多幀光譜聚類框架。(Rubino et al.， 2018)已經證明利用語義信息可以有助於處理退化運動和部分遮擋問題，提高運動分割精度。在過去的幾年里，多目標跟蹤算法已經從傳統的基於推理/濾波的算法(Khan et al. (2005); Zhou et al. (2015))轉變為數據驅動的（深度學習）方法(Milan et al. (2017); Kim et al. (2018))。目前最先進的STAM-MOT (Chu et al. (2017)) 應用空間和時間圖來處理跟蹤中的局部遮擋問題。為了找到目標的最優分配，該算法采用了跟蹤單一目標時常用的密集搜索策略。

3.方法

在本節中，我們將展示如何在基於點跟蹤的自由模型中建模剛體的運動。我們提出了一種因子圖來優化相機和目標運動的估計。

在我們系統的跟蹤組件中，如圖4所示，被選擇的用於估計相機位姿和目標運動的代價函數(見3.2節)與3D-2D重投影誤差相關，並在圖像上定義。由於噪聲在圖像平面上可以得到更好的特征，這就為相機定位提供了更准確的結果(Nist´er et al. (2004))。此外，基於這一誤差項，我們提出了一種新的公式來聯合優化光流以及相機位姿和目標運動，以確保對點的魯棒跟蹤(見3.2.3節)。在建圖模塊中，我們使用了三維誤差代價函數來保證三維結構和目標運動估計的最佳結果，如3.3節所述

3.1背景和符號

3.1.1坐標系

設⁰X_k,⁰L_k \(\epsilon\)\(SE(3)\)分別為在全局參考系0中k時刻的機器人/相機和目標的三維位姿，其中k\(\epsilon\)\(\Gamma\)為時間步長的集合。特別地，在我們的表示法中，大寫字母用來表示指標集。圖2中將這些位姿轉換顯示為實線。

3.1.2點

設⁰mⁱ_k為第i個三維點在k時刻的齊次坐標，⁰mⁱ=[mⁱ_x,mⁱ_y,mⁱ_z,1]^T\(\epsilon\)IE³,i\(\epsilon\)\(M\)（點集）.我們在機器人/相機框架中描述一個點的形式例如：^x_kmⁱ_k=⁰X^-1_k⁰mⁱ_k。

定義I_k為與相機在k時刻捕捉到的圖片左上角的參考系。通過投影函數\(\pi\)(.)得到齊次三維點^I_kPⁱ_k在坐標系I_k上的像素位置

其中，K為相機內參。

相機或目標運動都會產生光流，它可以由像素在圖像幀上的位移向量表示：

3.1.3目標和三維點的運動:

k-1到k時刻的目標運動可以由齊次變換表示：

圖2將這些運動轉換形式表示為虛線曲線。我們在其對應的目標坐標系中寫一個點

將時刻k的目標位姿由(3)代入，可得到:

對於剛性目標，（4）式可以表示為

(5)非常重要，因為它通過一個齊次變換把連續時間幀上運動的剛體上的同一個三維點聯系了起來。

則全局參照系中點的運動表示為:

方程(6)是我們的運動估計方法的核心，因為它以無模型（model-free）的方式通過駐留在目標上的點表示剛體位姿變化，而不需要在估計中包含目標的三維位姿作為隨機變量。第3.2.2節詳細介紹了如何根據上述方程估計剛體的位姿變化。方程中的⁰_k-1H_k \(\epsilon\)\(SE(3)\)表示全局參照系中目標（提取出的）點的運動。對於本論文的其余部分，為了便於閱讀，我們將這個量稱為目標的位姿變化或目標的運動。

3.2相機位姿和目標運動估計

3.2.1相機位姿估計

通過最小化重投影誤差來估計相機位姿X：

我們用李代數x_k \(\epsilon\)\(se(3)\)中的元素參數化SE(3)中的相機位姿:

通過對\(SE(3)\)進行李代數參數化，將(8)代入(7)，得到最小二乘代價的解為:

可以使用Levenberg-Marquardt（L-M）算法來求解（9）。

3.2.2 目標運動估計

與相機位姿估計類似，構造基於重投影誤差的代價函數可以求解目標運動。使用公式（6），目標的三維點通過重投影對應到圖像I_k中二維點的誤差項為:

通過最小化（誤差項）的方法可以找到最優解：

3.2.3光流聯合估計

相機位姿估計和目標運動估計都依賴於良好的圖像對應。由於遮擋、較大的相對運動和較大的相機-目標距離，對於運動目標上（提取出的）點的跟蹤非常具有挑戰性。為了保證對於點的跟蹤的魯棒性，本文提出的技術旨在結合運動估計和光流估計進行（全局）細分。

對於相機的位姿估計，使用(2)對(7)中的誤差項進行重新表述:

使用李代數參數化\(SE(3)\)中的元素，通過最小化代價函數可以得到最優解:

類似地，(11)中的目標運動代價函數可以結合光流進行細化：

3.3圖優化

該方法將動態SLAM作為一個圖優化問題來細化相機位姿和目標運動，並建立包含靜態和動態結構的全局一致地圖。我們將動態SLAM問題建模為一個因子圖，如圖3所示。因子圖公式具有很強的直觀性，它的優點是允許有效的實現批次處理 (Dellaert and Kaess (2006); Agarwal et al. (2012))和增量求解器(Kaess et al. (2011); Polok et al. (2013); Ila et al.(2017))

四種測量/觀測被整合到一個聯合優化問題中：三維點測量，視覺里程計測量，動態目標上點的運動的觀測和目標平滑運動的觀測。定義三維點的測量模型誤差為:

三維點測量因子如圖3中的白色圓圈所示。

定義視覺里程計模型誤差為:

里程計因子如圖3中橙色圓圈所示。

定義動態目標上點的運動模型誤差為:

被測剛體\(l\)上所有點的運動特征都由式(6)給出的相同位姿變換⁰_k-1H^l_k \(\epsilon\)\(SE(3)\)描述，對應因子是一個三元因子，如圖3中洋紅色圓圈所示，我們稱之為剛體上點的運動模型。

研究表明，在動態SLAM中，結合場景中目標運動的先驗知識是非常有價值的(Wang et al. (2007);Henein et al.(2020)).由於相機幀率和“控制相對較大目標(車輛)運動的物理定律的影響，並且為了防止它們的運動突然改變，我們引入平滑運動因素，用來最小化連續目標運動的變化，其中的誤差項可以定義為:

目標平滑運動因子用於最小化連續時間幀間目標運動的變化，如圖3中青色圓圈所示。

我們使用李代數參數化運動目標\(SE(3)\)中的元素：

設\(\theta\)=\(\theta\)_x\(\cup\)\(\theta\)_M \(\cup\)\(\theta\)_H代表圖中所有節點，利用\(SE(3)\)對X和H使用李代數參數化(將(8)代入(16)和(17)，將(20)代入(18)和(19)，得出最小二乘代價的解為:

系統采用Levenberg-Marquardt(L-M)方法求解(21)中的非線性最小二乘問題。

4 系統

在本節中，我們提出了一種新的目標感知動態SLAM系統，該系統可以穩健地估計相機和目標的運動，以及環境的靜態和動態結構。整個系統概述如圖4所示。該系統由圖像預處理、跟蹤和建圖三個主要部分組成。

系統的輸入是雙目圖像或RGB-D圖像。對於雙目圖像，首先采用 Yamaguchi et al. (2014)描述的雙目深度估計方法提取深度信息，生成深度地圖，所得數據被處理為RGB-D形式。

雖然本系統最初設計為RGB-D系統，但為了充分利用基於圖像的語義信息，我們采用了單目相機的單幅圖像深度估計來獲取深度信息。我們的“基於學習的單目”系統是單目的，因為系統只使用RGB圖像作為輸入，而估計問題是利用RGB- D數據的，圖像的深度是通過單幅圖像深度估計得到的。

4.1預處理

這個模塊需要滿足兩個挑戰問題。一是將靜態背景與（動態）目標實現魯棒地分離，二是保證對於動態目標的長期跟蹤。為了實現這一目標，我們利用了計算機視覺的最新技術，例如水平語義分割和密集光流估計，以確保高效的目標運動分割和魯棒的目標跟蹤要求。

4.1.1目標實例分割

使用實例級語義分割對場景中潛在的可移動位姿進行分割和識別。語義信息是區分靜止和運動目標點的重要先驗，例如建築物和道路總是靜態的，而汽車可以是靜態的，也可以是動態的。實例分割有助於將語義前景進一步划分為不同的實例mask，從而更容易跟蹤每個單獨的位姿。此外，分割mask提供了一個“精確”的目標邊界，以確保對目標上的點的魯棒跟蹤。

4.1.2光流估算

利用密集光流可以最大化跟蹤運動目標上點的數量。大多數運動目標只占圖像的一小部分。因此，使用稀疏特征匹配不能保證魯棒性和長期的特征跟蹤。我們的方法利用了密集光流，通過對語義mask內的所有點進行采樣來顯著增加（對於運動）目標（提取出的）點的數量。密集光流還可以通過分配給目標mask每個點的唯一標識符來持續地跟蹤多個位姿。在語義分割失敗的情況下，（DVO-SLAM）可以恢復目標mask;而使用稀疏特征匹配很難實現這種任務。

4.2 跟蹤

跟蹤組件包括兩個模塊；相機自身運動跟蹤包括特征檢測和相機位姿估計子模塊，目標運動跟蹤包括動態目標跟蹤和目標運動估計子模塊。

4.2.1特征檢測

為了實現快速的相機位姿估計，我們檢測一組稀疏的角點特征並利用光流對其進行跟蹤。在每一幀中，只有擬合相機運動估計的內點才被保存到地圖中，用於跟蹤下一幀的對應（點）。如果跟蹤到的內點的數量低於某一閾值，系統就會檢測並添加新的特征。在靜態背景下檢測到的稀疏特征中，圖像區域不包括被分割的位姿。

4.2.2相機位姿估計

對於所有檢測到的3D-2D對應靜態點，DVO-SLAM使用公式(13)可以計算得到相機位姿。為了保證估計的魯棒性，系統采用運動模型生成方法對數據進行初始化。具體說來，該方法基於重投影誤差生成了兩個模型，並對兩個模型的內值進行了比較。一個模型是通過計算相機之前的運動生成的，而另一個模型是通過使用RANSAC的P3P ((Ke and Roumeliotis (2017))算法計算新的運動變換生成的。系統會選擇產生較多內點的運動模型進行初始化操作。

4.2.3動態目標跟蹤

運動目標的跟蹤過程分為兩個步驟。第一步將分割的目標分為靜態目標和動態目標。第二步通過連續的幀來關聯動態目標。

• 實例級目標分割允許我們從背景中分離目標。雖然算法有能力對所有的分割目標估計其運動，動態目標的有效識別可以有助於降低系統的計算成本。這是基於場景流估計實現的。具體來說，在獲得相機位姿后，描述三維點在k-1到k幀之間運動的場景流向量可以被計算得到：

與光流法不同，場景流(理想情況下僅由場景移動引起)可以直接決定某些結構是否有過移動。理想情況下，對應於所有靜態三維點的場景流向量的大小應該為零。然而，在深度和匹配方面的噪聲或錯誤會使真實場景中的情況復雜化。為了穩健地處理這個問題，我們會對每個目標上的所有樣本點計算它們的場景流大小。如果場景中某些點的場景流數值大於預先設定的閾值，則認為該點是運動的。在本系統進行的所有實驗中，這個閾值都被設置為0.12。如果“動態”點的比例高於某一水平(所有點數的30%)，則該目標會被識別為是運動的，否則該目標為靜止的。如前文所述，（設置）閾值是用於識別一個位姿是否是動態的，為了（使結果）更加保守，系統可以靈活地將一個靜態位姿建模為動態位姿，並每個時間步長對其估計出一個（大小為）零的運動。然而，如果進行相反的處理，則會降低系統的性能。

• 實例級位姿分割只能提供單個圖像目標標簽，然后需要隨時間在幀之間跟蹤目標以及它們的運動模型。我們建議使用光流法來關聯幀之間的點標簽。一個點標簽與采樣點的唯一目標標識符相同。我們保持一個有限的跟蹤標簽集。當探測到更多運動的目標時，標簽集的元素數量就會增加。靜態位姿和背景使用\(l\)=0標記。

理想情況下，對於k幀中的每個被檢測位姿，其所有點的標簽都應該唯一地與k-1幀中的對應標簽對齊。但在實際應用中，這會受到噪聲、圖像邊界和遮擋的影響。為了克服這個問題，我們給所有的點都貼上了標簽，當這些點對應起來時就會使標簽的數目達到最大。對於一個動態目標而言，如果它前一幀中最頻繁的標簽為0，這意味着該目標（在這一幀才）開始移動，出現在場景的邊界上，或者從遮擋中重新出現。在這種情況下，該位姿會被分配一個新的跟蹤標簽。

4.2.4目標的運動估計

如上文所述，（運動）目標在場景中通常是小范圍出現的，很難得到足夠的稀疏特征來穩健地跟蹤和估計目標的運動。我們對所有位姿mask內的第三個點進行了采樣，並在圖像幀之間對它們進行跟蹤。與相機位姿估計相似，只有內點會被保存進地圖中並用於下一幀的跟蹤。當跟蹤的目標點減少到一定水平以下時，會對目標進行采樣並添加新的目標點。我們采用了4.2.2節中討論的方法來生成初始目標運動模型。

4.3地圖

在地圖模塊中，系統會構造並維護一個全局地圖。同時，系統會從全局地圖中提取出一個基於當前時間步長和前一個時間步長窗口的局部地圖。兩個地圖都是通過批量優化過程更新的。

4.3.1局部批量優化

我們的系統會維護和更新一個局部地圖。局部批量優化的目標是確保將精確的相機位姿估計提供給全局批量優化。相機位姿估計對目標運動估計的精度和算法的整體性能有很大的影響。局部地圖是使用一個固定大小的包含最后一幀信息的滑動窗口構建的。局部地圖會共享一些公共信息:這會造成不同窗口之間的重疊。我們選擇只在窗口大小范圍內局部優化相機位姿和靜態結構，因為局部優化動態結構不會給優化帶來任何好處，除非在窗口內假設存在硬約束(例如目標的恆定運動)。但是，如果需要的話，系統能夠在局部地圖中合並靜態和動態結構。當一個局部地圖以類似的方式構造時，因子圖優化將執行來細化局部地圖中的所有變量，然后將它們在全局地圖中進行更新。

4.3.2全局批量優化

跟蹤模塊的輸出和局部批量優化包含了相機位姿、目標運動和內點結構。它們被保存在一個全局地圖中，這個全局地圖是由之前的所有時間幀構建的，並且隨着每一個新的時間幀的加入不斷更新。在處理完所有輸入幀后，系統會基於全局地圖構造因子圖。為了有效地獲得時間約束，系統在因子圖中只添加被跟蹤超過3個實例的點。該圖被表述為如3.3節中描述的優化問題。優化結果將作為整個系統的輸出。

4.3.3從地圖到跟蹤:

維護地圖可以為跟蹤模塊估算當前狀態時提供歷史信息，如圖4所示，藍色箭頭從全局地圖指向系統跟蹤模塊的多個組件。利用最后一幀的內點可以跟蹤當前幀中的對應點並估計相機位姿和目標運動。最后的相機和目標運動也可能作為先驗模型來初始化4.2.2和4.2.4節中所述的當前估計問題。此外，在語義位姿分割失敗導致“間接遮擋”的情況下，通過利用之前分割的mask，（運動）目標（上提取出的）點可以幫助（系統實現）跨幀關聯語義mask，以確保對目標的魯棒跟蹤。

5 實驗

我們評估了DVO-SLAM在相機運動，目標運動和速度（檢測），以及目標跟蹤情況下的性能。對室內場景的牛津多運動數據集(Judd and Gammell (2019))和戶外場景的KITTI跟蹤數據集(Geiger et al.(2013))進行了評估。我們還將結果與兩個最新的方法，MVO（judd et al.(2018)）和CubeSLAM(Yang and Scherer(2019))進行對比，以證明VDO-SLAM的卓越性能。

5.1系統設置

我們采用基於學習的實例級位姿分割Mask R-CNN (He et al.(2017))來生成位姿分割mask。該方法的模型在COCO 數據集上訓練(Lin et al.(2014))，可以直接用於本工作中，不需要進行任何微調。對於密集光流，我們利用了最先進的方法PWC-Net (Sun et al.(2018))（進行測試）。該模型在FlyingChairs數據集上進行訓練(Mayer et al. (2016))，然后在Sintel上進行微調(Butler et al.(2012)和在KITTI數據集上進行訓練(Geiger et al.， 2012)。為了在我們系統的“單目”版本生成深度地圖，我們采用了一種基於學習的單目深度估計方法MonoDepth2 (Godard et al.(2019)).。在排除本文測試數據的情況下，對該模型進行了深度特征分割(Eigen et al.(2014))訓練。特征檢測使用FAST完成(Rosten and Drummond（2006））( Rublee et al. (2011)).

5.2誤差指標

我們使用位姿變化誤差來評估VDO-SLAM估計的SE(3)運動，即給定一個真值運動變換矩陣T和相應的估計運動矩陣\(\hat{T}\)（進行對比），其中T\(\epsilon\)SE(3)既可以描述為一個相機相對位姿也可以是一個目標運動。位姿變化誤差可以通過:E=\(\hat{T}\)^-1T求得。平移誤差E_t通過計算E的平移分量的L₂范數求得。旋轉誤差E_r通過計算E的旋轉分量的軸角表示的旋轉角度來計算。對於不同的相機時間步長和序列中不同的目標，我們分別計算相機位姿和目標運動的均方根誤差(RMSE)。利用目標位姿的真值，將慣性坐標系中的位姿變化轉換為目標坐標系中，我們可以得到目標固定坐標系中的位姿變化數值：

我們也評估了（運動）目標的速度誤差。利用位姿變化量並取其差值，即可得到該目標上一點在慣性坐標系中表示的線速度大小。

為了得到更可靠的測量結果，我們對在特定時間的一個（運動）目標上所有點取平均：

則估計速度與真實速度之間的速度誤差可以計算為:E_s=|\(\hat{v}\)|-|\(v\)|。對於在時間幀上跟蹤的不同位姿，我們也計算RMSE作為誤差指標。

5.3牛津多運動數據集

最新的牛津多運動數據集(Judd and Gammell(2019))包含了通過移動雙目或RGB-D相機傳感器在室內場景中觀察多個擺動箱或玩具車的視頻序列。通過Vicon運動捕捉系統我們可以獲取相機和運動目標的真實運動軌跡。我們只選擇了擺動箱SLAM序列進行評估，因為真實駕駛場景的結果是在KITTI數據集上進行評估的。表1顯示了DVO-SLAM與最先進的MVO(Judd et al. (2018))比較的結果。由於MVO是一個沒有全局優化（模塊）的視覺里程計系統，我們關閉了DVO-SLAM系統中的批量優化模塊，並生成結果來進行公平的對比。我們使用了5.2節中描述的誤差指標（進行評估）。

總體來說，我們的方法在10個誤差指標中有7個對於（數據集中）4個擺動箱的相機位姿估計和運動估計問題表現出了更高的精度。特別地，我們的方法在估計左上角和左下角的相機的位姿和擺動箱的運動時性能可以提高50%。當觀測目標的旋轉運動時，特別是觀測右上角和右下角的擺動箱時我們的系統會產生相對較高的誤差。有趣的是，我們提出的算法在這些情況下的表現不如Judd et al. (2018)，因為本算法不是為此類運動和場景設計的，而是為觀測相對較大的戶外目標的運動而設計的，例如城市駕駛場景。我們認為這是由於使用的光流算法不能很好地優化旋轉目標。其結果是對（旋轉目標上）點運動的估計能力較差，從而導致目標跟蹤性能的下降。即使在對於觀測旋轉目標的相關性能較差的情況下，密集光流運動估計在其他指標上的好處依然是顯而易見的。

圖5顯示了我們算法在牛津多運動數據集上的地圖輸出結果。擺動箱上的動態特征軌跡在視覺上與箱體的實際運動軌跡相對應。從圖5中左下角紫色框的搖擺運動中可以清楚地看出這一點。

5.4 KITTI跟蹤數據集

KITTI跟蹤數據集(Geiger et al.(2013))包含21個序列，其中包含關於相機和目標位姿的真值信息。在這些序列中，有些不包括在我們系統的評估要求中;因為它們不包含移動目標(只有靜態場景)或只包含非剛性目標的行人，這超出了本系統的能力范疇。

5.4.1相機和目標的運動

表2給出了DVO-SLAM在9個序列中對於相機和目標運動估計的結果，與CubeSLAM (Yang and Scherer(2019))給出了的5個序列的結果做對比。我們最初嘗試用我們的默認參數來評估CubeSLAM，但會產生很大的錯誤，因此我們只比較了由CubeSLAM的作者提供的序列結果。由於CubeSLAM是用於單目相機的，我們也計算了DVO-SLAM基於學習的單目版本(如第4節所述)的結果，來進行公平的對比。

總體來說，我們提出的RGB-D和基於學習的單目方法在相機和目標運動估計方面都獲得了很高的精度。與CubeSLAM相比，我們的RGB-D版本在（計算）相機運動方面的誤差更低，而我們基於學習的單目版本的誤差較高。然而，這兩個版本在目標運動估計中都獲得了一致的較低的誤差。特別的，如圖6所示，CubeSLAM的平移誤差和旋轉誤差均在3米和3度以上，極端情況下的誤差分別達到32米和5度。而在（使用）RGBD版本的情況下，我們的平移誤差在0.1-0.3米之間，旋轉誤差在0.2-1.5度之間，在（使用）單目學習版本的情況下，我們的平移誤差在0.1-0.3米之間，旋轉誤差在0.4-3度之間，這說明了我們的目標運動估計能力（相對於CubeSLAM）有了一個數量級的提高。

5.4.2目標跟蹤與速度

我們還演示了系統對於動態目標跟蹤的性能，並展示了對於目標速度估計的結果，這是自主駕駛應用的重要信息。圖7給出了在所有測試序列中，一些選中目標(跟蹤超過20幀)的跟蹤長度和速度（估計）的結果。我們的系統能夠跟蹤大多數目標在其出現的序列中超過80%的比例。此外，我們估計的目標速度始終與其真實速度接近。

5.5討論

除了在5.4節和5.3節的評估之外，我們還提供了詳細的實驗結果，以證明我們提出的系統中關鍵模塊的有效性。最后，我們將討論該系統的計算成本。

5.5.1對點的魯棒跟蹤

圖優化利用空間和時間信息細化相機位姿和目標運動，以及靜態和動態結構。這個過程需要在數量和質量上對正確的點進行魯棒的跟蹤。這是通過結合運動估計對光流估計進行細化實現的，如3.2.3節所述。通過對a的比較，證明了聯合優化的有效性，通過將只對(9)相機運動或(11)只對目標運動進行優化的基線方法和對運動和光流(結合)都進行優化的改進方法(使用(13)或(15))進行比較，可以顯示出聯合優化方法的高效性。表3顯示，聯合方法可以獲得更多的（保證系統可以）長期跟蹤的點。

利用聯合估計方法得到的跟蹤點，系統可以更好地估計相機位姿和目標運動。在之前展示的KITTI數據集的9個序列中，系統觀察到的平移和旋轉誤差都得到了大約15%到20%的改善。詳情見表4。

5.5.2對非直接遮擋的魯棒性

在某些情況下，由於直接或間接遮擋(光照變化等)，使用mask分割可能會失敗。通過在第4.3.3節中描述的mask傳播方法，我們提出的系統能夠處理由於間接遮擋引起的mask失效問題。圖9展示了DVO-SLAM跟蹤一輛白色貨車80幀的示例，其中掩模分割在33幀中失敗了。盡管目標分割失敗，我們的系統仍然能夠持續跟蹤貨車，在整個序列中估計得到速度誤差的平均值為2.64km/h。在序列的后半部分，由於部分遮擋和目標離相機越來越遠等的原因，系統測得的速度誤差變大。

5.5.3對目標運動的全局細化

初始目標運動估計(在系統的跟蹤部分)是獨立於幀之間的，因為它純粹與傳感器的測量能力有關。如圖10所示。藍色曲線描述了在KITTI跟蹤數據集的03序列中觀察了55幀的貨車的初始目標速度估計。如圖所示，速度估計不平滑，在序列的后半部分出現了較大的誤差。這主要是由於目標距離相機的距離增加導致，整個目標只占據了場景的一小部分。在這種情況下，使用傳感器單獨測量目標的運動估計變得具有挑戰性且十分容易出錯。因此，和在第3.3節討論的內容相同，我們制定了一個因子圖並對靜態和動態結構都進行了細化。圖10中的綠色曲線顯示的是全局細化后的目標速度估計結果，序列的前半部分曲線變得更加平滑，后半段（誤差）也得到了顯著的改善。

5.5.4計算分析

最后，我們提供DVO-SLAM系統的計算分析結果。實驗在一台16GB內存的Intel Core i7 2.6 GHz筆記本電腦上進行。對目標語義分割和密集光流計算時間取決於電腦的GPU性能和CNN模型的復雜度。許多最新的算法都可以（在上面進行）實時運行(Bolya et al. (2019);Hui et al. (2020))。在本文中，語義分割和光流結果都是離線生成並輸入到系統中。整個SLAM系統在CPU上用c++實現，使用修改后的g2o作為其后端 (K¨ummerleet al. (2011))。我們在表5中顯示了DVO-SLAM在兩個數據集上的計算時間。在局部批量優化中，窗口大小被設置為20幀，並有4幀的重疊。每個系統組件在所有幀和序列上的時間成本都是平均的。總的來說，我們提出的系統的跟蹤部分可以根據檢測到的運動物體的數量以5-8幀每秒的幀率運行，采用並行計算可以進一步改善性能。全局批量優化的運行時間很大程度上取決於場景中相機位姿數量(幀數)和目標密度(根據每幀觀察到的動態目標的數量計算得到)。

6 結論

在本文中，我們介紹了VDO-SLAM。這是一種新穎的基於動態特征的，基於場景中圖像語義信息的，不需要額外的目標位姿或幾何信息，就可以實現對動態目標同步定位、建圖和跟蹤的SLAM系統。該系統可以在室內和具有挑戰性的室外數據集中獲得全局一致的魯棒性和准確性的結果，並能在目標運動估計方面展示其卓越的性能。我們認為在目標運動估計中之所以能取得高精度結果是由於我們的系統是一個基於特征的系統。特征點(法)仍然是SLAM系統中最容易檢測，跟蹤和集成的(方法)，不需要前端提供任何關於目標模型的額外信息。

如何減少動態目標的SLAM計算復雜度是一個重要的課題。在長期應用，不同的技術可以被應用來限制圖像數量的增長(Strasdat et al. (2011); Ila et al.(2010)).事實上，總結/刪除很久之前觀測到的動態目標相關的地圖點，似乎是長期SLAM系統適應高度動態環境的自然（nature）處理手段。

文獻插圖

圖1。VDO-SLAM系統的結果。(上圖)一個完整的地圖，包括相機軌跡，靜態背景和移動目標的結構。(下圖)靜態背景上（運動）目標檢測得到的點，以及被估計的目標（移動）速度。黑色的圓圈代表靜態點，每個目標都用不同的顏色顯示。

圖2。符號和坐標系。實線曲線表示相機和目標在慣性坐標系下的位姿;虛線曲線表示了在目標固定坐標系中⁰X和⁰L各自的運動。實線表示慣性坐標系中的三維點，虛線表示相機坐標系中的三維點。（⁰X,⁰L分別為在全局參考系0中機器人/相機和目標的三維位姿）

圖3。帶有移動位姿的目標感知SLAM的因子圖。黑色方塊表示相機在不同時間步長的位姿，藍色方塊表示三個靜態點，紅色方塊表示一個運動目標(虛線框)在不同時間步長的相同動態點，綠色方塊表示目標在不同時間步長的位姿變化。為了便於可視化，這里只畫一個動態點，不過在估計時，被檢測動態位姿上的所有點都會被使用到。先驗一元因子以黑色圓圈表示，測程二值因子以橙色圓圈表示，點測量二值因子以白色圓圈表示，點運動三值因子以洋紅色圓圈表示,平滑運動二元因子以青色圓圈表示。

圖4。VDO -SLAM系統概述。首先對輸入圖像進行預處理，生成實例級位姿分割和密集光流。然后，對預處理后的圖像跟蹤靜態背景結構和動態位姿的特征。接着在全局批量優化中細化根據特征軌跡估計得到的相機位姿和目標運動，並根據每一幀更新局部地圖。該系統能夠輸出相機位姿，靜態結構，動態目標位姿的軌跡，並可以估計其隨時間的位姿變化情況。

表1。VDO-SLAM與MVO (Judd et al. (2018))在牛津多運動數據集中的swinging-4_unconstrained序列上對於相機和目標運動估計精度的比較結果。粗體的數字表示更好的結果。

圖5。我們的方法對牛津多運動數據集的定性結果。(左圖：)含有相機軌跡、靜態結構和動態點軌跡的3D地圖。(右圖：)靜態背景和運動目標上檢測得到的點。黑色點對應靜態點，每個目標上的特征以不同的顏色顯示。

圖6。VDO-SLAM與CubeSLAM (Yang and Scherer (2019))對於目標運動估計精度的比較結果。顏色豎條對應於左邊y軸的數值，表示對數尺度下的平移誤差。曲線表示旋轉誤差，在線性尺度上對應於右y軸。

表2。VDO-SLAM與CubeSLAM (Yang and Scherer (2019))在KITTI數據集的第九個序列上對於相機和目標運動估計精度的比較結果。粗體的數字表示更好的結果。

、

圖7。跟蹤性能和速度估計。由於空間的限制,VDO-SLAM對於一些選定的目標(跟蹤超過20幀)的跟蹤長度和速度的演示結果。顏色條表示運動目標軌跡的長度，與左y軸相對應。圓圈代表目標的移動速度，與右y軸相對應。GT表示真值, EST.表示估計值。

表3。在KITTI數據集的9個序列上跟蹤超過5幀的點的數量。粗體的數字表示更好的結果。粗體下划線的數字表示增加的數量級。

表4。KITTI數據集第九個序列上的（DVO-SLAM系統的）平均相機位姿和目標運動誤差。粗體的數字表示更好的結果。

圖8。系統輸出的圖例展示：含有相機位姿，靜態背景結構，和動態目標位姿軌跡的動態地圖。如圖是VDO-SLAM對KITTI序列的樣本結果。黑色點代表靜態背景，每個檢測到的（運動）目標都用不同的顏色顯示。左上角的圖片表示01序列，並對序列末尾結果進行了放大展示，右上角的圖片表示06序列，底部的圖片表示03序列。

圖9。當語義分割失敗時，系統跟蹤和速度估計的魯棒性展示。如圖是一個在00序列上對於白色貨車(地面真實平均速度20km/h)在VDO-SLAM下跟蹤和速度估計性能的展示例子。(上圖)藍色豎條代表成功的目標分割，綠色曲線代表目標的檢測速度誤差。(左下圖)對貨車語義分割失敗的圖例展示。(右下圖)VDO-SLAM在貨車（語義分割）之前的跟蹤特征結果

圖10。全局細化對目標運動估計的影響。在序列03上對沿直線前進的貨車的速度估計結果如圖所示。藍色線條表示初始估計速度，綠色線條表示全局細化后的估計速度，紅色為速度真值。

表5。在兩個數據集上的不同系統組件的運行時間。每個組件的運行時間是所有幀和序列的用時的平均值。特別的，目標運動估計時間統計需要對目標數量的取平均，局部批處理優化時間統計需要對幀數量的取平均。
歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、檢測分割識別、三維視覺、醫學影像、GAN、自動駕駛、計算攝影、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據研究方向邀請進入相關微信群。請勿在群內發送廣告，否則會請出群，謝謝理解~
投稿、合作也歡迎聯系：[e-mail]

長按關注計算機視覺life