據(jù)我所知，這是第一個完整實現(xiàn)運動分割、動態(tài)目標追蹤等的「開源」動態(tài)SLAM系統(tǒng)！
點擊上方“小白學視覺”，選擇加"星標"或“置頂”
重磅干貨，第一時間送達
今天給大家分享一篇最新文章，VDO-SLAM ：一種動態(tài)目標感知的視覺SLAM系統(tǒng)，原文名稱?VDO-SLAM ：A Visual Dynamic Object-aware SLAM System
論文地址：https://arxiv.org/pdf/2005.11052v2.pdf
代碼：https://github.com/halajun/vdo_slam
摘要：
場景剛性假設，也稱為靜態(tài)環(huán)境假設，經(jīng)常被使用到SLAM算法中。如今大部分算法在復雜的動態(tài)環(huán)境下運行時，會考慮通過刪除移動物體或單獨跟蹤移動物體來簡化問題。這種強假設限制了自主移動機器人系統(tǒng)在涉及高度動態(tài)和非結(jié)構(gòu)化環(huán)境等一系列重要現(xiàn)實場景中應用的發(fā)展。本文介紹了一種魯棒的動態(tài)目標感知SLAM系統(tǒng)（VDO-SLAM），該系統(tǒng)利用語義信息，可以在不具有目標形狀或運動模型先驗信息的條件下，對場景中的剛性物體進行運動估計。該方法將環(huán)境中的動態(tài)和靜態(tài)結(jié)構(gòu)集成到一個統(tǒng)一的估計框架中，從而實現(xiàn)對于機器人相機和時空地圖的精確估計。我們提供了一種從運動目標的位姿變化中提取速度估計值的方法，來實現(xiàn)（機器人在）復雜動態(tài)環(huán)境下的導航功能。我們在大量的室內(nèi)和室外真實數(shù)據(jù)集上演示了該系統(tǒng)的性能。結(jié)果顯示，與最先進的算法相比，我們的系統(tǒng)實現(xiàn)了持續(xù)且實質(zhì)性的改進。本系統(tǒng)已經(jīng)開放了源代碼。
關(guān)鍵詞：SLAM，動態(tài)場景，目標感知，速度，跟蹤，位姿變換
1.介紹
用于復雜環(huán)境下的高質(zhì)量三維虛擬模型，在機器人、智能交通、監(jiān)視、檢查、娛樂和電影制作、自然環(huán)境的勘探和監(jiān)測等等一系列領(lǐng)域中的應用需求正在不斷增大。創(chuàng)建這些模型需要重復地感知環(huán)境，并使用例如同時定位和建圖(SLAM)或從運動中恢復結(jié)構(gòu)(SfM)等估計技術(shù)將多種傳感器測量的結(jié)果融合成一致的表示方式。SLAM是一個在機器人領(lǐng)域成熟的技術(shù)，它可以同時估計機器人的狀態(tài)并構(gòu)建其環(huán)境的模型。盡管存在許多精確和高效的解決方案，但目前的SLAM算法在高度動態(tài)的環(huán)境下依然很容易失效(Cadena et al. (2016))。SLAM中處理動態(tài)物體的傳統(tǒng)方法不是將任何與移動物體相關(guān)的傳感器數(shù)據(jù)作為異常值處理，并將其從估計過程中去除(Hahnel et al. (2002, 2003);Wolf and Sukhatme (2005); Zhao et al. (2008); Bescoset al. (2018))，就是使用傳統(tǒng)的多目標跟蹤方法檢測運動目標并單獨跟蹤它們(Wang et al. (2003); Miller and Campbell (2007); Rogerset al. (2010); Kundu et al. (2011))。前者排除了場景中動態(tài)物體的信息，只生成靜態(tài)地圖。后者的精度取決于相機的位姿估計，然而在復雜的動態(tài)環(huán)境中這種方法很容易失效，因為（在這種場景下）都不一定存在可靠的靜態(tài)結(jié)構(gòu)。隨著越來越多的自主系統(tǒng)被應用到動態(tài)環(huán)境中，作為目前大多數(shù)開源SLAM算法和視覺里程計（VO）算法基礎的場景剛性假設（或者稱為靜態(tài)環(huán)境假設）不斷地遭受著挑戰(zhàn)。
一個典型的SLAM系統(tǒng)由處理來自傳感器原始數(shù)據(jù)的前端模塊，和將獲取的信息(原始的和隱式的高級信息)集成到概率估計框架中的后端模塊組成。簡單原語，例如特征的三維位置等，通常被用來描述環(huán)境（信息）。這主要是由于在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))都被考慮過用于表達更豐富的地圖信息。語義信息和目標分割可以為動態(tài)目標的識別提供重要的場景先驗信息 (Wang et al. (2007); G′alvez-L′opez et al. (2016))。最新的深度學習技術(shù)已經(jīng)幾乎可以實時檢測和分割目標類別了(Girshick et al. (2018); He et al. (2017))。盡管基于視覺的目標檢測和分割技術(shù)在最近有了發(fā)展，但視覺SLAM社區(qū)還沒有充分利用這些信息(Nicholson et al. (2018))。（如果）要將這些信息合并到現(xiàn)有的幾何SLAM算法中，場景中每個目標的三維模型數(shù)據(jù)集必須是可用的(Salas-Moreno et al. (2013);G′alvez-L′opez et al. (2016)) 或者前端必須顯式地提供除了檢測和分割之外的目標位姿信息 (Milan et al. (2016); Byravan and Fox (2017);Wohlhart and Lepetit (2015)) （這些要求）給這個問題增加了一層復雜性。對三維模型的精確性要求嚴重限制了它的潛在應用領(lǐng)域，而就我們所知，多目標跟蹤和三維位姿估計仍然是一個具有挑戰(zhàn)性的技術(shù)。這顯然需要一種算法，可以利用當前深度學習算法強大的檢測和分割能力，而不依賴于額外的位姿估計或目標模型先驗。
在本文中，我們提出了VDO-SLAM，一種新穎的基于特征的雙目/RGB-D動態(tài)SLAM系統(tǒng)，利用基于圖像的語義信息來同時定位機器人，繪制靜態(tài)和動態(tài)結(jié)構(gòu)，以及跟蹤場景中剛體的運動?？傊?，本論文的貢獻有:
(提出)一種基于機器人位姿、靜態(tài)和動態(tài)三維點以及目標運動的估計的統(tǒng)一框架的動態(tài)場景模型。
本算法可以對動態(tài)目標的位姿變化進行精確估計并估算出場景中目標的（移動）速度，效果優(yōu)于目前最先進的算法。
(提出)一種利用語義信息跟蹤運動目標的魯棒方法，能夠處理由于語義目標分割失敗而導致的間接遮擋問題。
(提出)一個在復雜且有代表性的現(xiàn)實場景下能夠被演示證明的系統(tǒng)
據(jù)我們所知,這是第一個完整的能夠實現(xiàn)運動分割，動態(tài)目標追蹤，估算跟隨著靜態(tài)和動態(tài)結(jié)構(gòu)相機的位姿，估算場景中的每一個剛體的完全SE(3)位姿變化，提取速度信息和可以在現(xiàn)實戶外場景中被演示證明的動態(tài)SLAM系統(tǒng)。我們證明了我們的算法在真實數(shù)據(jù)集中的性能，并展示了系統(tǒng)解決剛性目標運動估計的能力。本系統(tǒng)對目標運動和相機位姿估計的準確性都比當前最新算法在城市駕駛場景中（的結(jié)果）高出一個數(shù)量級的性能。
本文的其余部分結(jié)構(gòu)如下，在下面的第2節(jié)中，我們將討論相關(guān)的工作。在第3節(jié)和第4節(jié)，我們會講解所提出的算法和系統(tǒng)。我們在第5節(jié)介紹實驗配置，以及結(jié)果和評估。我們將在第6部分進行總結(jié)并提出結(jié)論。
2.相關(guān)工作
為了可以自主地執(zhí)行任務，一個機器人必須能夠?qū)λ幍沫h(huán)境進行分析。在場景中建立機器人、靜止物體和移動物體之間的時空關(guān)系是（機器人）理解場景的基礎，同時定位、建圖和對移動物體進行跟蹤等問題是相互促進的 (Wang et al. (2007))。然而，在SLAM社區(qū)中，（研究人員認為）與靜止物體相關(guān)的信息被認為是積極的，而從移動物體提取到的信息則被視為會降低算法性能。最新的SLAM系統(tǒng)要么將從移動物體提取到的數(shù)據(jù)視為異常值 (Hahnel et al. (2002,2003); Wolf and Sukhatme (2005); Zhao et al. (2008);Bescos et al. (2018)) 要么使用多目標跟蹤技術(shù)單獨跟蹤它們 (Wang et al. (2003); Miller and Campbell(2007); Rogers et al. (2010); Kundu et al. (2011))很少有人會把靜態(tài)和動態(tài)物體的信息處理到一個框架中，來提高估計的準確性 (Bibby and Reid (2007); Judd et al. (2018);Yang and Scherer (2019)).
最早應用在動態(tài)環(huán)境中的SLAM系統(tǒng)之一是由Hahnel et al. (2003)提出的。他使用了最大期望(EM)算法來更新相對于靜態(tài)/動態(tài)物體的測量值概率估計，并在當它們對應于動態(tài)物體的估計值時將其刪去。Alcantarilla et al. (2012)引入了密集場景流方法用于動態(tài)目標檢測，并通過消除估計中對動態(tài)目標的“錯誤”測量，改進了定位和映射結(jié)果。Tan et al. (2013)提出了在線關(guān)鍵幀更新方法，該方法能正確地檢測外觀和結(jié)構(gòu)方面的變化特征，并在必要時丟棄它們。
Wang et al. (2007)開發(fā)了一種用于運動目標跟蹤(SLAMMOT）的SLAM理論。在他們最新版本的用于運動目標的檢測和跟蹤的SLAM算法中，估計問題被分解成兩個獨立的估計器(對于運動目標的和靜止物體的)， 并可以實時更新兩個過濾器。Kunduet等人(Kundu et al. (2011)) 通過并行解決結(jié)構(gòu)自運動(SfM)和運動目標跟蹤問題，將系統(tǒng)輸出統(tǒng)一為包含靜態(tài)和運動目標結(jié)構(gòu)和軌跡的三維動態(tài)場景圖。Reddy等人(2015)利用了光流和深度計算語義運動分割。在使用語義約束改進三維重建算法之前，他們將靜態(tài)物體與運動物體分離并獨立重構(gòu)。
Bibby and Reid’s SLAMIDE (Bibby and Reid (2007)使用了一種將可逆數(shù)據(jù)關(guān)聯(lián)（包括動態(tài)位姿）統(tǒng)一進單一SLAM框架的廣義EM算法來估計三維特征的狀態(tài)(靜止或動態(tài))。Zou and Tan(2013)提出了一種多相機SLAM系統(tǒng)，該系統(tǒng)能夠跟蹤多個相機，并重建靜態(tài)背景和移動前景點的三維位置。他們的系統(tǒng)利用了運動目標上的點在同一時刻共享相關(guān)相機信息的想法，可以同時估計出所有的相機位姿。Judd et al.(2018)通過應用多運動視覺測程(MVO)多模型擬合技術(shù)估計相機和場景中剛體的全部SE(3)運動。Yang and Scherer(2019)最近的一項工作提出了一種用于單圖像三維長方體檢測，以及用于靜態(tài)和動態(tài)環(huán)境的多視圖位姿SLAM方法。不過，他們的主要目標是相機位姿和目標檢測精度，沒有提供其目標位姿估計的精度評估。
在過去的二十年里，計算機視覺社區(qū)對物體運動分割和多目標跟蹤問題進行了大量的研究，這些都是動態(tài)場景估計（研究成果）的重要組成部分。當前最先進的運動分割算法試圖將基于仿射假設的方法(Vidal and Hartley(2004))和基于對極幾何的方法(Rao et al. (2010))結(jié)合成一個單一的框架，發(fā)揮出他們各自的優(yōu)勢((Elhamifar and Vidal (2009); Lai et al. (2016))。在最近的一種方法中，Xuet al. (2018) 提出了一種仿射模型、一種單應性模型和一個基本矩陣聯(lián)合集成的多幀光譜聚類框架。(Rubino et al.， 2018)已經(jīng)證明利用語義信息可以有助于處理退化運動和部分遮擋問題，提高運動分割精度。在過去的幾年里，多目標跟蹤算法已經(jīng)從傳統(tǒng)的基于推理/濾波的算法(Khan et al. (2005); Zhou et al. (2015))轉(zhuǎn)變?yōu)閿?shù)據(jù)驅(qū)動的（深度學習）方法(Milan et al. (2017); Kim et al. (2018))。目前最先進的STAM-MOT ?(Chu et al. (2017)) 應用空間和時間圖來處理跟蹤中的局部遮擋問題。為了找到目標的最優(yōu)分配，該算法采用了跟蹤單一目標時常用的密集搜索策略。
3.方法
在本節(jié)中，我們將展示如何在基于點跟蹤的自由模型中建模剛體的運動。我們提出了一種因子圖來優(yōu)化相機和目標運動的估計。
在我們系統(tǒng)的跟蹤組件中，如圖4所示，被選擇的用于估計相機位姿和目標運動的代價函數(shù)(見3.2節(jié))與3D-2D重投影誤差相關(guān)，并在圖像上定義。由于噪聲在圖像平面上可以得到更好的特征，這就為相機定位提供了更準確的結(jié)果(Nist′er et al. (2004))。此外，基于這一誤差項，我們提出了一種新的公式來聯(lián)合優(yōu)化光流以及相機位姿和目標運動，以確保對點的魯棒跟蹤(見3.2.3節(jié))。在建圖模塊中，我們使用了三維誤差代價函數(shù)來保證三維結(jié)構(gòu)和目標運動估計的最佳結(jié)果，如3.3節(jié)所述
3.1背景和符號
3.1.1坐標系
設0Xk,0Lk 分別為在全局參考系0中k時刻的機器人/相機和目標的三維位姿，其中k為時間步長的集合。特別地，在我們的表示法中，大寫字母用來表示指標集。圖2中將這些位姿轉(zhuǎn)換顯示為實線。
3.1.2點
設0mik為第i個三維點在k時刻的齊次坐標，0mi=[mix,miy,miz,1]TIE3,i（點集）.我們在機器人/相機框架中描述一個點的形式例如：xkmik=0X-1k0mik。
定義Ik為與相機在k時刻捕捉到的圖片左上角的參考系。通過投影函數(shù)(.)得到齊次三維點IkPik在坐標系Ik上的像素位置:
其中，K為相機內(nèi)參。
相機或目標運動都會產(chǎn)生光流，它可以由像素在圖像幀上的位移向量表示：
3.1.3目標和三維點的運動:
k-1到k時刻的目標運動可以由齊次變換表示：
圖2將這些運動轉(zhuǎn)換形式表示為虛線曲線。我們在其對應的目標坐標系中寫一個點。
對于剛性目標，（4）式可以表示為
(5)非常重要，因為它通過一個齊次變換把連續(xù)時間幀上運動的剛體上的同一個三維點聯(lián)系了起來。
則全局參照系中點的運動表示為:
方程(6)是我們的運動估計方法的核心，因為它以無模型（model-free）的方式通過駐留在目標上的點表示剛體位姿變化，而不需要在估計中包含目標的三維位姿作為隨機變量。第3.2.2節(jié)詳細介紹了如何根據(jù)上述方程估計剛體的位姿變化。方程中的0k-1Hk 表示全局參照系中目標（提取出的）點的運動。對于本論文的其余部分，為了便于閱讀，我們將這個量稱為目標的位姿變化或目標的運動。
3.2相機位姿和目標運動估計
3.2.1相機位姿估計
通過最小化重投影誤差來估計相機位姿X：
我們用李代數(shù)xk 中的元素參數(shù)化SE(3)中的相機位姿:
通過對進行李代數(shù)參數(shù)化，將(8)代入(7)，得到最小二乘代價的解為:
可以使用Levenberg-Marquardt（L-M）算法來求解（9）。
3.2.2 目標運動估計
與相機位姿估計類似，構(gòu)造基于重投影誤差的代價函數(shù)可以求解目標運動。使用公式（6），目標的三維點通過重投影對應到圖像Ik中二維點的誤差項為:
通過最小化（誤差項）的方法可以找到最優(yōu)解：
3.2.3光流聯(lián)合估計
相機位姿估計和目標運動估計都依賴于良好的圖像對應。由于遮擋、較大的相對運動和較大的相機-目標距離，對于運動目標上（提取出的）點的跟蹤非常具有挑戰(zhàn)性。為了保證對于點的跟蹤的魯棒性，本文提出的技術(shù)旨在結(jié)合運動估計和光流估計進行（全局）細分。
對于相機的位姿估計，使用(2)對(7)中的誤差項進行重新表述:
使用李代數(shù)參數(shù)化中的元素，通過最小化代價函數(shù)可以得到最優(yōu)解:
類似地，(11)中的目標運動代價函數(shù)可以結(jié)合光流進行細化：
3.3圖優(yōu)化
該方法將動態(tài)SLAM作為一個圖優(yōu)化問題來細化相機位姿和目標運動，并建立包含靜態(tài)和動態(tài)結(jié)構(gòu)的全局一致地圖。我們將動態(tài)SLAM問題建模為一個因子圖，如圖3所示。因子圖公式具有很強的直觀性，它的優(yōu)點是允許有效的實現(xiàn)批次處理 (Dellaert and Kaess (2006); Agarwal et al. (2012))和增量求解器(Kaess et al. (2011); Polok et al. (2013); Ila et al.(2017))
四種測量/觀測被整合到一個聯(lián)合優(yōu)化問題中：三維點測量，視覺里程計測量，動態(tài)目標上點的運動的觀測和目標平滑運動的觀測。定義三維點的測量模型誤差為:
三維點測量因子如圖3中的白色圓圈所示。
定義視覺里程計模型誤差為:
里程計因子如圖3中橙色圓圈所示。
定義動態(tài)目標上點的運動模型誤差為:
被測剛體上所有點的運動特征都由式(6)給出的相同位姿變換0k-1Hlk 描述，對應因子是一個三元因子，如圖3中洋紅色圓圈所示，我們稱之為剛體上點的運動模型。
研究表明，在動態(tài)SLAM中，結(jié)合場景中目標運動的先驗知識是非常有價值的(Wang et al. (2007);Henein et al.(2020)).由于相機幀率和“控制相對較大目標(車輛)運動的物理定律的影響，并且為了防止它們的運動突然改變，我們引入平滑運動因素，用來最小化連續(xù)目標運動的變化，其中的誤差項可以定義為:
目標平滑運動因子用于最小化連續(xù)時間幀間目標運動的變化，如圖3中青色圓圈所示。
我們使用李代數(shù)參數(shù)化運動目標中的元素：
設=xM H代表圖中所有節(jié)點，利用對X和H使用李代數(shù)參數(shù)化(將(8)代入(16)和(17)，將(20)代入(18)和(19)，得出最小二乘代價的解為:
系統(tǒng)采用Levenberg-Marquardt(L-M)方法求解(21)中的非線性最小二乘問題。
4 系統(tǒng)
在本節(jié)中，我們提出了一種新的目標感知動態(tài)SLAM系統(tǒng)，該系統(tǒng)可以穩(wěn)健地估計相機和目標的運動，以及環(huán)境的靜態(tài)和動態(tài)結(jié)構(gòu)。整個系統(tǒng)概述如圖4所示。該系統(tǒng)由圖像預處理、跟蹤和建圖三個主要部分組成。
系統(tǒng)的輸入是雙目圖像或RGB-D圖像。對于雙目圖像，首先采用 Yamaguchi et al. (2014)描述的雙目深度估計方法提取深度信息，生成深度地圖，所得數(shù)據(jù)被處理為RGB-D形式。
雖然本系統(tǒng)最初設計為RGB-D系統(tǒng)，但為了充分利用基于圖像的語義信息，我們采用了單目相機的單幅圖像深度估計來獲取深度信息。我們的“基于學習的單目”系統(tǒng)是單目的，因為系統(tǒng)只使用RGB圖像作為輸入，而估計問題是利用RGB- D數(shù)據(jù)的，圖像的深度是通過單幅圖像深度估計得到的。
4.1預處理
這個模塊需要滿足兩個挑戰(zhàn)問題。一是將靜態(tài)背景與（動態(tài)）目標實現(xiàn)魯棒地分離，二是保證對于動態(tài)目標的長期跟蹤。為了實現(xiàn)這一目標，我們利用了計算機視覺的最新技術(shù)，例如水平語義分割和密集光流估計，以確保高效的目標運動分割和魯棒的目標跟蹤要求。
4.1.1目標實例分割
使用實例級語義分割對場景中潛在的可移動位姿進行分割和識別。語義信息是區(qū)分靜止和運動目標點的重要先驗，例如建筑物和道路總是靜態(tài)的，而汽車可以是靜態(tài)的，也可以是動態(tài)的。實例分割有助于將語義前景進一步劃分為不同的實例mask，從而更容易跟蹤每個單獨的位姿。此外，分割mask提供了一個“精確”的目標邊界，以確保對目標上的點的魯棒跟蹤。
4.1.2光流估算
利用密集光流可以最大化跟蹤運動目標上點的數(shù)量。大多數(shù)運動目標只占圖像的一小部分。因此，使用稀疏特征匹配不能保證魯棒性和長期的特征跟蹤。我們的方法利用了密集光流，通過對語義mask內(nèi)的所有點進行采樣來顯著增加（對于運動）目標（提取出的）點的數(shù)量。密集光流還可以通過分配給目標mask每個點的唯一標識符來持續(xù)地跟蹤多個位姿。在語義分割失敗的情況下，（DVO-SLAM）可以恢復目標mask;而使用稀疏特征匹配很難實現(xiàn)這種任務。
4.2 跟蹤
跟蹤組件包括兩個模塊；相機自身運動跟蹤包括特征檢測和相機位姿估計子模塊，目標運動跟蹤包括動態(tài)目標跟蹤和目標運動估計子模塊。
4.2.1特征檢測
為了實現(xiàn)快速的相機位姿估計，我們檢測一組稀疏的角點特征并利用光流對其進行跟蹤。在每一幀中，只有擬合相機運動估計的內(nèi)點才被保存到地圖中，用于跟蹤下一幀的對應（點）。如果跟蹤到的內(nèi)點的數(shù)量低于某一閾值，系統(tǒng)就會檢測并添加新的特征。在靜態(tài)背景下檢測到的稀疏特征中，圖像區(qū)域不包括被分割的位姿。
4.2.2相機位姿估計
對于所有檢測到的3D-2D對應靜態(tài)點，DVO-SLAM使用公式(13)可以計算得到相機位姿。為了保證估計的魯棒性，系統(tǒng)采用運動模型生成方法對數(shù)據(jù)進行初始化。具體說來，該方法基于重投影誤差生成了兩個模型，并對兩個模型的內(nèi)值進行了比較。一個模型是通過計算相機之前的運動生成的，而另一個模型是通過使用RANSAC的P3P ((Ke and Roumeliotis (2017))算法計算新的運動變換生成的。系統(tǒng)會選擇產(chǎn)生較多內(nèi)點的運動模型進行初始化操作。
4.2.3動態(tài)目標跟蹤
運動目標的跟蹤過程分為兩個步驟。第一步將分割的目標分為靜態(tài)目標和動態(tài)目標。第二步通過連續(xù)的幀來關(guān)聯(lián)動態(tài)目標。
實例級目標分割允許我們從背景中分離目標。雖然算法有能力對所有的分割目標估計其運動，動態(tài)目標的有效識別可以有助于降低系統(tǒng)的計算成本。這是基于場景流估計實現(xiàn)的。具體來說，在獲得相機位姿后，描述三維點在k-1到k幀之間運動的場景流向量可以被計算得到： 
與光流法不同，場景流(理想情況下僅由場景移動引起)可以直接決定某些結(jié)構(gòu)是否有過移動。理想情況下，對應于所有靜態(tài)三維點的場景流向量的大小應該為零。然而，在深度和匹配方面的噪聲或錯誤會使真實場景中的情況復雜化。為了穩(wěn)健地處理這個問題，我們會對每個目標上的所有樣本點計算它們的場景流大小。如果場景中某些點的場景流數(shù)值大于預先設定的閾值，則認為該點是運動的。在本系統(tǒng)進行的所有實驗中，這個閾值都被設置為0.12。如果“動態(tài)”點的比例高于某一水平(所有點數(shù)的30%)，則該目標會被識別為是運動的，否則該目標為靜止的。如前文所述，（設置）閾值是用于識別一個位姿是否是動態(tài)的，為了（使結(jié)果）更加保守，系統(tǒng)可以靈活地將一個靜態(tài)位姿建模為動態(tài)位姿，并每個時間步長對其估計出一個（大小為）零的運動。然而，如果進行相反的處理，則會降低系統(tǒng)的性能。
實例級位姿分割只能提供單個圖像目標標簽，然后需要隨時間在幀之間跟蹤目標以及它們的運動模型。我們建議使用光流法來關(guān)聯(lián)幀之間的點標簽。一個點標簽與采樣點的唯一目標標識符相同。我們保持一個有限的跟蹤標簽集。當探測到更多運動的目標時，標簽集的元素數(shù)量就會增加。靜態(tài)位姿和背景使用=0標記。
理想情況下，對于k幀中的每個被檢測位姿，其所有點的標簽都應該唯一地與k-1幀中的對應標簽對齊。但在實際應用中，這會受到噪聲、圖像邊界和遮擋的影響。為了克服這個問題，我們給所有的點都貼上了標簽，當這些點對應起來時就會使標簽的數(shù)目達到最大。對于一個動態(tài)目標而言，如果它前一幀中最頻繁的標簽為0，這意味著該目標（在這一幀才）開始移動，出現(xiàn)在場景的邊界上，或者從遮擋中重新出現(xiàn)。在這種情況下，該位姿會被分配一個新的跟蹤標簽。
4.2.4目標的運動估計
如上文所述，（運動）目標在場景中通常是小范圍出現(xiàn)的，很難得到足夠的稀疏特征來穩(wěn)健地跟蹤和估計目標的運動。我們對所有位姿mask內(nèi)的第三個點進行了采樣，并在圖像幀之間對它們進行跟蹤。與相機位姿估計相似，只有內(nèi)點會被保存進地圖中并用于下一幀的跟蹤。當跟蹤的目標點減少到一定水平以下時，會對目標進行采樣并添加新的目標點。我們采用了4.2.2節(jié)中討論的方法來生成初始目標運動模型。
4.3地圖
在地圖模塊中，系統(tǒng)會構(gòu)造并維護一個全局地圖。同時，系統(tǒng)會從全局地圖中提取出一個基于當前時間步長和前一個時間步長窗口的局部地圖。兩個地圖都是通過批量優(yōu)化過程更新的。
4.3.1局部批量優(yōu)化
我們的系統(tǒng)會維護和更新一個局部地圖。局部批量優(yōu)化的目標是確保將精確的相機位姿估計提供給全局批量優(yōu)化。相機位姿估計對目標運動估計的精度和算法的整體性能有很大的影響。局部地圖是使用一個固定大小的包含最后一幀信息的滑動窗口構(gòu)建的。局部地圖會共享一些公共信息:這會造成不同窗口之間的重疊。我們選擇只在窗口大小范圍內(nèi)局部優(yōu)化相機位姿和靜態(tài)結(jié)構(gòu)，因為局部優(yōu)化動態(tài)結(jié)構(gòu)不會給優(yōu)化帶來任何好處，除非在窗口內(nèi)假設存在硬約束(例如目標的恒定運動)。但是，如果需要的話，系統(tǒng)能夠在局部地圖中合并靜態(tài)和動態(tài)結(jié)構(gòu)。當一個局部地圖以類似的方式構(gòu)造時，因子圖優(yōu)化將執(zhí)行來細化局部地圖中的所有變量，然后將它們在全局地圖中進行更新。
4.3.2全局批量優(yōu)化
跟蹤模塊的輸出和局部批量優(yōu)化包含了相機位姿、目標運動和內(nèi)點結(jié)構(gòu)。它們被保存在一個全局地圖中，這個全局地圖是由之前的所有時間幀構(gòu)建的，并且隨著每一個新的時間幀的加入不斷更新。在處理完所有輸入幀后，系統(tǒng)會基于全局地圖構(gòu)造因子圖。為了有效地獲得時間約束，系統(tǒng)在因子圖中只添加被跟蹤超過3個實例的點。該圖被表述為如3.3節(jié)中描述的優(yōu)化問題。優(yōu)化結(jié)果將作為整個系統(tǒng)的輸出。
4.3.3從地圖到跟蹤:
維護地圖可以為跟蹤模塊估算當前狀態(tài)時提供歷史信息，如圖4所示，藍色箭頭從全局地圖指向系統(tǒng)跟蹤模塊的多個組件。利用最后一幀的內(nèi)點可以跟蹤當前幀中的對應點并估計相機位姿和目標運動。最后的相機和目標運動也可能作為先驗模型來初始化4.2.2和4.2.4節(jié)中所述的當前估計問題。此外，在語義位姿分割失敗導致“間接遮擋”的情況下，通過利用之前分割的mask，（運動）目標（上提取出的）點可以幫助（系統(tǒng)實現(xiàn)）跨幀關(guān)聯(lián)語義mask，以確保對目標的魯棒跟蹤。
5 實驗
我們評估了DVO-SLAM在相機運動，目標運動和速度（檢測），以及目標跟蹤情況下的性能。對室內(nèi)場景的牛津多運動數(shù)據(jù)集(Judd and Gammell (2019))和戶外場景的KITTI跟蹤數(shù)據(jù)集(Geiger et al.(2013))進行了評估。我們還將結(jié)果與兩個最新的方法，MVO（judd et al.(2018)）和CubeSLAM(Yang and Scherer(2019))進行對比，以證明VDO-SLAM的卓越性能。
5.1系統(tǒng)設置
我們采用基于學習的實例級位姿分割Mask R-CNN (He et al.(2017))來生成位姿分割mask。該方法的模型在COCO 數(shù)據(jù)集上訓練(Lin et al.(2014))，可以直接用于本工作中，不需要進行任何微調(diào)。對于密集光流，我們利用了最先進的方法PWC-Net (Sun et al.(2018))（進行測試）。該模型在FlyingChairs數(shù)據(jù)集上進行訓練(Mayer et al. (2016))，然后在Sintel上進行微調(diào)(Butler et al.(2012)和在KITTI數(shù)據(jù)集上進行訓練(Geiger et al.， 2012)。為了在我們系統(tǒng)的“單目”版本生成深度地圖，我們采用了一種基于學習的單目深度估計方法MonoDepth2 (Godard et al.(2019)).。在排除本文測試數(shù)據(jù)的情況下，對該模型進行了深度特征分割(Eigen et al.(2014))訓練。特征檢測使用FAST完成(Rosten and Drummond（2006））( Rublee et al. (2011)).
5.2誤差指標
我們使用位姿變化誤差來評估VDO-SLAM估計的SE(3)運動，即給定一個真值運動變換矩陣T和相應的估計運動矩陣（進行對比），其中TSE(3)既可以描述為一個相機相對位姿也可以是一個目標運動。位姿變化誤差可以通過:E=-1T求得。平移誤差Et通過計算E的平移分量的L2范數(shù)求得。旋轉(zhuǎn)誤差Er通過計算E的旋轉(zhuǎn)分量的軸角表示的旋轉(zhuǎn)角度來計算。對于不同的相機時間步長和序列中不同的目標，我們分別計算相機位姿和目標運動的均方根誤差(RMSE)。利用目標位姿的真值，將慣性坐標系中的位姿變化轉(zhuǎn)換為目標坐標系中，我們可以得到目標固定坐標系中的位姿變化數(shù)值：
我們也評估了（運動）目標的速度誤差。利用位姿變化量并取其差值，即可得到該目標上一點在慣性坐標系中表示的線速度大小。
為了得到更可靠的測量結(jié)果，我們對在特定時間的一個（運動）目標上所有點取平均：
則估計速度與真實速度之間的速度誤差可以計算為:Es=||-||。對于在時間幀上跟蹤的不同位姿，我們也計算RMSE作為誤差指標。
5.3牛津多運動數(shù)據(jù)集
最新的牛津多運動數(shù)據(jù)集(Judd and Gammell(2019))包含了通過移動雙目或RGB-D相機傳感器在室內(nèi)場景中觀察多個擺動箱或玩具車的視頻序列。通過Vicon運動捕捉系統(tǒng)我們可以獲取相機和運動目標的真實運動軌跡。我們只選擇了擺動箱SLAM序列進行評估，因為真實駕駛場景的結(jié)果是在KITTI數(shù)據(jù)集上進行評估的。表1顯示了DVO-SLAM與最先進的MVO(Judd et al. (2018))比較的結(jié)果。由于MVO是一個沒有全局優(yōu)化（模塊）的視覺里程計系統(tǒng)，我們關(guān)閉了DVO-SLAM系統(tǒng)中的批量優(yōu)化模塊，并生成結(jié)果來進行公平的對比。我們使用了5.2節(jié)中描述的誤差指標（進行評估）。
總體來說，我們的方法在10個誤差指標中有7個對于（數(shù)據(jù)集中）4個擺動箱的相機位姿估計和運動估計問題表現(xiàn)出了更高的精度。特別地，我們的方法在估計左上角和左下角的相機的位姿和擺動箱的運動時性能可以提高50%。當觀測目標的旋轉(zhuǎn)運動時，特別是觀測右上角和右下角的擺動箱時我們的系統(tǒng)會產(chǎn)生相對較高的誤差。有趣的是，我們提出的算法在這些情況下的表現(xiàn)不如Judd et al. (2018)，因為本算法不是為此類運動和場景設計的，而是為觀測相對較大的戶外目標的運動而設計的，例如城市駕駛場景。我們認為這是由于使用的光流算法不能很好地優(yōu)化旋轉(zhuǎn)目標。其結(jié)果是對（旋轉(zhuǎn)目標上）點運動的估計能力較差，從而導致目標跟蹤性能的下降。即使在對于觀測旋轉(zhuǎn)目標的相關(guān)性能較差的情況下，密集光流運動估計在其他指標上的好處依然是顯而易見的。
圖5顯示了我們算法在牛津多運動數(shù)據(jù)集上的地圖輸出結(jié)果。擺動箱上的動態(tài)特征軌跡在視覺上與箱體的實際運動軌跡相對應。從圖5中左下角紫色框的搖擺運動中可以清楚地看出這一點。
5.4 KITTI跟蹤數(shù)據(jù)集
KITTI跟蹤數(shù)據(jù)集(Geiger et al.(2013))包含21個序列，其中包含關(guān)于相機和目標位姿的真值信息。在這些序列中，有些不包括在我們系統(tǒng)的評估要求中;因為它們不包含移動目標(只有靜態(tài)場景)或只包含非剛性目標的行人，這超出了本系統(tǒng)的能力范疇。
5.4.1相機和目標的運動
表2給出了DVO-SLAM在9個序列中對于相機和目標運動估計的結(jié)果，與CubeSLAM (Yang and Scherer(2019))給出了的5個序列的結(jié)果做對比。我們最初嘗試用我們的默認參數(shù)來評估CubeSLAM，但會產(chǎn)生很大的錯誤，因此我們只比較了由CubeSLAM的作者提供的序列結(jié)果。由于CubeSLAM是用于單目相機的，我們也計算了DVO-SLAM基于學習的單目版本(如第4節(jié)所述)的結(jié)果，來進行公平的對比。
總體來說，我們提出的RGB-D和基于學習的單目方法在相機和目標運動估計方面都獲得了很高的精度。與CubeSLAM相比，我們的RGB-D版本在（計算）相機運動方面的誤差更低，而我們基于學習的單目版本的誤差較高。然而，這兩個版本在目標運動估計中都獲得了一致的較低的誤差。特別的，如圖6所示，CubeSLAM的平移誤差和旋轉(zhuǎn)誤差均在3米和3度以上，極端情況下的誤差分別達到32米和5度。而在（使用）RGBD版本的情況下，我們的平移誤差在0.1-0.3米之間，旋轉(zhuǎn)誤差在0.2-1.5度之間，在（使用）單目學習版本的情況下，我們的平移誤差在0.1-0.3米之間，旋轉(zhuǎn)誤差在0.4-3度之間，這說明了我們的目標運動估計能力（相對于CubeSLAM）有了一個數(shù)量級的提高。
5.4.2目標跟蹤與速度
我們還演示了系統(tǒng)對于動態(tài)目標跟蹤的性能，并展示了對于目標速度估計的結(jié)果，這是自主駕駛應用的重要信息。圖7給出了在所有測試序列中，一些選中目標(跟蹤超過20幀)的跟蹤長度和速度（估計）的結(jié)果。我們的系統(tǒng)能夠跟蹤大多數(shù)目標在其出現(xiàn)的序列中超過80%的比例。此外，我們估計的目標速度始終與其真實速度接近。
5.5討論
除了在5.4節(jié)和5.3節(jié)的評估之外，我們還提供了詳細的實驗結(jié)果，以證明我們提出的系統(tǒng)中關(guān)鍵模塊的有效性。最后，我們將討論該系統(tǒng)的計算成本。
5.5.1對點的魯棒跟蹤
圖優(yōu)化利用空間和時間信息細化相機位姿和目標運動，以及靜態(tài)和動態(tài)結(jié)構(gòu)。這個過程需要在數(shù)量和質(zhì)量上對正確的點進行魯棒的跟蹤。這是通過結(jié)合運動估計對光流估計進行細化實現(xiàn)的，如3.2.3節(jié)所述。通過對a的比較，證明了聯(lián)合優(yōu)化的有效性，通過將只對(9)相機運動或(11)只對目標運動進行優(yōu)化的基線方法和對運動和光流(結(jié)合)都進行優(yōu)化的改進方法(使用(13)或(15))進行比較，可以顯示出聯(lián)合優(yōu)化方法的高效性。表3顯示，聯(lián)合方法可以獲得更多的（保證系統(tǒng)可以）長期跟蹤的點。
利用聯(lián)合估計方法得到的跟蹤點，系統(tǒng)可以更好地估計相機位姿和目標運動。在之前展示的KITTI數(shù)據(jù)集的9個序列中，系統(tǒng)觀察到的平移和旋轉(zhuǎn)誤差都得到了大約15%到20%的改善。詳情見表4。
5.5.2對非直接遮擋的魯棒性
在某些情況下，由于直接或間接遮擋(光照變化等)，使用mask分割可能會失敗。通過在第4.3.3節(jié)中描述的mask傳播方法，我們提出的系統(tǒng)能夠處理由于間接遮擋引起的mask失效問題。圖9展示了DVO-SLAM跟蹤一輛白色貨車80幀的示例，其中掩模分割在33幀中失敗了。盡管目標分割失敗，我們的系統(tǒng)仍然能夠持續(xù)跟蹤貨車，在整個序列中估計得到速度誤差的平均值為2.64km/h。在序列的后半部分，由于部分遮擋和目標離相機越來越遠等的原因，系統(tǒng)測得的速度誤差變大。
5.5.3對目標運動的全局細化
初始目標運動估計(在系統(tǒng)的跟蹤部分)是獨立于幀之間的，因為它純粹與傳感器的測量能力有關(guān)。如圖10所示。藍色曲線描述了在KITTI跟蹤數(shù)據(jù)集的03序列中觀察了55幀的貨車的初始目標速度估計。如圖所示，速度估計不平滑，在序列的后半部分出現(xiàn)了較大的誤差。這主要是由于目標距離相機的距離增加導致，整個目標只占據(jù)了場景的一小部分。在這種情況下，使用傳感器單獨測量目標的運動估計變得具有挑戰(zhàn)性且十分容易出錯。因此，和在第3.3節(jié)討論的內(nèi)容相同，我們制定了一個因子圖并對靜態(tài)和動態(tài)結(jié)構(gòu)都進行了細化。圖10中的綠色曲線顯示的是全局細化后的目標速度估計結(jié)果，序列的前半部分曲線變得更加平滑，后半段（誤差）也得到了顯著的改善。
5.5.4計算分析
最后，我們提供DVO-SLAM系統(tǒng)的計算分析結(jié)果。實驗在一臺16GB內(nèi)存的Intel Core i7 2.6 GHz筆記本電腦上進行。對目標語義分割和密集光流計算時間取決于電腦的GPU性能和CNN模型的復雜度。許多最新的算法都可以（在上面進行）實時運行(Bolya et al. (2019);Hui et al. (2020))。在本文中，語義分割和光流結(jié)果都是離線生成并輸入到系統(tǒng)中。整個SLAM系統(tǒng)在CPU上用c++實現(xiàn)，使用修改后的g2o作為其后端 (K¨ummerleet al. (2011))。我們在表5中顯示了DVO-SLAM在兩個數(shù)據(jù)集上的計算時間。在局部批量優(yōu)化中，窗口大小被設置為20幀，并有4幀的重疊。每個系統(tǒng)組件在所有幀和序列上的時間成本都是平均的?？偟膩碚f，我們提出的系統(tǒng)的跟蹤部分可以根據(jù)檢測到的運動物體的數(shù)量以5-8幀每秒的幀率運行，采用并行計算可以進一步改善性能。全局批量優(yōu)化的運行時間很大程度上取決于場景中相機位姿數(shù)量(幀數(shù))和目標密度(根據(jù)每幀觀察到的動態(tài)目標的數(shù)量計算得到)。
6 結(jié)論
在本文中，我們介紹了VDO-SLAM。這是一種新穎的基于動態(tài)特征的，基于場景中圖像語義信息的，不需要額外的目標位姿或幾何信息，就可以實現(xiàn)對動態(tài)目標同步定位、建圖和跟蹤的SLAM系統(tǒng)。該系統(tǒng)可以在室內(nèi)和具有挑戰(zhàn)性的室外數(shù)據(jù)集中獲得全局一致的魯棒性和準確性的結(jié)果，并能在目標運動估計方面展示其卓越的性能。我們認為在目標運動估計中之所以能取得高精度結(jié)果是由于我們的系統(tǒng)是一個基于特征的系統(tǒng)。特征點(法)仍然是SLAM系統(tǒng)中最容易檢測，跟蹤和集成的(方法)，不需要前端提供任何關(guān)于目標模型的額外信息。
如何減少動態(tài)目標的SLAM計算復雜度是一個重要的課題。在長期應用，不同的技術(shù)可以被應用來限制圖像數(shù)量的增長(Strasdat et al. (2011); Ila et al.(2010)).事實上，總結(jié)/刪除很久之前觀測到的動態(tài)目標相關(guān)的地圖點，似乎是長期SLAM系統(tǒng)適應高度動態(tài)環(huán)境的自然**（nature）**處理手段。
文獻插圖
圖1。VDO-SLAM系統(tǒng)的結(jié)果。(上圖)一個完整的地圖，包括相機軌跡，靜態(tài)背景和移動目標的結(jié)構(gòu)。(下圖)靜態(tài)背景上（運動）目標檢測得到的點，以及被估計的目標（移動）速度。黑色的圓圈代表靜態(tài)點，每個目標都用不同的顏色顯示。
圖2。符號和坐標系。實線曲線表示相機和目標在慣性坐標系下的位姿;虛線曲線表示了在目標固定坐標系中0X和0L各自的運動。實線表示慣性坐標系中的三維點，虛線表示相機坐標系中的三維點。（0X,0L分別為在全局參考系0中機器人/相機和目標的三維位姿）
圖3。帶有移動位姿的目標感知SLAM的因子圖。黑色方塊表示相機在不同時間步長的位姿，藍色方塊表示三個靜態(tài)點，紅色方塊表示一個運動目標(虛線框)在不同時間步長的相同動態(tài)點，綠色方塊表示目標在不同時間步長的位姿變化。為了便于可視化，這里只畫一個動態(tài)點，不過在估計時，被檢測動態(tài)位姿上的所有點都會被使用到。先驗一元因子以黑色圓圈表示，測程二值因子以橙色圓圈表示，點測量二值因子以白色圓圈表示，點運動三值因子以洋紅色圓圈表示,平滑運動二元因子以青色圓圈表示。
圖4。VDO -SLAM系統(tǒng)概述。首先對輸入圖像進行預處理，生成實例級位姿分割和密集光流。然后，對預處理后的圖像跟蹤靜態(tài)背景結(jié)構(gòu)和動態(tài)位姿的特征。接著在全局批量優(yōu)化中細化根據(jù)特征軌跡估計得到的相機位姿和目標運動，并根據(jù)每一幀更新局部地圖。該系統(tǒng)能夠輸出相機位姿，靜態(tài)結(jié)構(gòu)，動態(tài)目標位姿的軌跡，并可以估計其隨時間的位姿變化情況。
表1。VDO-SLAM與MVO (Judd et al. (2018))在牛津多運動數(shù)據(jù)集中的swinging-4_unconstrained序列上對于相機和目標運動估計精度的比較結(jié)果。粗體的數(shù)字表示更好的結(jié)果。
圖5。我們的方法對牛津多運動數(shù)據(jù)集的定性結(jié)果。(左圖：)含有相機軌跡、靜態(tài)結(jié)構(gòu)和動態(tài)點軌跡的3D地圖。(右圖：)靜態(tài)背景和運動目標上檢測得到的點。黑色點對應靜態(tài)點，每個目標上的特征以不同的顏色顯示。
圖6。VDO-SLAM與CubeSLAM (Yang and Scherer (2019))對于目標運動估計精度的比較結(jié)果。顏色豎條對應于左邊y軸的數(shù)值，表示對數(shù)尺度下的平移誤差。曲線表示旋轉(zhuǎn)誤差，在線性尺度上對應于右y軸。
表2。VDO-SLAM與CubeSLAM (Yang and Scherer (2019))在KITTI數(shù)據(jù)集的第九個序列上對于相機和目標運動估計精度的比較結(jié)果。粗體的數(shù)字表示更好的結(jié)果。
圖7。跟蹤性能和速度估計。由于空間的限制,VDO-SLAM對于一些選定的目標(跟蹤超過20幀)的跟蹤長度和速度的演示結(jié)果。顏色條表示運動目標軌跡的長度，與左y軸相對應。圓圈代表目標的移動速度，與右y軸相對應。GT表示真值, EST.表示估計值。
表3。在KITTI數(shù)據(jù)集的9個序列上跟蹤超過5幀的點的數(shù)量。粗體的數(shù)字表示更好的結(jié)果。粗體下劃線的數(shù)字表示增加的數(shù)量級。
表4。KITTI數(shù)據(jù)集第九個序列上的（DVO-SLAM系統(tǒng)的）平均相機位姿和目標運動誤差。粗體的數(shù)字表示更好的結(jié)果。
圖8。系統(tǒng)輸出的圖例展示：含有相機位姿，靜態(tài)背景結(jié)構(gòu)，和動態(tài)目標位姿軌跡的動態(tài)地圖。如圖是VDO-SLAM對KITTI序列的樣本結(jié)果。黑色點代表靜態(tài)背景，每個檢測到的（運動）目標都用不同的顏色顯示。左上角的圖片表示01序列，并對序列末尾結(jié)果進行了放大展示，右上角的圖片表示06序列，底部的圖片表示03序列。
圖9。當語義分割失敗時，系統(tǒng)跟蹤和速度估計的魯棒性展示。如圖是一個在00序列上對于白色貨車(地面真實平均速度20km/h)在VDO-SLAM下跟蹤和速度估計性能的展示例子。(上圖)藍色豎條代表成功的目標分割，綠色曲線代表目標的檢測速度誤差。(左下圖)對貨車語義分割失敗的圖例展示。(右下圖)VDO-SLAM在貨車（語義分割）之前的跟蹤特征結(jié)果
圖10。全局細化對目標運動估計的影響。在序列03上對沿直線前進的貨車的速度估計結(jié)果如圖所示。藍色線條表示初始估計速度，綠色線條表示全局細化后的估計速度，紅色為速度真值。
表5。在兩個數(shù)據(jù)集上的不同系統(tǒng)組件的運行時間。每個組件的運行時間是所有幀和序列的用時的平均值。特別的，目標運動估計時間統(tǒng)計需要對目標數(shù)量的取平均，局部批處理優(yōu)化時間統(tǒng)計需要對幀數(shù)量的取平均。