劉家麒 高永彬 姜曉燕 方志軍

摘 要：針對傳統(tǒng)視覺SLAM在動態(tài)場景下容易出現(xiàn)特征匹配錯誤從而導(dǎo)致定位精度下降的問題，提出了一種基于動態(tài)物體跟蹤的語義SLAM算法?；诮?jīng)典的視覺SLAM框架，提取動態(tài)物體進(jìn)行幀間跟蹤，并利用動態(tài)物體的位姿信息來輔助相機(jī)自身的定位。首先，算法在數(shù)據(jù)預(yù)處理中使用YOLACT、RAFT以及SCDepth網(wǎng)絡(luò)，分別提取圖像中的語義掩膜、光流向量以及像素深度值。其次，視覺前端模塊根據(jù)所提信息，通過語義分割掩膜、運(yùn)動一致性檢驗以及遮擋點(diǎn)檢驗算法計算概率圖以平滑區(qū)分場景中的動態(tài)特征與靜態(tài)特征。然后，后端中的捆集調(diào)整模塊融合了物體運(yùn)動的多特征約束以提高算法在動態(tài)場景中的位姿估計性能。最后，在KITTI和OMD數(shù)據(jù)集的動態(tài)場景中進(jìn)行對比驗證。實(shí)驗表明，所提算法能夠準(zhǔn)確地跟蹤動態(tài)物體，在室內(nèi)外動態(tài)場景中具備魯棒、良好的定位性能。

關(guān)鍵詞：視覺SLAM；語義信息；動態(tài)物體跟蹤；捆集調(diào)整

中圖分類號：TP242?? 文獻(xiàn)標(biāo)志碼：A??? 文章編號：1001-3695（2023）12-048-3821-07

doi： 10.19734/j.issn.1001-3695.2023.03.0147

Semantic SLAM based on dynamic object tracking

Abstract：This paper proposed a semantic SLAM algorithm based on dynamic object tracking to address the issue of decreased localization accuracy in traditional visual SLAM methods due to feature matching errors in dynamic scenes. Based on the classic visual SLAM framework， The algorithm extracted dynamic objects for interframe tracking and utilized their pose information to assist the cameras own localization. Firstly， it employed? YOLACT， RAFT， and SCDepth networks in the data preprocessing stage to extract semantic masks， optical flow vectors， and pixel depths from the images. Subsequently， the visual frontend module utilized the extracted information to compute probability maps， employing semantic segmentation masks， motion consistency checks， and occlusion point verification algorithms. These probability maps aided in effectively distinguishing between dynamic and static features in the scene. Then， the bundle adjustment module in the backend integrated multiple feature constraints derived from object motion to enhance the algorithms pose estimation performance in dynamic scenes. Finally， comprehensive comparisons and validations were conducted on the dynamic scenes of the KITTI and OMD datasets. The experimental results demonstrate that the proposed algorithm accurately tracks dynamic objects and exhibits robust and accurate localization performance in both indoor and outdoor dynamic scenes.

Key words：visual SLAM; semantic information; dynamic object tracking; bundle adjustment

0 引言

同步定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）［1］技術(shù)廣泛應(yīng)用于機(jī)器人自主導(dǎo)航、自動駕駛和環(huán)境感知等領(lǐng)域。這種技術(shù)使載體在未知的場景中逐步構(gòu)建周圍環(huán)境地圖，并基于構(gòu)建的地圖對載體自身進(jìn)行定位。傳統(tǒng)的視覺SLAM［2］基于幾何特征的幀間匹配來進(jìn)行相機(jī)位姿估計，盡管這種方法具有實(shí)時性高、可擴(kuò)展性強(qiáng)以及成本較低的優(yōu)勢，但易受到光照變化、紋理缺失、抖動等影響，從而產(chǎn)生錯誤的特征匹配并導(dǎo)致定位精度下降。為了提高視覺SLAM［3］的定位精度與魯棒性，近年來許多研究人員將語義信息融合至視覺SLAM中，并提出了基于語義信息的高級特征，使算法更能像人類一樣理解周圍環(huán)境，增強(qiáng)算法在復(fù)雜場景下的性能。例如，在室外場景中，語義SLAM［4］可將環(huán)境中的汽車、路牌、紅綠燈等物體識別為語義特征，并構(gòu)建出物體級地標(biāo)來優(yōu)化相機(jī)位姿軌跡。因此，研究與語義信息相結(jié)合的SLAM技術(shù)具有重大的理論意義與實(shí)際價值。

使用語義信息解決動態(tài)場景的位姿估計問題是語義SLAM研究的熱點(diǎn)。Zhong等人［5］設(shè)計了運(yùn)動概率更新方程和運(yùn)動概率傳播方式，可將動態(tài)點(diǎn)和靜態(tài)點(diǎn)的運(yùn)動概率輻射到周圍區(qū)域，并在跟蹤時僅利用靜態(tài)點(diǎn)。Yu等人［6］提出了DSSLAM，該算法采用語義分割網(wǎng)絡(luò)和運(yùn)動一致性檢測剔除場景中的動態(tài)物體。Bescos等人［7］提出的DynaSLAM基于特征投影的深度閾值判斷是否為動態(tài)特征，并根據(jù)特征投影恢復(fù)被動態(tài)物體遮擋的背景。大多數(shù)動態(tài)SLAM算法都采用直接剔除潛在動態(tài)點(diǎn)的方法，然而當(dāng)圖像中潛在的動態(tài)物體較多時，特征點(diǎn)幾乎會被全部剔除，從而導(dǎo)致跟蹤丟失。基于此，Brasch等人［8］提出了地圖點(diǎn)靜態(tài)率的概念，每一幀新的觀測數(shù)據(jù)都會更新地圖點(diǎn)的靜態(tài)率，實(shí)現(xiàn)地圖點(diǎn)由靜態(tài)轉(zhuǎn)為動態(tài)的平滑過渡。An等人［9］基于語義信息的輔助并計算相鄰幀間物體的投影誤差，該誤差作為RANSAC［10］特征選擇的權(quán)重，由于動態(tài)點(diǎn)誤差大而靜態(tài)點(diǎn)誤差小，所以更容易選擇靜態(tài)點(diǎn)。Ballester等人［11］根據(jù)實(shí)例分割網(wǎng)絡(luò)以及多視圖幾何生成動態(tài)物體的語義掩膜，并根據(jù)相機(jī)位姿估計動態(tài)物體的運(yùn)動，能夠有效地分辨場景中真正運(yùn)動的物體。

