基于目標檢測和動靜點分離的視覺即時定位與地圖構建技術

□ 來金強

寧波大學 機械工程與力學學院 浙江寧波 315211

1 研究背景

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

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

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

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

2.1 系統(tǒng)框架

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

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

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

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

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

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

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

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

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

2.3 隨機抽樣一致性算法

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

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

(1)

式中:x1為P1的歸一化坐標。

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

(2)

式中:x2為P2的歸一化坐標。

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

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

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

(3)

(4)

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

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

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

(5)

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

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

在TUM數(shù)據(jù)集[8]上做點的分離測試,特征點分離和標記如圖3所示。框外使用隨機抽樣一致性算法保留后的點標記為白色,框內(nèi)區(qū)域的可用靜點標記為灰色,動點則標記為黑色。可以看出,這一策略能有效區(qū)分各類特征點。

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

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

(6)

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

E用于引導誤差項的求解方向,可表示為:

(7)

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

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

(8)

Eη=ηE

(9)

式中:xj為檢測框內(nèi)保留的可用靜點的像素坐標系坐標;Xj為檢測框內(nèi)保留的可用靜點的世界坐標系坐標;Eη為保留可用靜點的信息矩陣;η為權重因數(shù),默認小于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,可以通過兩個指標對算法進行評估,分別為絕對軌跡誤差和相對位姿誤差。

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)嚴重偏移,基本喪失了魯棒性。

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

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

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

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

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

4 結(jié)論

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