一種適用于動態(tài)場景的多層次地圖構(gòu)建算法

作者：Xinggang Hu

在動態(tài)場景中視覺SLAM中的定位和地圖構(gòu)建面臨著巨大的挑戰(zhàn)。近年來，許多優(yōu)秀的研究作品已經(jīng)提出了有效的定位問題解決方案。然而，關(guān)于在動態(tài)場景中構(gòu)建長期一致地圖的優(yōu)秀作品相對較少，這嚴(yán)重阻礙了地圖應(yīng)用的發(fā)展。為了解決這個問題，我們設(shè)計了一個針對動態(tài)場景的多層次地圖構(gòu)建系統(tǒng)。在這個系統(tǒng)中采用了多目標(biāo)跟蹤算法、DBSCAN聚類算法和深度信息來矯正目標(biāo)檢測的結(jié)果，準(zhǔn)確提取靜態(tài)點云，構(gòu)建密集的點云地圖和八叉樹地圖。我們提出了一個專門針對動態(tài)場景的平面地圖構(gòu)建算法，涉及動態(tài)環(huán)境中平面的提取、過濾、數(shù)據(jù)關(guān)聯(lián)和融合優(yōu)化，從而創(chuàng)建一個平面地圖。此外引入了一個專門針對動態(tài)場景的物體地圖構(gòu)建算法，包括物體參數(shù)化、數(shù)據(jù)關(guān)聯(lián)和更新優(yōu)化。對公共數(shù)據(jù)集和實際場景的大量實驗驗證了本研究中構(gòu)建的多層次地圖的準(zhǔn)確性，以及所提出算法的魯棒性。此外，通過利用構(gòu)建的物體地圖進(jìn)行動態(tài)物體跟蹤，我們展示了算法的實際應(yīng)用前景。

主要貢獻(xiàn)

本文提出了一種適用于動態(tài)場景的多層次地圖構(gòu)建算法，如圖1所示的系統(tǒng)框架。首先，利用YOLOX[8]獲取場景的語義信息，采用多目標(biāo)跟蹤算法對漏檢進(jìn)行補償，利用DBSCAN密度聚類算法和深度信息進(jìn)一步優(yōu)化潛在移動物體的檢測邊界框。隨后，我們提取點云和平面，并利用主成分分析（PCA）和最小外接矩形對物體進(jìn)行參數(shù)化。此外，對點云、平面和物體進(jìn)行過濾。接下來，基于我們之前的研究[9]提供的相機位姿，我們進(jìn)行點云拼接和融合，對平面和物體進(jìn)行數(shù)據(jù)關(guān)聯(lián)和更新優(yōu)化，隨后，將稠密點云地圖轉(zhuǎn)換為八叉樹地圖。最終實現(xiàn)一個多層次地圖的構(gòu)建，包括稠密點云地圖、八叉樹地圖、平面地圖和物體地圖，從而豐富了地圖的應(yīng)用場景。圖1顯示了用于動態(tài)場景的多層次地圖構(gòu)建算法的系統(tǒng)框架，通過在公開可用的數(shù)據(jù)集和實際場景中進(jìn)行的實驗，充分驗證了我們算法的有效性。

圖1. 適用于動態(tài)場景的多層次地圖構(gòu)建算法的系統(tǒng)框架，淺綠色部分是輸入模塊，負(fù)責(zé)輸入RGB圖像和深度圖像。深綠色部分是預(yù)處理模塊，主要負(fù)責(zé)獲取和預(yù)處理語義信息。黃色、藍(lán)色和棕色模塊是地圖構(gòu)建模塊，分別代表了構(gòu)建稠密點云地圖和八叉樹地圖、平面地圖、物體地圖的一般過程。紫紅色部分是輸出模塊，負(fù)責(zé)輸出地圖構(gòu)建模塊構(gòu)建的多層次地圖。

本文的貢獻(xiàn)總結(jié)如下：?

基于矯正后的物體檢測結(jié)果對點云進(jìn)行過濾，構(gòu)建了僅包含靜態(tài)元素的純凈點云地圖和八叉樹地圖。

提出了一種在動態(tài)場景中構(gòu)建平面地圖的方法，實現(xiàn)了對環(huán)境結(jié)構(gòu)的感知。?