語義SLAM算法的一個重要挑戰(zhàn)是如何處理動態(tài)區(qū)域，盡管更好地識別出靜態(tài)特征是一種可行的方法，但在位姿跟蹤以及建圖階段仍舊忽略了動態(tài)特征。因此，當(dāng)前的研究方向之一是探索如何在整個SLAM系統(tǒng)中同時利用靜態(tài)和動態(tài)特征的信息。Li等人［12］提出了一種基于雙目相機(jī)的語義物體跟蹤SLAM算法，根據(jù)目標(biāo)檢測網(wǎng)絡(luò)輸出的2D檢測框和視角分類器構(gòu)建三維物體包圍框，并能夠輸出物體的重投影輪廓和遮擋掩膜，用于提取物體特征。語義信息與特征觀測通過緊耦合的方式進(jìn)行BA優(yōu)化，以實(shí)現(xiàn)語義物體的幀間跟蹤。Zhang等人［13］提出了融合Mask RCNN［14］目標(biāo)檢測、PWCNet［15］光流檢測以及MonoDepth2［16］深度估計網(wǎng)絡(luò)的VDOSLAM，該算法的前端部分利用語義信息實(shí)現(xiàn)對場景中動態(tài)剛性物體的準(zhǔn)確運(yùn)動估計與跟蹤，而無須物體的形狀、尺寸、位姿或幾何模型等先驗知識，算法的后端部分構(gòu)建了圖優(yōu)化，包含了經(jīng)典的空間點(diǎn)重投影誤差、相機(jī)位姿觀測誤差以及動態(tài)物體運(yùn)動誤差?；谡Z義信息構(gòu)建的物體級特征，在動態(tài)環(huán)境下可實(shí)現(xiàn)強(qiáng)大的位姿估計性能。Qiu等人［17］提出了動態(tài)物體感知系統(tǒng)AirDOS，基于剛性和運(yùn)動約束模擬關(guān)節(jié)式物體，通過將相機(jī)位姿、物體運(yùn)動以及物體的3D結(jié)構(gòu)進(jìn)行聯(lián)合優(yōu)化，以修正相機(jī)位姿估計，并為動態(tài)場景與靜態(tài)場景構(gòu)建4D地圖。雖然在視覺SLAM中，場景剛性假設(shè)較為普遍，但這也限制了它們在復(fù)雜現(xiàn)實(shí)世界中的普適性。Bescos等人［18］在DynaSLAM的基礎(chǔ)上提出了一種新的動態(tài)物體特征跟蹤方法，該方法利用2D實(shí)例匹配進(jìn)行引導(dǎo)，可以同時跟蹤多個動態(tài)物體，并將它們與相機(jī)位姿和空間點(diǎn)進(jìn)行聯(lián)合優(yōu)化。研究結(jié)果表明，跟蹤動態(tài)物體不僅有助于SLAM系統(tǒng)對環(huán)境的理解，還可以提高相機(jī)跟蹤性能。

針對大多數(shù)視覺SLAM普遍基于靜態(tài)世界與剛性場景假設(shè)的限制，本文提出了一種基于動態(tài)物體跟蹤的語義SLAM算法，下文簡述為DTSLAM。其主要工作如下：a）采用YOLACT［19］目標(biāo)檢測網(wǎng)絡(luò)、RAFT［20］光流估計網(wǎng)絡(luò)以及SCDepth［21］深度估計網(wǎng)絡(luò)進(jìn)行數(shù)據(jù)預(yù)處理以獲取圖像中的語義掩膜、光流向量以及像素深度，并根據(jù)語義掩膜、運(yùn)動一致性檢驗以及遮擋點(diǎn)檢驗算法計算動態(tài)概率圖，動態(tài)概率圖能夠平滑區(qū)分場景中的動態(tài)區(qū)域與靜態(tài)區(qū)域，其中靜態(tài)特征用于相機(jī)位姿估計，動態(tài)特征用于動態(tài)物體跟蹤；b）動態(tài)物體的2D檢測框由目標(biāo)檢測網(wǎng)絡(luò)輸出，將2D檢測框的頂點(diǎn)逆投影到空間中并根據(jù)物體的運(yùn)動預(yù)測下一時刻的位置，再將頂點(diǎn)投影到下一幀的像素坐標(biāo)系中，與對應(yīng)的檢測框形成重投影誤差。本文將物體檢測框的投影誤差納入了圖優(yōu)化，通過構(gòu)建多特征約束的因子圖優(yōu)化，達(dá)到減少相機(jī)位姿漂移的目的。

1 系統(tǒng)框架

圖1為本文算法設(shè)計框架，主要包含數(shù)據(jù)預(yù)處理、前端跟蹤以及后端優(yōu)化三個模塊。DTSLAM在VDOSLAM［13］框架的基礎(chǔ)上進(jìn)行研究，主要面向動態(tài)場景，如街道、高速公路等，跟蹤場景中的人物、車輛等動態(tài)目標(biāo)，并將估計的動態(tài)目標(biāo)位姿加入到后端進(jìn)行聯(lián)合優(yōu)化，以提高算法精度。該算法與VDOSLAM框架的主要區(qū)別與聯(lián)系如下：a）數(shù)據(jù)預(yù)處理模塊用于提取圖像中的語義掩膜、光流向量以及全局一致的像素深度值，其采用的深度學(xué)習(xí)網(wǎng)絡(luò)在效率或精度上優(yōu)于原框架采用的Mask RCNN、PWCNet以及MonoDepth2；b）在前端跟蹤模塊中，靜態(tài)特征用于相機(jī)位姿估計，動態(tài)特征用于物體的位姿跟蹤。原框架僅依賴目標(biāo)檢測的結(jié)果對特征的狀態(tài)進(jìn)行判斷，當(dāng)輸入的圖像或目標(biāo)檢測包含噪聲時，檢測結(jié)果不準(zhǔn)確。同時，僅依賴目標(biāo)檢測的結(jié)果會將潛在運(yùn)動區(qū)域全部剔除，可能導(dǎo)致特征點(diǎn)數(shù)量較少而跟蹤失敗。本文算法在目標(biāo)檢測結(jié)果的基礎(chǔ)上，加入了運(yùn)動一致性檢驗以及遮擋點(diǎn)檢驗算法計算動態(tài)概率圖，并對每種檢測結(jié)果賦予不同的權(quán)重，實(shí)現(xiàn)特征點(diǎn)動態(tài)率的精確判斷；c）考慮到動態(tài)物體在圖像中所占區(qū)域較小，特征點(diǎn)法的稀疏性不足以較好地跟蹤動態(tài)物體，因此本文采用光流法跟蹤特征點(diǎn)，在概率圖的基礎(chǔ)上針對動態(tài)區(qū)域采用稠密光流，針對靜態(tài)區(qū)域采用稀疏光流，通過區(qū)分不同區(qū)域的特征跟蹤數(shù)量，減少算法的計算開銷；d）在動態(tài)物體跟蹤時，首先根據(jù)概率圖中的動態(tài)率判斷是否為動態(tài)物體，若動態(tài)率滿足設(shè)定的閾值條件，則直接判定為動態(tài)物體，不再計算場景流，從而減少計算量；e）在后端優(yōu)化模塊中，原框架包含了經(jīng)典的空間點(diǎn)重投影誤差、相機(jī)位姿觀測誤差以及動態(tài)物體運(yùn)動誤差，本文算法在此基礎(chǔ)上加入了動態(tài)物體檢測框的投影誤差，能夠提高系統(tǒng)的魯棒性并減小動態(tài)場景造成的軌跡漂移問題。

