基于目標(biāo)檢測和動靜點分離的視覺即時定位與地圖構(gòu)建技術(shù)

□ 來金強

寧波大學(xué) 機械工程與力學(xué)學(xué)院 浙江寧波 315211

1 研究背景

對于視覺即時定位與地圖構(gòu)建而言,真實環(huán)境往往存在復(fù)雜的動態(tài)現(xiàn)象,而很多開源方案都假設(shè)相機在一個理想的靜態(tài)環(huán)境中運動。為了減少環(huán)境中動態(tài)信息的干擾,一般采用幾何約束設(shè)置閾值,或使用光流法等。Wang Runzhi等[1]運用極線幾何約束尋找外點,應(yīng)用聚類信息進一步分離動態(tài)物體,但這一方法在高動態(tài)環(huán)境中魯棒性丟失較為嚴(yán)重。艾青林等[2]提出一種改進的幾何和運動約束方法,但在數(shù)據(jù)集部分序列相對于靜態(tài)即時定位與地圖構(gòu)建,精度有所下降。

目前,產(chǎn)生了很多與深度學(xué)習(xí)相結(jié)合的技術(shù)方案。Bescos等[3]提出DynaSLAM,使用Mask-區(qū)域卷積神經(jīng)網(wǎng)絡(luò)[4]進行語義分割,同時使用多視圖幾何方法判斷外點,但計算量大,且實時性差。Palazzolo等[5]提出ReFusion方案,在KinectFusion[6]稠密系統(tǒng)的基礎(chǔ)上,使用純幾何方法分割動態(tài)區(qū)域,通過自適應(yīng)閾值和形態(tài)學(xué)處理得到最終動態(tài)區(qū)域,但實時性下降嚴(yán)重。

針對動態(tài)場景下的視覺即時定位與地圖構(gòu)建魯棒性丟失,采用語義分割實時性差等問題,筆者提出一種基于目標(biāo)檢測和動靜點分離的視覺即時定位與地圖構(gòu)建技術(shù)。這一技術(shù)使用一階段算法YOLO v5將圖像分為框內(nèi)框外區(qū)域,通過觀測點的運動分離出可用靜點,將特征點分類為框外靜點、框內(nèi)可用靜點、動點,并一起納入位姿優(yōu)化。

2 視覺即時定位與地圖構(gòu)建系統(tǒng)

2.1 系統(tǒng)框架

筆者所提出的系統(tǒng)框架是在ORB-SLAM2[7]的基礎(chǔ)上新增三個部分,如圖1所示。

在圖像輸入前端增加YOLO v5的語義檢測線程,在跟蹤線程中對優(yōu)化相機位姿的光束法平差進行了改進,使用濾除動態(tài)物的關(guān)鍵幀信息進行稠密建圖。

在語義檢測線程中,YOLO v5對系統(tǒng)輸入的圖像進行檢測,將檢測結(jié)果,即帶檢測框的圖像返回至跟蹤線程。檢測結(jié)果用于區(qū)域劃分。在室內(nèi)環(huán)境中,一般只存在人這個動態(tài)物,因此只將人視為動態(tài)目標(biāo)物。

在改進光束法平差優(yōu)化中,由于YOLO v5的檢測框內(nèi)除了動態(tài)目標(biāo)物外還存在部分靜態(tài)物體和背景區(qū)域,因此為了盡可能利用已知信息,通過運動約束的方法對框內(nèi)區(qū)域進行動靜點分離,并對不同類型的點進行權(quán)重分配。

稠密建圖線程將舍棄檢測框區(qū)域內(nèi)的點云信息顯示,并使用優(yōu)化后的位姿進行點云拼接和地圖重建。

2.2 目標(biāo)檢測和區(qū)域劃分

YOLO v5是一種實時性高的輕量型網(wǎng)絡(luò),使用較高性能的顯卡可以在計算機端達到每秒30幀圖像的處理效率,符合即時定位與地圖構(gòu)建系統(tǒng)對實時性的要求,而語義分割算法,如DynaSLAM使用的Mask-區(qū)域卷積神經(jīng)網(wǎng)絡(luò),在計算機端的速度僅為10秒一幀圖像。