提出了一種在動態(tài)場景中構(gòu)建物體地圖的方法，使SLAM能夠滿足更高級的需求，如機器人環(huán)境理解、物體操縱和語義增強現(xiàn)實。?

就我們所知，在動態(tài)場景中，本文是第一篇構(gòu)建平面地圖的工作，也是第一篇準(zhǔn)確參數(shù)化物體并構(gòu)建準(zhǔn)確完整的輕量級物體地圖的工作。

內(nèi)容概述

幾何地圖的構(gòu)建?

A. 稠密點云地圖和八叉樹地圖的構(gòu)建?

在存在語義先驗信息的情況下，可以根據(jù)語義類別刪除目標(biāo)檢測框或語義掩碼中的點云，從而構(gòu)建僅包含靜態(tài)因素的稠密點云地圖。然而，僅依賴于原始的語義結(jié)果，語義信息的“漏檢”和“欠分割”問題可能導(dǎo)致動態(tài)對象的不完整去除。本文使用YOLOX進(jìn)行語義信息獲取以解決這個問題，為了解決“漏檢”問題，本文利用多目標(biāo)跟蹤算法進(jìn)行漏檢補償，為了解決“欠分割”問題，首先采用DBSCAN聚類算法在潛在移動對象的邊界框內(nèi)提取前景點。隨后，基于鄰近像素沿著檢測框邊界和前景點的深度信息，適當(dāng)擴展檢測框。為了避免DBSCAN聚類引起的錯誤，我們將檢測框的所有四個方向設(shè)置為擴展限制，限制為50個像素。在關(guān)鍵幀中，提取并映射到3D世界坐標(biāo)系中的潛在移動對象的校正邊界框外的像素。然后，基于我們先前研究提供的相機姿態(tài)，將不同關(guān)鍵幀提取的點云進(jìn)行拼接和融合，隨后通過體素網(wǎng)格濾波進(jìn)行下采樣，為了提高存儲效率并支持導(dǎo)航和避障等任務(wù)，點云地圖轉(zhuǎn)換為八叉樹地圖。

B. 平面地圖的構(gòu)建?

使用PEAC算法 [30] 用于平面提取，獲取當(dāng)前相機坐標(biāo)系中平面的參數(shù)和點云，然后提取平面的邊緣點。隨后，利用PCL點云庫對平面進(jìn)行二次擬合，獲取精化后的參數(shù)和內(nèi)點，接著移除平面邊緣點的異常值，在此過程中，根據(jù)深度信息、內(nèi)點比例以及與目標(biāo)檢測框的位置關(guān)系等各種因素對平面進(jìn)行過濾。完成平面地圖初始化后，遍歷當(dāng)前幀中檢測到的平面和地圖中已存在的平面，建立數(shù)據(jù)關(guān)聯(lián)。然而，在復(fù)雜的動態(tài)場景中，檢測到的平面常常會出現(xiàn)顯著的錯誤和隨機性，導(dǎo)致平面數(shù)據(jù)關(guān)聯(lián)失敗。隨著更多的觀測，未成功關(guān)聯(lián)的兩個平面將朝著正確的方向進(jìn)行優(yōu)化，使得后續(xù)關(guān)聯(lián)變得更加容易。因此，在局部地圖構(gòu)建線程中，對地圖中的平面進(jìn)行成對比較。如果兩個平面滿足上述關(guān)聯(lián)條件，它們將被視為潛在的未關(guān)聯(lián)。然后，將觀測次數(shù)較少的平面合并到觀測次數(shù)較多的平面中并進(jìn)行優(yōu)化,隨后，將觀測次數(shù)較少的平面從地圖中移除。

構(gòu)建對象地圖?

A. 對象參數(shù)化和數(shù)據(jù)關(guān)聯(lián)