2 算法實(shí)現(xiàn)

2.1 動態(tài)概率更新

視覺SLAM通?；趥鹘y(tǒng)的靜態(tài)世界假設(shè)，當(dāng)面對動態(tài)場景時，將造成錯誤的數(shù)據(jù)關(guān)聯(lián)并導(dǎo)致定位精度下降。因此，主流的做法是在特征提取階段保留靜態(tài)區(qū)域特征，并剔除動態(tài)區(qū)域特征，如DetectSLAM［5］、DSSLAM［6］、DynaSLAM［7］等。動態(tài)區(qū)域剔除通常基于語義分割網(wǎng)絡(luò)輸出的掩膜區(qū)域來判斷，這樣做的弊端是圖像中靜止?fàn)顟B(tài)的物體也會被當(dāng)做動態(tài)區(qū)域而被剔除，如停在路邊的汽車、躺在地上睡覺的小狗。此外，上述方式對于圖像中動態(tài)區(qū)域占比不高的圖像有可觀的精度提升，但對于動態(tài)區(qū)域占比較高的圖像，存在將特征幾乎全部剔除從而導(dǎo)致跟蹤失敗的情況。同時，直接丟棄動態(tài)特征的方式［22］也使算法損失了這部分有價值的信息。在實(shí)際場景中，物體的運(yùn)動狀態(tài)往往是變化的，針對這些復(fù)雜的情況，本文將特征點(diǎn)賦予動態(tài)概率，并通過后續(xù)源源不斷的觀測數(shù)據(jù)對動態(tài)概率進(jìn)行更新，使提取的特征點(diǎn)可以由靜至動或由動至靜，實(shí)現(xiàn)特征運(yùn)動狀態(tài)的平滑過渡，進(jìn)而更加精確地區(qū)分場景中的動態(tài)區(qū)域與靜態(tài)區(qū)域。為了使動態(tài)特征的識別具有良好的魯棒性，本文基于語義分割掩膜、運(yùn)動一致性約束以及遮擋點(diǎn)檢驗三種方式進(jìn)行動態(tài)概率更新，特征點(diǎn)的動態(tài)概率更新公式如下：

Pt（Xi）=α1Pt－1（Xi）+α2St（xi）+α3Mt（xi）+α4Dt（xi）（1）

其中：xi為圖像上的某個特征點(diǎn)；Xi為該特征點(diǎn)對應(yīng)的地圖點(diǎn)；Pt（Xi）表示當(dāng)前幀Xi地圖點(diǎn)的動態(tài)概率；Pt－1（Xi）表示上一幀Xi地圖點(diǎn)的動態(tài)概率；St（xi）表示xi特征點(diǎn)是否處于語義分割掩膜區(qū)域內(nèi)；Mt（Xi）表示xi特征點(diǎn)的運(yùn)動一致性檢驗結(jié)果；Dt（Xi）表示xi特征點(diǎn)的遮擋檢驗結(jié)果；α1－4表示對應(yīng)的權(quán)重系數(shù)，且滿足α1+α2+α3+α4=1。若滿足St（xi）、Mt（Xi）、Dt（Xi）的檢驗結(jié)果，則取值為1，否則取值為0。

運(yùn)動一致性檢驗Mt（·）基于對極約束判斷圖像中特征點(diǎn)的運(yùn)動狀態(tài)。如圖2所示，It－1和It為兩幀成像平面，Ot－1和Ot分別為對應(yīng)相機(jī)光心，設(shè)有一空間點(diǎn)Tt-1Pt-1i，投影到兩幀成像平面上分別為It-1pt-1i和Itpti?？梢园l(fā)現(xiàn)，Ot－1、Ot以及Pi三點(diǎn)可以構(gòu)成一個平面，稱為極平面，Ot－1Ot稱為基線，該基線與成像平面It－1和It的交點(diǎn)分別為et－1i和eti，稱為極點(diǎn)，極平面與兩幀成像平面的交線lt－1i和lti稱為極線。對極約束的幾何意義是要滿足空間點(diǎn)Tt-1Pt-1i與投影到兩幀的點(diǎn)It-1pt-1iItpti，形成共面的關(guān)系，若為動態(tài)點(diǎn)，則點(diǎn)的移動使其三者無法共面，因此根據(jù)點(diǎn)到極平面的距離即可判斷是否為動態(tài)點(diǎn)。已知一對匹配好的點(diǎn)特征可以構(gòu)成對極約束，根據(jù)上述內(nèi)容可構(gòu)造等式如下：

（Itpti）TFt-1，tIt-1pt-1i=0（2）

其中：Ft－1，t為基礎(chǔ)矩陣，表示兩幀之間的位姿變換關(guān)系，It-1pt-1i與Itpti表示齊次坐標(biāo)。本文通過RANSAC隨機(jī)選取內(nèi)點(diǎn)的方式求解基礎(chǔ)矩陣Ft－1，t，基于該種方式求解的基礎(chǔ)矩陣代表了相機(jī)位姿的初始估計：

Ft－1，t=K－Tt∧t－1，tRt－1，tK－1（3）

其中：K為相機(jī)內(nèi)參，t與R對應(yīng)當(dāng)前幀的旋轉(zhuǎn)平移變換。進(jìn)一步可計算出極線lti：