語義檢測線程以多線程并行的方式,對輸入的圖像進行動態(tài)目標(biāo)物檢測,并將檢測結(jié)果返回至ORB-SLAM2的跟蹤線程,包括檢測框的類別、寬度w、高度h,以及中心坐標(biāo)值(px,py)。

通過檢測框的四個邊界坐標(biāo),將圖像劃分為框內(nèi)區(qū)域和框外區(qū)域。這四個坐標(biāo)分別為(px±0.5w,py±0.5h)。對于框外區(qū)域的特征點,默認(rèn)為靜點。

2.3 隨機抽樣一致性算法

當(dāng)動態(tài)目標(biāo)物所占的圖像區(qū)域較小或者圖像模糊時,YOLO v5存在檢測失敗的可能,導(dǎo)致部分人身上的動點誤判為靜點。對此,對檢測框外區(qū)域的特征點采取隨機抽樣一致性算法。首先選取八個分散的框外特征點,使用八點法估計基礎(chǔ)矩陣F,再對其余點進行對極約束。

對極約束原理如圖2所示,點O1、O2為相機光心,點P是物體上某個特征點,在I1、I2時刻的圖像投影分別為P1、P2。面O1O2P與I1和I2的交線l1、l2記為極線,l1可表示為:

(1)

式中:x1為P1的歸一化坐標(biāo)。

若點P2到l2的距離D可表示為:

(2)

式中:x2為P2的歸一化坐標(biāo)。

由于計算所得的基礎(chǔ)矩陣有一定誤差,因此設(shè)定閾值ε,當(dāng)D大于ε時,判斷點P為動點。迭代k次之后,若動點數(shù)量占比小于5%,則停止。

2.4 運動規(guī)律和動靜點分離

對于框內(nèi)區(qū)域的特征點,根據(jù)靜點和動點運動規(guī)律的不同來區(qū)分。靜點在任意兩個時刻的速度變化差值和速度之和都接近0,動點在短時間內(nèi)速度變化差的絕對值很小,遠小于前后兩時刻速度之和的絕對值。由此建立一個觀測模型,并計算t時刻特征點的運動速度Vt為:

(3)

(4)

式中:Tcw_t為t時刻世界坐標(biāo)系到相機坐標(biāo)系的變換矩陣;K為相機內(nèi)參矩陣;Pt為t時刻的投影坐標(biāo)。

由上述運動規(guī)律,建立動點的分離條件,為:

‖Vt+1-Vt‖1<λ‖Vt+1+Vt‖1

(5)

式中:λ為工程經(jīng)驗值,需要大于10。

對于框內(nèi)區(qū)域滿足式(5)的點,視為動點,剩余的點視為可用的靜點?？捎玫撵o點指在一段時間內(nèi)沒有體現(xiàn)出動點規(guī)律而被標(biāo)記的點,若發(fā)生了運動現(xiàn)象,則需要標(biāo)記為動點。因此,需要對框內(nèi)的點持續(xù)觀測計算,dt取前后各五個關(guān)鍵幀的時間差。

在TUM數(shù)據(jù)集[8]上做點的分離測試,特征點分離和標(biāo)記如圖3所示?？蛲馐褂秒S機抽樣一致性算法保留后的點標(biāo)記為白色,框內(nèi)區(qū)域的可用靜點標(biāo)記為灰色,動點則標(biāo)記為黑色?？梢钥闯?這一策略能有效區(qū)分各類特征點。

2.5 改進光束法平差優(yōu)化

ORB-SLAM2跟蹤線程的光束法平差目標(biāo)函數(shù)是最小二乘形式,為:

(6)