由于要建模的對象通常屬于背景，且遠(yuǎn)離相機，提取的地圖點通常數(shù)量稀少且質(zhì)量較差，使用聚類算法進(jìn)行異常值去除是不可行的。因此每幀中使用密集點云進(jìn)行對象建模，并利用DBSCAN密度聚類算法處理點云。在當(dāng)前幀k中，對于每個檢測到的實例，我們對地圖中的每個對象實例進(jìn)行關(guān)聯(lián)判斷。運動IoU，投影IoU，3D-IoU和非參數(shù)統(tǒng)計是常見的對象數(shù)據(jù)關(guān)聯(lián)策略。盡管它們存在限制，但在集成時，這些策略可以相互補充，從而產(chǎn)生更強大、準(zhǔn)確和多功能的對象數(shù)據(jù)關(guān)聯(lián)算法。

圖2. 地圖點的異常值剔除。（a）確定桌面平面。（b）根據(jù)點到平面的距離剔除異常值。（c）使用隔離森林算法剔除異常值。

B 對象的更新與優(yōu)化?

我們分別使用密集點云和稀疏地圖點來對檢測實例和對象實例進(jìn)行參數(shù)化，這種方法彌補了單個幀中地圖點不足的缺點和多個幀中密集點云的顯著時間消耗。在成功的數(shù)據(jù)關(guān)聯(lián)之后，地圖點和參數(shù)將會更新，隨后，利用對象的地圖點與平面或與對象關(guān)聯(lián)的平面之間的距離以及孤立森林算法，從這些地圖點中去除異常值，如圖2所示。

實驗

我們在TUM RGB-D數(shù)據(jù)集上評估了我們的算法性能，并在實際場景中應(yīng)用算法進(jìn)行動態(tài)物體跟蹤。本研究的主要重點是地圖構(gòu)建，由于測試序列沒有提供真值地圖，實驗主要旨在定性地展示地圖構(gòu)建結(jié)果。我們的算法在一臺搭載i9-12900H CPU、3060 GPU和16GB內(nèi)存的筆記本電腦上運行。

幾何地圖的構(gòu)建?

稠密點云地圖和八叉樹地圖的構(gòu)建結(jié)果如圖3所示?？梢杂^察到，由于缺乏處理動態(tài)物體的模塊，ORB-SLAM2算法無法在高動態(tài)場景中進(jìn)行定位和地圖構(gòu)建。在低動態(tài)場景中，該算法保留了動態(tài)物體的點云。由于物體檢測中的漏檢和完全覆蓋潛在運動物體的邊界框的挑戰(zhàn)，通過刪除位于原始潛在運動物體檢測邊界框內(nèi)的點云構(gòu)建的稠密點云地圖包含了這些物體的大量殘留痕跡。

圖3. 點云地圖和八叉樹地圖。頂部一行顯示了使用ORB-SLAM2算法和密集建圖模塊構(gòu)建的稠密點云地圖。第二行展示了使用先前研究的方法 [9] 作為定位模塊構(gòu)建的稠密點云地圖，其中排除了位于潛在運動物體檢測區(qū)域內(nèi)的點云。第三行展示了我們算法構(gòu)建的稠密點云地圖。底部一行展示了我們算法生成的八叉樹地圖。

圖4中構(gòu)建的平面地圖在動態(tài)場景中準(zhǔn)確地感知靜態(tài)背景平面結(jié)構(gòu)。這可以應(yīng)用于高級場景，如增強現(xiàn)實，并作為地標(biāo)來增強相機姿態(tài)估計的準(zhǔn)確性。

圖4：相鄰放置的重復(fù)物體的生成地圖，左側(cè)的圖像提供了序列的概覽。

物體地圖的構(gòu)建?

我們在TUM數(shù)據(jù)集的8個動態(tài)序列上評估了物體地圖構(gòu)建的性能，如圖5所示。為了驗證物體地圖構(gòu)建的準(zhǔn)確性，我們將構(gòu)建的物體模型疊加在稠密地圖上，并將它們投影到圖像平面上。在高動態(tài)場景中，我們的算法能夠準(zhǔn)確地對場景中的幾乎所有物體進(jìn)行建模，不受相機不同的運動模式和環(huán)境中動態(tài)物體的影響。然而，在低動態(tài)場景中，兩個人一直坐在桌子旁邊，導(dǎo)致了靜態(tài)物體和背景的嚴(yán)重遮擋。因此，我們的算法對于某些物體缺乏足夠的觀測，導(dǎo)致了一些物體的建模不夠準(zhǔn)確，這是不可避免的。實驗結(jié)果表明，我們的算法在物體參數(shù)化、物體數(shù)據(jù)關(guān)聯(lián)和物體優(yōu)化策略方面具有很強的有效性，通過克服動態(tài)物體的影響，構(gòu)建的物體地圖為后續(xù)的應(yīng)用，如語義導(dǎo)航、物體抓取和增強現(xiàn)實，提供了有力的支持。