其中：A，B，C表示極線的方向向量；極線lt－1i的計算方式類似。于是可計算出特征點(diǎn)Itpti到極線lti的距離di為

特征點(diǎn)到極線的距離即特征點(diǎn)與對極約束的滿足程度，距離越小滿足程度越高，因此本文設(shè)定經(jīng)驗閾值，若處于閾值區(qū)間內(nèi)則認(rèn)為滿足運(yùn)動一致性檢驗，Mt（·）函數(shù)返回值為0，若超出閾值區(qū)間范圍則判定為動態(tài)特征點(diǎn)，Mt（·）函數(shù)返回值為1。

Dt（·）函數(shù)基于空間點(diǎn)投影到圖像幀之間的夾角判斷是否為遮擋點(diǎn)，進(jìn)而篩選出這部分點(diǎn)以減少對算法性能的影響。圖3為遮擋點(diǎn)夾角計算示意圖，其中pi為圖像提取的角點(diǎn)，Pi為對應(yīng)的空間點(diǎn)，θ為夾角，對于能夠遮擋背景的物體，在相機(jī)移動過程中往往夾角較大?，F(xiàn)對遮擋點(diǎn)夾角的計算采用公式進(jìn)行一般描述，設(shè)存在兩圖像幀It－1和It，Tt－1和Tt表示兩幀的相機(jī)位姿，圖像幀It－1上通過FAST提取算法提取了角點(diǎn)It-1pt-1i，并且It-1pt-1i與Itpti基于光流跟蹤建立了匹配關(guān)系。已知相機(jī)內(nèi)參為K，根據(jù)逆投影公式可以計算得到角點(diǎn)It-1pt-1i在第t－1幀相機(jī)坐標(biāo)系下的空間點(diǎn)Tt-1Pt-1i：

其中：Tt-1Pt-1i左上角的符號表明了該空間點(diǎn)位于第t－1幀相機(jī)坐標(biāo)系下，It-1zi為空間點(diǎn)的深度。通過同樣的方式可以求解出角點(diǎn)Itpti在第t幀坐標(biāo)系下的空間點(diǎn)TtPti，再根據(jù)本質(zhì)矩陣E可分解出相鄰兩幀之間的旋轉(zhuǎn)R與平移t，接著將空間點(diǎn)TtPti變換至第t－1幀相機(jī)坐標(biāo)系下：

Tt-1Pti=R-1TtPti-R-1t（7）

根據(jù)式（6）（7）求得的空間點(diǎn)坐標(biāo)，構(gòu)建向量λt－1與λt：

最后，根據(jù)反余弦公式即可計算出向量λt－1與λt的夾角，并與系統(tǒng)設(shè)定的閾值相比較，若夾角大于設(shè)定的閾值即可判斷該點(diǎn)為遮擋點(diǎn)，Dt（·）函數(shù)返回值為1，若夾角小于設(shè)定的閾值，Dt（·）返回值為0。

本文基于上述四種函數(shù)更新特征點(diǎn)的動態(tài)概率。為了使算法能夠滿足不同硬件算力的需要，系統(tǒng)為模塊化設(shè)計，可以選擇開啟其中部分模塊，減少模塊的開啟可以有效降低系統(tǒng)的計算復(fù)雜度。具體情況可根據(jù)實(shí)際工程應(yīng)用而定。

2.2 動態(tài)物體跟蹤

針對動態(tài)對象進(jìn)行位姿跟蹤可以減少靜態(tài)特征誤匹配帶來的噪聲，提高算法的魯棒性與性能。得益于實(shí)例分割網(wǎng)絡(luò)輸出的物體掩膜，場景中所有物體的像素區(qū)域都可以直接獲得，但是如果不加分辨地計算所有物體的運(yùn)動軌跡，將會大幅提高算法的復(fù)雜度，減少SLAM的實(shí)時性。于是本文算法將針對場景中運(yùn)動的物體進(jìn)行跟蹤，忽略靜態(tài)物體。物體的狀態(tài)判斷可以根據(jù)概率圖中的概率信息，若概率圖中物體所包含的特征區(qū)域的動態(tài)概率較大（動態(tài)率>0.8），則直接判斷為動態(tài)物體，否則通過本文提出的場景流進(jìn)一步判斷。場景流描述的是地圖點(diǎn)的運(yùn)動方向向量，例如第t幀有一空間點(diǎn)TtPti，則場景流fti表示點(diǎn)TtPti從第t－1幀到t幀在世界坐標(biāo)系下的平移向量：

fti=0Pt-1i-0Pti=0Pt-1i-0TtTtPti（9）

其中：0Tt表示當(dāng)前幀相機(jī)的位姿，式（9）中所有的空間點(diǎn)和相機(jī)位姿都是在世界坐標(biāo)系下。與光流不同的是場景流在理想情況下只由場景中的動態(tài)物體移動才會產(chǎn)生，如果場景中的物體全部為靜止?fàn)顟B(tài)，則fti向量為零向量。然而，真實(shí)世界中采集的數(shù)據(jù)通常受到各種噪聲的影響，并且光流跟蹤可能存在誤差，使得三角化的空間點(diǎn)出現(xiàn)偏移，從而導(dǎo)致計算得到的場景流不能滿足假設(shè)。為了避免噪聲的干擾，本文將計算所有潛在運(yùn)動物體的場景流，細(xì)化到每一個采樣點(diǎn)，一旦出現(xiàn)某個點(diǎn)的場景流大于系統(tǒng)預(yù)先設(shè)定的閾值，則該點(diǎn)被視為動態(tài)點(diǎn)，如果某個物體中有35%的點(diǎn)為動態(tài)點(diǎn)，則該物體被認(rèn)定為動態(tài)物體，否則被認(rèn)定為靜態(tài)物體。上述方式可使算法在每個時刻都評估物體的運(yùn)動狀態(tài)，無論物體是由靜至動還是由動至靜，都能使物體在動態(tài)與靜態(tài)之間平滑過渡。