式中:ρ為魯棒核函數(shù);{R,t}為相機位姿變換矩陣;R為相機旋轉(zhuǎn)矩陣;t為相機平移向量;xi為點的像素坐標(biāo)系坐標(biāo);Xi為點的世界坐標(biāo)系坐標(biāo);π()函數(shù)用于將相機坐標(biāo)系坐標(biāo)轉(zhuǎn)為像素坐標(biāo)系坐標(biāo);E為三階信息矩陣。

E用于引導(dǎo)誤差項的求解方向,可表示為:

(7)

式中:n為圖像金字塔層數(shù)。

在原目標(biāo)函數(shù)中加入可用靜點的誤差項,并在該誤差項的信息矩陣上乘以因數(shù),以降低權(quán)重分配。對于誤差項,需要使用高斯-牛頓迭代法不斷迭代求解,于是有:

(8)

Eη=ηE

(9)

式中:xj為檢測框內(nèi)保留的可用靜點的像素坐標(biāo)系坐標(biāo);Xj為檢測框內(nèi)保留的可用靜點的世界坐標(biāo)系坐標(biāo);Eη為保留可用靜點的信息矩陣;η為權(quán)重因數(shù),默認(rèn)小于1。

3 性能測試

3.1 數(shù)據(jù)集

在公開的數(shù)據(jù)集TUM RGB-D中測試算法性能。該數(shù)據(jù)集有專門測試動態(tài)的序列Dynamic Objects,其中的fr3序列是室內(nèi)動態(tài)場景,序列中的前綴sitting和walking表示場景中人物的運動形式,后綴halfsphere、rpy等表示相機的主要運動路徑。數(shù)據(jù)集自帶真實的軌跡文件groundtruth,可以通過兩個指標(biāo)對算法進行評估,分別為絕對軌跡誤差和相對位姿誤差。

3.2 測試結(jié)果

筆者方法和ORB-SLAM2對于序列sitting_half和walking_half的絕對軌跡誤差如圖4、圖5所示。由圖4可知,筆者方法在低動態(tài)的sitting_half序列和高動態(tài)的walking_half序列得到的軌跡均優(yōu)于ORB-SLAM2。walking_half序列中,ORB-SLAM2的軌跡已經(jīng)嚴(yán)重偏移,基本喪失了魯棒性。

為進一步驗證筆者方法的魯棒性和精度,對fr3所有序列進行測試,每一個序列測試14次,去除精度最高和最低的四個序列后,記錄絕對軌跡誤差的均方根值,并與其它算法、方法對比,見表1。

表1 絕對軌跡誤差均方根值對比 m

從表1中數(shù)據(jù)可以看出,筆者方法的優(yōu)勝序列較其它算法、方法更多,且所有序列均保持較高的精度。

與ORB-SLAM2+YOLO v5的簡單結(jié)合相比,筆者方法在各個序列中精度均有不同程度的提升,這表明筆者提出的區(qū)域劃分和特征點分離方案可以有效提升軌跡精度。

最終得到的稠密點云地圖如圖6所示。筆者的稠密建圖線程根據(jù)語義檢測線程確定動態(tài)區(qū)域,只使用框外區(qū)域的點云信息,從而有效濾除動態(tài)信息,構(gòu)建靜態(tài)地圖。

4 結(jié)論

筆者提出了一種針對動態(tài)環(huán)境的,基于目標(biāo)檢測和動靜點分離的視覺即時定位與地圖構(gòu)建技術(shù)。使用YOLO v5網(wǎng)絡(luò)進行檢測,將檢測結(jié)果用于圖像區(qū)域劃分。對檢測框內(nèi)的點做進一步劃分后,在位姿計算中賦以相應(yīng)權(quán)重。在TUM數(shù)據(jù)集的fr3序列walking前綴動態(tài)測試中,筆者方法的絕對軌跡誤差相比ORB-SLAM2減小了90%以上,并且在所有fr3序列中的精度均高于YOLO v5與ORB-SLAM2的簡單結(jié)合。在下一步工作中,將嘗試在視覺慣性里程計系統(tǒng)中增加目標(biāo)檢測和去動態(tài)處理,通過慣性測量單位信息來提高系統(tǒng)魯棒性。