圖5. 物體地圖。常規(guī)形狀的物體，如顯示器、書籍和鍵盤，使用立方體表示，而不規(guī)則形狀的物體，如椅子、瓶子和泰迪熊，使用二次曲面表示。

在真實環(huán)境中的穩(wěn)健性測試?

我們還使用Realsense D435i相機在真實世界場景中對我們的方法進(jìn)行了測試，以驗證其有效性和穩(wěn)健性。在實驗中，一個人在相機的視野范圍內(nèi)進(jìn)行了不規(guī)則的運動。為了驗證算法的穩(wěn)健性，我們評估了兩種相機運動方式：1）從場景的一端到另一端的移動；2）幾乎靜止。多級地圖構(gòu)建的結(jié)果如圖6所示。實驗結(jié)果表明，我們的算法能夠在物體和相機的不同運動狀態(tài)下構(gòu)建準(zhǔn)確的稠密點云地圖、八叉樹地圖、平面地圖和輕量級物體地圖。

圖6. 真實世界場景中的多級地圖構(gòu)建結(jié)果，在上面一組圖像中，相機從場景的一端移動到另一端；在下面一組圖像中，相機幾乎保持靜止。圖像(a)、(b)和(c)分別代表稠密點云地圖、八叉樹地圖和平面地圖。圖像(d)展示了輕量級物體地圖，其中物體被疊加在稠密點云地圖上（圖像(e)），并投影到圖像上（圖像(f)），以展示物體地圖構(gòu)建的有效性。

動態(tài)物體跟蹤實驗?

我們進(jìn)一步將構(gòu)建的物體地圖應(yīng)用于動態(tài)物體跟蹤，使用Pico Neo3設(shè)備捕獲場景圖像，并利用我們的算法構(gòu)建物體地圖。在這種情況下，通過立體匹配獲得地圖點的深度信息，這些計算僅在關(guān)鍵幀上進(jìn)行，以確保實時性能。構(gòu)建的物體地圖如圖7(a)所示。一旦構(gòu)建了物體地圖，用戶可以選擇目標(biāo)物體進(jìn)行跟蹤。當(dāng)用戶移動物體時，系統(tǒng)使用KCF單物體跟蹤和光流跟蹤算法計算物體的實時姿態(tài)。圖7(b)-(d)展示了書、鍵盤和瓶子的動態(tài)跟蹤結(jié)果。實驗結(jié)果表明，我們的算法能夠在動態(tài)環(huán)境中準(zhǔn)確建模物體，為物體跟蹤提供準(zhǔn)確的物體模型和姿態(tài)，從而對實際應(yīng)用非常有價值。此外，這也突顯出我們的算法不依賴于特定設(shè)備，展示了其穩(wěn)健性和通用性。

圖7. 實際場景中的物體建模和動態(tài)跟蹤。

總結(jié)

本文提出了一個針對動態(tài)場景量身定制的多層地圖構(gòu)建算法。我們成功地構(gòu)建了包含靜態(tài)背景和物體的稠密點云地圖、八叉樹地圖、平面地圖和物體地圖，能夠在動態(tài)干擾存在的情況下實現(xiàn)，這豐富了移動機器人的環(huán)境感知能力，并擴展了在動態(tài)環(huán)境中構(gòu)建地圖的應(yīng)用場景。廣泛的實驗證明了我們算法的準(zhǔn)確性和穩(wěn)健性，而動態(tài)物體跟蹤實驗進(jìn)一步證實了其實用性，在未來，我們計劃考慮除人類以外的其他可移動物體的真實運動，并利用平面和物體作為地標(biāo)來優(yōu)化相機姿態(tài)，進(jìn)一步提高定位精度。

編輯：黃飛