動態(tài)物體跟蹤是指在連續(xù)幀之間估計物體的位姿和運(yùn)動狀態(tài)。本文采用光流法跟蹤不同幀之間的物體采樣點(diǎn)，并根據(jù)實(shí)例級目標(biāo)分割網(wǎng)絡(luò)輸出語義物體掩膜，對掩膜中每個采樣點(diǎn)都賦予了物體標(biāo)簽，例如人、車、動物等。為了同時跟蹤不同的物體，本文構(gòu)建了一個跟蹤標(biāo)簽集合L，該集合包括了所有能識別的物體類別，并對每幀圖像中的像素點(diǎn)打上了語義標(biāo)簽l∈L，若l=0則表示為背景點(diǎn)或靜態(tài)物體，若為動態(tài)物體，則標(biāo)簽從1開始增加。在理想情況下，對于第t幀檢測到的所有對象，每個對象掩膜中像素點(diǎn)的標(biāo)簽應(yīng)當(dāng)與第t－1幀中的語義標(biāo)簽一致，然而實(shí)際中容易受到圖像邊界、遮擋點(diǎn)以及物體堆疊的干擾，導(dǎo)致語義標(biāo)簽并不一致。為了解決這個問題，本文采取了少數(shù)服從多數(shù)策略，即少數(shù)其他標(biāo)簽應(yīng)與大部分相同的語義標(biāo)簽一致。

其中：李群0tGt-1∈SE（3），李群與對應(yīng)李代數(shù)0tgt-1∈se（3）映射公式為

0tGt-1：=exp（0tgt-1）（11）

其中：ρh為Huber核函數(shù)；ΣP為空間點(diǎn)重投影誤差的協(xié)方差矩陣。本文通過列文伯格—馬夸爾特算法求解上述最小二乘問題，最后基于當(dāng)前幀估計的位姿0Tk即可恢復(fù)動態(tài)物體的運(yùn)動：

0tHt-1=0Tt0tGt-1（13）

2.3 圖優(yōu)化

捆集調(diào)整（BA）能夠優(yōu)化前端里程計估計的相機(jī)位姿，減少系統(tǒng)產(chǎn)生的累積誤差。傳統(tǒng)的視覺SLAM通過構(gòu)建地圖點(diǎn)的重投影誤差進(jìn)行BA優(yōu)化，本文在此基礎(chǔ)上增加了動態(tài)的物體級地標(biāo)，并將動態(tài)物體跟蹤表述為圖優(yōu)化問題以提高相機(jī)軌跡精度以及動態(tài)物體跟蹤精度，同時構(gòu)建包含靜態(tài)結(jié)構(gòu)與動態(tài)結(jié)構(gòu)的全局一致性地圖。

圖優(yōu)化的表述方式較為直觀，可以高效地運(yùn)用批量式［23，24］和增量式［25，26］求解器。因子圖建模示意圖如圖5所示，其中黑色圓形表示不同幀的相機(jī)位姿節(jié)點(diǎn)，綠色圓形表示靜態(tài)點(diǎn)，橙色圓形表示動態(tài)點(diǎn)，紫色圓形表示物體的位姿變換，黃色圓形表示物體檢測框的頂點(diǎn)，藍(lán)色矩形表示先驗因子，黑色矩形表示里程計因子，白色矩形表示空間點(diǎn)觀測因子，橙色矩形表示動態(tài)點(diǎn)運(yùn)動因子，黃色矩形表示頂點(diǎn)運(yùn)動因子，紫色矩形表示平滑運(yùn)動因子（參見電子版）。在本文構(gòu)建的因子圖中，共有六種因子加入聯(lián)合優(yōu)化，為了便于理解，圖中僅繪制了一個動態(tài)物體及其對應(yīng)的動態(tài)點(diǎn)與檢測框頂點(diǎn)。本文將空間點(diǎn)觀測誤差定義為

et（0Tt-1，0Tt）=（（0Tt-1）-10Tt）-1Tt-1tXt-1（15）

動態(tài)物體的2D檢測框由目標(biāo)檢測網(wǎng)絡(luò)輸出，可將2D檢測框的頂點(diǎn)逆投影到空間中，并根據(jù)物體的運(yùn)動預(yù)測下一時刻的位置，再將頂點(diǎn)投影到下一幀的像素坐標(biāo)系中與對應(yīng)的檢測框形成重投影誤差，具體誤差函數(shù)為

本文將物體建模為剛體，即同一個物體上所有特征點(diǎn)的運(yùn)動相同。已知所有的物體都具有慣性，表現(xiàn)為物體對其運(yùn)動狀態(tài)改變的抵抗程度，且慣性的大小與質(zhì)量成正比。因此在圖優(yōu)化中引入平滑運(yùn)動因子，目的是最小化動態(tài)物體在相鄰幀之間的運(yùn)動，誤差項定義為

ev，t（0t-2Ht-1v，0t-1Htv）=（0t-2Ht-1v）-10t-1Htv（18）

由于旋轉(zhuǎn)矩陣對加法不封閉，即兩個旋轉(zhuǎn)矩陣相加不再是旋轉(zhuǎn)矩陣，無法用導(dǎo)數(shù)的定義求導(dǎo)，所以上述公式中，李群需轉(zhuǎn)換為李代數(shù)。例如李群H∈SE（3）的李代數(shù)形式為h∈se（3），對應(yīng)的映射公式為h=ln（H）∨。令θ={0Pti，0tHt-1v，0Tt}，最終構(gòu)建的最小二乘整體目標(biāo)函數(shù)為

其中：Σ為協(xié)方差矩陣，n為各類節(jié)點(diǎn)對應(yīng)的節(jié)點(diǎn)總數(shù)。例如ΣP為空間點(diǎn)投影誤差的協(xié)方差矩陣，nP為空間點(diǎn)總數(shù)，Σc為相機(jī)位姿節(jié)點(diǎn)誤差的協(xié)方差矩陣，nc為相機(jī)位姿節(jié)點(diǎn)總數(shù)，Σb為檢測框頂點(diǎn)投影誤差的協(xié)方差矩陣，nb為頂點(diǎn)總數(shù)，Σg為物體運(yùn)動的協(xié)方差矩陣，ng為物體運(yùn)動節(jié)點(diǎn)總數(shù)，Σs為平滑運(yùn)動因子的協(xié)方差矩陣，ns為平滑因子節(jié)點(diǎn)總數(shù)。列出的非線性最小二乘問題可通過列文伯格—馬夸爾特方法迭代求解。

3 實(shí)驗分析

