作者：Xinggang Hu

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

主要貢獻(xiàn)

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

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

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

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

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

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

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

內(nèi)容概述

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

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

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

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

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

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

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

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

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

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

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

實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

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

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

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

動(dòng)態(tài)物體跟蹤實(shí)驗(yàn)?

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

圖7. 實(shí)際場(chǎng)景中的物體建模和動(dòng)態(tài)跟蹤。

總結(jié)

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

編輯：黃飛