為了驗證算法在動態(tài)場景下的性能，將本文算法與目前主流的動態(tài)SLAM算法在KITTI跟蹤數(shù)據(jù)集（KITTI tracking dataset）［27］與OMD數(shù)據(jù)集（Oxford multimotion dataset）［28］上進(jìn)行對比，包括動態(tài)物體跟蹤的精度以及相機(jī)自身定位的精度，評估標(biāo)準(zhǔn)［29］包括絕對軌跡誤差（abosolute trajectory error，ATE）與相對位姿誤差（relative pose error，RPE）。KITTI跟蹤數(shù)據(jù)集涵蓋了諸多基于道路、園區(qū)、鄉(xiāng)鎮(zhèn)的動態(tài)場景，主要用于車輛、人物、自行車等物體的動態(tài)跟蹤，數(shù)據(jù)集提供了語義標(biāo)簽，并給出了動態(tài)目標(biāo)在不同時刻下的位姿參考值。OMD數(shù)據(jù)集是由牛津大學(xué)公開發(fā)布的一個多模態(tài)數(shù)據(jù)集，數(shù)據(jù)格式包括雙目RGB圖像、深度圖、IMU慣導(dǎo)數(shù)據(jù)以及相機(jī)位姿的參考值，用于研究機(jī)器人在復(fù)雜環(huán)境中的定位、感知以及控制問題。本文實(shí)驗設(shè)備為Intel Xeon CPU E52678 @ 2.5 GHz，內(nèi)存32 GB以及一塊圖形處理器GTX 1080Ti。為了便于描述和對比，本文將該方法簡述為DTSLAM。

3.1 動態(tài)物體跟蹤評估

KITTI跟蹤數(shù)據(jù)集不僅提供了軌跡的參考值，同時也提供了場景中車輛以及人物的運(yùn)動參考值，及其對應(yīng)的語義標(biāo)簽，每一個動態(tài)物體都有唯一的數(shù)字標(biāo)識，以此檢驗算法對于動態(tài)物體跟蹤的性能。本文選取了動態(tài)物體占比較多的序列，通過評估動態(tài)物體運(yùn)動軌跡的絕對位姿誤差與相對位姿誤差分析跟蹤的準(zhǔn)確性。在選取的序列中，本文選取了部分包含相機(jī)自運(yùn)動以及靜止的序列，分析場景中移動物體的絕對位姿誤差，以此能夠體現(xiàn)本文算法對于不同類型動態(tài)目標(biāo)的總體跟蹤性能。

圖6為目標(biāo)檢測網(wǎng)絡(luò)估計的語義掩膜以及算法計算的動態(tài)概率圖，其中動態(tài)概率圖采用單通道表示，趨近于黑色的像素表示靜態(tài)區(qū)域，趨近于白色像素表示動態(tài)區(qū)域。觀察圖6中左列部分，可見目標(biāo)檢測網(wǎng)絡(luò)正確地估計了圖像中運(yùn)動的車輛，但同時將道路旁的石頭誤判為潛在動態(tài)物體。然而，動態(tài)概率圖中的運(yùn)動一致性檢測等算法能夠?qū)⑹^正確地估計為靜態(tài)物體，具體可以對比車輛與石頭的灰度值。

觀察圖6中右列部分，可見目標(biāo)檢測網(wǎng)絡(luò)并未正確估計圖像中的所有動態(tài)目標(biāo)，例如在遠(yuǎn)處紅綠燈下方的車輛并未被語義掩膜標(biāo)出，然而在動態(tài)概率圖中，該車輛所處的像素被正確估計為動態(tài)區(qū)域?；谏鲜龊Y選和細(xì)化過程，本文算法可以更加準(zhǔn)確地識別出動態(tài)物體，從而為SLAM優(yōu)化提供更精確的初始化。此外，本文基于RAFT稠密光流網(wǎng)絡(luò)跟蹤動態(tài)特征的運(yùn)動，圖7為動態(tài)物體特征匹配示意圖，該圖定性反映了動態(tài)特征點(diǎn)分別在相鄰兩幀的匹配情況，對于相同目標(biāo)的動態(tài)特征匹配較為準(zhǔn)確。

3.2 相機(jī)定位精度評估

3.2.1 KITTI跟蹤序列實(shí)驗分析

為了更精確地評估算法的性能，本文選取了KITTI跟蹤數(shù)據(jù)中軌跡較長的序列，剔除了里程較短以及相機(jī)靜止不動的序列。針對動態(tài)場景，本文首先根據(jù)三種深度學(xué)習(xí)網(wǎng)絡(luò)對圖像進(jìn)行預(yù)處理：基于YOLACT目標(biāo)檢測提取圖像幀的語義標(biāo)簽以及物體掩膜；基于RAFT光流估計網(wǎng)絡(luò)提取前后幀的稠密光流；基于SCDepth單目估計網(wǎng)絡(luò)估計圖像中的深度信息。預(yù)處理后輸出的結(jié)果如圖8所示，從上至下依次為原始圖像、語義分割掩膜圖、光流圖以及深度圖。

本文根據(jù)預(yù)處理的信息更新像素點(diǎn)相應(yīng)的動態(tài)概率，進(jìn)而更精準(zhǔn)地提取場景中的靜態(tài)點(diǎn)。提取的動態(tài)特征與靜特征分布如圖9所示，其中黑色的空心圓圈代表靜態(tài)特征點(diǎn)，而其他顏色的點(diǎn)則代表動態(tài)點(diǎn)，每一種顏色的特征點(diǎn)都代表了一類物體。圖中展示了前后兩幀跟蹤的結(jié)果，反映了本文算法對于動態(tài)車輛跟蹤的魯棒性。此外，圖10定性展示了本文算法在部分KITTI跟蹤序列上的軌跡誤差示例，圖像表明本系統(tǒng)輸出的軌跡與參考值吻合度較高，平均的絕對軌跡誤差低于1 m。

為了定量分析算法的定位性能，將本文算法與經(jīng)典的ORBSLAM2［30］以及主流的基于動態(tài)場景的SLAM算法DynaSLAM Ⅱ和VDOSLAM在KITTI跟蹤數(shù)據(jù)集上進(jìn)行對比。表2列出了本文算法與對比算法的絕對軌跡的均方根誤差，結(jié)果表明，本文算法在所有序列上的精度表現(xiàn)均優(yōu)于ORBSLAM2和DynaSLAM Ⅱ，在02序列上的精度略低于VDOSLAM。表3列出了本文算法與VDOSLAM的相對位姿誤差對比，VDOSLAM在數(shù)據(jù)預(yù)處理時采用了PWCNet光流估計網(wǎng)絡(luò)，該網(wǎng)絡(luò)對于光流估計的性能與本文采用的RAFT相比，存在一定差距。DynaSLAM Ⅱ與VDOSLAM類似，它們在特征點(diǎn)的處理上區(qū)分了動態(tài)特征與靜態(tài)特征，其中靜態(tài)特征用于相機(jī)的位姿估計，動態(tài)特征用于動態(tài)物體的跟蹤，并構(gòu)建約束加入BA中進(jìn)行聯(lián)合優(yōu)化。然而，基于像素級的實(shí)例分割網(wǎng)絡(luò)存在一定誤差，當(dāng)圖像出現(xiàn)模糊、物體堆疊、邊緣不清晰等噪聲時尤為明顯，因此特征點(diǎn)的動靜狀態(tài)分類存在不確定性，該不確定性不僅影響相機(jī)位姿估計，同時影響動態(tài)物體跟蹤的精度。本文在實(shí)例分割的基礎(chǔ)上引入了深度一致性檢測以及遮擋點(diǎn)檢測，并根據(jù)三者的輸出結(jié)果賦予特征點(diǎn)動態(tài)概率，使其在幀與幀之間動態(tài)變化，將特征點(diǎn)的動態(tài)判斷由二分類問題轉(zhuǎn)換為概率問題，以略微增加計算復(fù)雜度的條件下提高系統(tǒng)整體性能。

圖11展示了本文算法與ORBSLAM2在序列02、03以及20上的絕對位姿誤差對比。由于ORBSLAM2并不區(qū)分場景中的動態(tài)特征與靜態(tài)特征，當(dāng)場景中存在較多動態(tài)物體時會嚴(yán)重影響相機(jī)位姿的求解，從而產(chǎn)生較大的漂移。

3.2.2 OMD數(shù)據(jù)集實(shí)驗分析

OMD數(shù)據(jù)集中的動態(tài)物體在圖像中占據(jù)較大區(qū)域，這對SLAM算法構(gòu)建穩(wěn)定的地圖和跟蹤相機(jī)位姿提出了更高要求。如圖12為OMD數(shù)據(jù)集中不同序列的原始數(shù)據(jù)，其中包含物體搖擺、遮擋以及多物體快速運(yùn)動等場景，在每個場景中分別采集了載體在靜止、平移，以及平移和旋轉(zhuǎn)同時進(jìn)行狀態(tài)下的數(shù)據(jù)序列，本文在物體遮擋以及遙控車快速運(yùn)動的序列中選取了前300幀圖像，并且選取的場景全部處于載體不受約束的狀態(tài)，即載體既可以平移也可以旋轉(zhuǎn)，以充分驗證算法的性能。

本文將DTSLAM在OMD數(shù)據(jù)集上與ORBSLAM2、MVO［31］、ClusterVO［32］、DynaSLAM以及VDOSLAM進(jìn)行比較驗證。表4列出了不同算法的相機(jī)絕對位姿誤差對比，其中每一項的誤差為均方根誤差。MVO基于經(jīng)典的雙目里程計框架，根據(jù)光流與場景流對場景中的多個動態(tài)物體進(jìn)行位姿跟蹤，而本文算法基于RAFT稠密光流網(wǎng)絡(luò)以及數(shù)據(jù)集中提供的深度值計算光流向量與場景流，精度相比MVO有大幅提升。ClusterVO針對動態(tài)物體特征進(jìn)行多層概率關(guān)聯(lián)，以提高動態(tài)特征的匹配精度，并提出了一種融合空間信息、運(yùn)動信息以及語義信息的異構(gòu)條件隨機(jī)場，而本文算法提出了動態(tài)概率圖以更精確地區(qū)分動態(tài)特征與靜態(tài)特征，精度相比ClusterVO有較大提升。DynaSLAM Ⅱ構(gòu)建了動態(tài)物體、特征點(diǎn)以及相機(jī)位姿緊耦合的BA優(yōu)化方法，然而基于特征的視覺前端部分難以提取準(zhǔn)確的三維物體框，并且在面對低紋理的場景時將影響性能。

表5以及圖13為本文算法與VDOSLAM在部分OMD序列上的絕對位姿誤差對比；第一行展示了算法誤差的宏觀對比；第二行表明本文算法的誤差范圍更小且更加集中，體現(xiàn)了其穩(wěn)定性較高；第三行顯示了絕對位姿誤差的各項指標(biāo)對比。綜上所述，本文算法在動態(tài)場景中的位姿估計性能優(yōu)于其他對比算法，同時體現(xiàn)了本文設(shè)計方案的可行性和有效性。

4 結(jié)束語

本文提出了基于動態(tài)物體跟蹤的語義SLAM算法，該算法引入了YOLACT、RAFT、SCDepth深度學(xué)習(xí)網(wǎng)絡(luò)，分別提取語義信息、稠密光流以及像素深度值，基于以上三種先驗信息構(gòu)建了動態(tài)概率圖使特征點(diǎn)在靜態(tài)與動態(tài)之間的平滑過渡。同時，本文利用稠密光流對動態(tài)物體進(jìn)行跟蹤，計算出每幀動態(tài)物體在相機(jī)坐標(biāo)系下的位姿。在后端優(yōu)化中，本文構(gòu)建了圖優(yōu)化，優(yōu)化的變量包括相機(jī)位姿、動態(tài)物體位姿以及空間點(diǎn)的位置，通過迭代調(diào)整以上變量能夠最小化系統(tǒng)輸出的位姿誤差。在公開數(shù)據(jù)集上的實(shí)驗結(jié)果表明，本文在動態(tài)物體跟蹤以及系統(tǒng)定位精度上的平均誤差均優(yōu)于目前主流的動態(tài)語義SLAM算法，在動態(tài)場景中具備魯棒性，性能表現(xiàn)良好。

參考文獻(xiàn)：

［1］DurrantWhyte H，Bailey T. Simultaneous localization and mapping： part I［J］. IEEE Robotics & Automation Magazine，2006，13（2）： 99-110.

［2］吳凡，宗艷桃，湯霞清. 視覺SLAM的研究現(xiàn)狀與展望［J］. 計算機(jī)應(yīng)用研究，2020，37（8）：2248-2254. （Wu Fan，Zong Yantao，Tang Xiaqing. Research status and prospect of visual SLAM ［J］. Application Research of Computers，2020，37（8）：2248-2254. ）

［3］王霞，左一凡. 視覺SLAM研究進(jìn)展［J］. 智能系統(tǒng)學(xué)報，2020，15（5）：825-834. （Wang Xia，Zuo Yifan. Research progress in visual SLAM ［J］. CAAI Trans on Intelligent Systems，2020，15（5）：825-834. ）

［4］Chen Kaiqi，Zhang Jianhua，Liu Jialing，et al. Semantic visual simultaneous localization and mapping： a survey［EB/OL］. （2022）. https：//arxiv.org/abs/2209.06428.

［5］Zhong Fangwei，Wang Sheng，Zhang Ziqi，et al. DetectSLAM： making object detection and SLAM mutually beneficial ［C］// Proc of IEEE Winter Conference on Applications of Computer Vision. Piscataway，NJ： IEEE Press，2018： 1001-1010.

［6］Yu Chao，Liu Zuxin，Liu Xinjun，et al. DSSLAM： a semantic visual SLAM towards dynamic environments ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2018： 1168-1174.

［7］Bescos B，F(xiàn)ácil J M，Civera J，et al. DynaSLAM： tracking，mapping，and inpainting in dynamic scenes［J］. IEEE Robotics and Automation Letters，2018，3（4）： 4076-4083.

［8］Brasch N，Bozic A，Lallemand J，et al. Semantic monocular SLAM for highly dynamic environments ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2018： 393-400.

［9］An Lifeng，Zhang Xinyu，Gao Hongbo，et al. Semantic segmentationaided visual odometry for urban autonomous driving［J］. International Journal of Advanced Robotic Systems，2017，14（5）：172988141773566.

［10］Fischler M A，Bolles R C. Random sample consensus： a paradigm for model fitting with applications to image analysis and automated cartography［J］. Communications of the ACM，1981，24（6）： 381-395.

［11］Ballester I，F(xiàn)ontán A，Civera J，et al. DOT： dynamic object tracking for visual SLAM ［C］// Proc of IEEE International Conference on Robotics and Automatio. Piscataway，NJ： IEEE Press，2021： 11705-11711.

［12］Li Peiliang，Qin Tong. Stereo visionbased semantic 3D object and egomotion tracking for autonomous driving ［C］// Proc of European Conference on Computer Vision. Berlin： Springer，2018： 646-661.

［13］Zhang Jun，Henein M，Mahony R，et al. VDOSLAM： a visual dynamic objectaware SLAM system［EB/OL］. （2020）. https：//arxiv.org/abs/ 2005.11052.

［14］He Kaiming，Gkioxari G，Dollár P，et al. Mask RCNN ［C］// Proc of IEEE International Conference on Computer Vision. Piscataway，NJ： IEEE Press，2017： 2961-2969.

［15］Sun Deqing，Yang Xiaodong，Liu Mingyu，et al. PWCNet： CNNs for optical flow using pyramid，warping，and cost volume ［C］// Proc of IEEE Conference on Computer Vision and Pattern Recognition. Piscataway，NJ： IEEE Press，2018： 8934-8943.

［16］Godard C，Mac Aodha O，F(xiàn)irman M，et al. Digging into selfsupervised monocular depth estimation ［C］// Proc of the IEEE/CVF International Conference on Computer Vision. Piscataway，NJ： IEEE Press，2019： 3828-3838.

［17］Qiu Yuheng，Wang Chen，Wang Wenshan，et al. AirDOS： dynamic SLAM benefits from articulated objects ［C］// Proc of International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2022： 8047-8053.

［18］Bescos B，Campos C，Tardós J D，et al. DynaSLAM Ⅱ： tightlycoupled multiobject tracking and SLAM［J］. IEEE Robotics and Automation Letters，2021，6（3）： 5191-5198.

［19］Bolya D，Zhou Chong，Xiao Fanyi，et al. YOLACT： realtime instance segmentation ［C］// Proc of IEEE/CVF International Conference on Computer Vision. Piscataway，NJ： IEEE Press，2019： 9157-9166.

［20］Teed Z，Deng Jia. RAFT： recurrent allpairs field transforms for optical flow ［C］// Proc of the 16th European Conference. Berlin： Springer，2020： 402-419.

［21］Sun Libo，Bian Jiawang，Zhan Huangying，et al. SCDepthV3： robust selfsupervised monocular depth estimation for dynamic scenes［EB/OL］. （2022）. https：//arxiv.org/abs/ 2211.03660.

［22］Cheng Jiyu，Zhang Hong，Meng Qinghu. Improving visual localization accuracy in dynamic environments based on dynamic region removal［J］. IEEE Trans on Automation Science and Engineering，2020，17（3）： 1585-1596.

［23］Dellaert F，Kaess M. Square root SAM： simultaneous localization and mapping via square root information smoothing［J］.The International Journal of Robotics Research，2006，25（12）： 1181-1203.

［24］Agarwal S，Mierle K. Ceres solver： tutorial & reference［EB/OL］. （2012）. http：//ceressolver.org/tutorial.html.

［25］Polok L，Ila V，Solony M，et al. Incremental block cholesky factorization for nonlinear least squares problems in robotics ［C］// Proc of IFAC Symposium on Intelligent Autonomous Vehicles. 2013： 328-336.

［26］Kaess M，Johannsson H，Roberts R，et al. iSAM2： incremental smoothing and mapping using the Bayes tree［J］. The International Journal of Robotics Research，2012，31（2）： 216-235.

［27］Geiger A，Lenz P，Stiller C，et al. Vision meets robotics： the KITTI dataset［J］. The International Journal of Robotics Research，2013，32（11）： 1231-1237.

［28］Judd K M，Gammell J D. The Oxford multimotion dataset： multiple SE （3） motions with ground truth［J］. IEEE Robotics and Automation Letters，2019，4（2）： 800-807.

［29］Sturm J，Engelhard N，Endres F，et al. A benchmark for the evaluation of RGBD SLAM systems ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2012： 573-580.

［30］MurArtal R，Tardós J D. ORBSLAM2： an opensource SLAM system for monocular，stereo，and RGBD cameras［J］. IEEE Trans on Robotics，2017，33（5）： 1255-1262.

［31］Judd K M，Gammell J D，Newman P. Multimotion visual odometry （MVO）： simultaneous estimation of camera and thirdparty motions ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2018： 3949-3956.

［32］Huang Jiahui，Yang Sheng，Mu Taijiang，et al. ClusterVO： clustering moving instances and estimating visual odometry for self and surroundings ［C］// Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway，NJ： IEEE Press，2020： 2168-2177.