基于語義分割和光流加速的動態場景ORB-SLAM算法

趙 健,李 虹

(太原科技大學 電子信息工程學院,太原 030024)

同步定位與地圖建立(simultaneous localization and mapping,SLAM)算法,是在未知環境下,利用傳感器采集周圍環境信息定位自身位置和位姿的同時增量式的構建地圖,分為VSLAM和激光SLAM.由于相機相對激光傳感器價格比較低、攜帶方便并且可以獲得語義信息,所以VSLAM受到了廣泛的關注。

如今,VSLAM算法發展飛快,其中最具有代表性的就是以提取特征點法為基礎的ORB-SLAM[1](Orient FAST and Rotated BRIEF SLAM)系列和以直接法為基礎的LSD-SLAM[2](Large Scale Direct monocular SLAM),但其在動態環境下的建圖誤差較大。

Fang[3]等利用改進的光流法與卡爾曼濾波相結合用于檢測動態物體,Kunde[4]等利用改進的極平面約束方法檢測運動物體,Lim[5]等利用稠密光流對前后景分離檢測動態物體。如今因為深度學習在圖片處理方面的優越性,很多人都將其加入算法中,大大提高了SLAM系統的性能。Zhong[6]等在SLAM系統中增加SSD語義檢測模塊用于檢測動態物體,增加算法在動態環境的魯棒性,Yu[7]等提出DS-SLAM,利用SegNet語義分割網絡加RANSAC運動一致性檢測剔除特征點。文獻[8]利用YOLOv4網絡對圖片分割并剔除動態點,剔除策略將所有的動態框特征點剔除,但同時也會剔除大量的非動態點,從而導致系統性能下降。Bescos[9]等提出Dyna-SLAM,使用MASK R-CNN分割圖片并且利用多視圖幾何方法剔除動點,由于網絡本身的運行速率低下很難實現實時性。文獻[10]使用DeepLabv3語義分割網絡,再利用稠密光流約束計算每個點運動概率,但因為采用稠密光流剔除動態物體,計算時間偏長,運行實時性得不到保障。文獻[11]利用Light Weight RefineNet輕量級網絡用于語義分割,在對極約束的基礎上增加深度約束用以檢測動態特征點。文獻[12]并沒有使用原有傳統的特征點提取,使用GCNv2網絡提取特征點,使用ESPNetV2進行語義分割,并利用改進的運動一致性剔除動態點,其在一定程度上可以解決弱紋理情況下無法提取特征點的情況,但很有可能出現誤匹配,導致系統精度下降。

本文針對動態場景下運行精度差、不滿足實時性,做出了以下的工作:

在ORB-SLAM2系統增加語義分割線程,并針對高、中、低動態物體設定不同的運動檢測閾值,實現了對動態物體上的特征點剔除又不至于剔除動態框內的靜態物體,保留更多的靜態點實現追蹤。

增加LK光流法追蹤,利用LK光流法追蹤上一幀關鍵幀,加快了Tracking線程,節省了大量提取特征點的時間。

設計以下兩個對比實驗:

1.在TUM數據集上,將增加YOLOv5的ORB-SLAM2系統與ORB-SLAM2、DS-SLAM系統對比,通過結果評價增加YOLOv5的ORB-SLAM2系統的可行性。

2.在TUM數據集上,在增加YOLOv5的ORB-SLAM2系統上對比增加光流的結果和不增加光流的結果,查看其運行時間與RMSE誤差的變化。

1 系統設計

在ORB-SLAM2中并沒有專門的進行動態點的檢測,在特征匹配后直接進行Tracking線程,雖然在追蹤中利用RANSAC剔除了外點,但是對于存在大量動態點的時候,追蹤效果會非常差。如今深度學習網絡是重要的目標檢測的方法,并且它可以對圖片進行語義分割。這對SLAM算法會有一個極大的提升,但算法時實性也很重要,所以速度較慢的RCNN系列不予以考慮,在one-stage的深度學習模型中常用于SLAM的是YOLO[13]和SSD[14],最新的YOLOv5-s模型在速度非?？?并且YOLOv5推出了4種檢測模型,靈活的配置參數,可得到不同的復雜度的模型,所以使用YOLOv5深度學習網絡。

算法模型如圖1所示,在進行特征點提取的同時對圖片語義分割,為每個檢測到的目標標注信息,包括檢測框信息(x、y、w、h、label)與檢測框類別(所屬為高、中、低檢測框)。系統判斷是否需要插入關鍵幀,繼而選擇不同的追蹤模式,如果插入關鍵幀,則利用ORB-SLAM2原本的Tracking追蹤,如果不插入關鍵幀,利用LK光流追蹤加快運行速度。在追蹤成功后利用運動一致性算法剔除動態特征點。

圖1 算法框架圖

2 語義分割

對圖片進行語義分割可以帶來3點好處,如下:

一是語義信息可以在特征點提取時,對圖片的前后背景分離,將一個一個點變為一塊一塊的檢測物體;

二是語義信息可以在建圖時,將標簽帶入地圖中,便于其更好的理解周圍的環境,更好的人機交互;

三是語義信息也可以在回環檢測或者Bundle Adjustment中帶來更多的優化條件,提升系統的精度。

YOLO模型是由Joseph Redmon在2015年提出,為深度學習網絡帶來了新方法,不同于當時的兩階段目標檢測,例如RCNN、Fast-RCNN,YOLO是端到端的目標檢測模型,大大縮小目標檢測的運行時間。YOLOv5雖然不是原作者提出,但其性能十分突出,使用也相當廣泛,模型有三個部分,分別為Backbone、Neck、Output.

Backbone:采用CSPDarknet,在不同圖像細粒度上聚合并形成圖像特征的卷積神經網絡,在傳入圖像上提取大量的特征信息,網絡不斷地向下個階段發送副本,在這方面類似ResNet,有效防止梯度消失問題。

Neck:采用PANet,一系列混合和組合圖像特征的網絡層,并將圖像特征傳遞到預測層,生成特征金字塔對不同縮放對象檢測,利用準確的低層特征自下而上的增強整個特征層次,縮短低層與頂層特征之間的信息路徑。

Output:對圖像特征進行預測,生成邊界框和并預測類別,在特征圖上應用錨定框得到最終輸出。

YOLOv5模型結構圖如圖2所示。

圖2 YOLOv5網絡模型結構示意圖

圖3 對極約束

圖4 檢測框剔除特征點

圖5 ORB-SLAM_YOLOv5的軌跡誤差熱力圖和APE誤差

YOLOv5原生模型可以識別80種類別,在TUM數據集上將“人”“車”等列為高運動物體,“椅子”“杯子”等列為中運動物體,“電視”“電腦“列為低運動物體,其級別越高那么運動的概率就越大。

3 特征點剔除策略

在ORB-SLAM2中Tracking線程讀入圖片,采用四叉數的方法均勻提取特征點用于后續追蹤。在其內部采用RANSAC來剔除外點,這種方法如果在靜態場景或者低動態場景可以有效的運行,但若在動態物體上存在大量的特征點,那么動態點也會被記入內點中,嚴重影響結果。

3.1 特征點剔除原理

利用基礎矩陣的性質p2F21p1=0,判斷特征點是否為動態點。并根據動態點數量閾值判斷是否需要剔除動態點。

空間點P=[X,Y,Z]T,它投影在兩個相機的歸一化平面滿足

x2=R21x1+t

將x1=K-1p1,x2=K-1p2帶入,得:

可以將F21看為Op1到極線l2的投影矩陣,所以Fp1可以得到l2在O2坐標系下的三個直線系數,將三個系數記為a,b,c.

由點到直線的距離公式

將p2記為[u2v21]T

得到匹配點p2距離極線l2的距離

若距離遠大于0,認為其為動態特征點。

3.2 動態點數量閾值設定

考慮不同物體的潛在運動概率不同,高、中、低動態物體的動態點數量閾值應該不同。

若圖片經過深度學習網絡,檢測出“人”這個所屬類別,屬于高運動物體,需判斷這個物體目前是否處于運動狀態,由于本身具有高運動概率,所以需要更嚴格的判斷。設定物體上的特征點超過70%的點為靜態點,那么認為其目前是靜止的,換而言之,若30%以上的點為動態點,那么認為其是運動的。中運動物體的閾值為40%,低運動物體的閾值為50%.

如上圖所示,可以看到人體上的特征點已經剔除,而在后面電腦上的特征點保留。說明算法在剔除動態點時可以對檢測框中靜態點分辨保留,增加特征點數量使算法更精確,在存在大量動態點時仍舊保持比較好的魯棒性。

4 加入LK光流法

Tracking之后的線程都是針對關鍵幀運行,所以普通幀在后續線程中的作用較小,而在系統的運行過程中的特征點提取花費了大量的時間,導致系統運行時間過長。本文將普通幀的特征提取改為利用相鄰幀的光流追蹤,以加快系統的運行。

在Tracking線程中,經過以下四種判斷標準決定是否插入關鍵幀,其滿足(c1||c2||c3)&&c4

c1:系統長時間沒有插入關鍵幀,默認經過30幀仍舊沒插入關鍵幀;

c2:滿足插入關鍵幀的最小間隔并且局部地圖空閑,插入關鍵幀最小間隔默認設置為0,若局部地圖空閑會返回一個標志位;

c3:在雙目或RGBD情況下滿足當前幀跟蹤到的地圖點數量和關鍵幀蹤到的地圖點數量相比比例少于0.25或者追蹤到的近點數量較少遠點數量較多;

c4:當前幀跟蹤到的地圖點數量和關鍵幀蹤到的地圖點數量相比比例少于0.75或者追蹤到的近點數量較少遠點數量較多并且匹配的內點數量大于15.

4.1 LK光流原理

LK光流法基于三個條件下使用:

(1)灰度不變性假設;(2)相鄰空間一致性原則;(3)小運動原則。

灰度不變性假設,是一個很強的假設,也是光流法最基本的一個假設,在實際應用中往往會造成很大的問題,對光線變換敏感,但在本文中光流追蹤關鍵幀,關鍵幀與普通幀之間的時間相隔非常小,可以保證灰度不變性,增加了LK光流法的可用性,也加快了算法運行。

灰度一致性公式:

I(x,y,t)=I(x+?x,y+?y,t+?t)

對公式右側泰勒展開得到:

I(x+?x,y+?y,t+?t)=

根據上面兩式,可得到下式,

IxVx+IyVy=-It

取特征點3×3窗口內9個像素點聯立,使其成為超定方程組,再求解Vx、Vy.

4.2 求解位姿

光流法得到的特征點,利用PnPRansac計算位姿。

在PnPRansac中,使用PnP算法求單應矩陣H的同時利用RANSAC算法剔除誤匹配,以此來提高變換矩陣T的精度。其具體原理如下:

單應矩陣H是一個3×3滿足尺度不變性的矩陣,所以它的自由度為8.匹配的兩個點滿足p1=H12p2.

上述式子,可以展開得到兩個約束關系,如下

每一個匹配點可得到兩個方程,所以最少需要4個匹配點來計算H.

再利用RANSAC算法剔除外點,通過迭代篩選內點,算法的精度會隨著迭代的次數提升,設定迭代50次。

5 實驗結果與分析

本次實驗在TUM數據集進行兩次對比,如下。

(1)由于算法在ORB-SLAM2系統上增加YOLOv5,所以命名為ORB-SLAM_YOLOv5,分別與ORB-SLAM2、DS-SLAM算法對比絕對軌跡誤差,查看算法可行性。

(2)在ORB-SLAM_YOLOv5算法上增加LK算法,將其命名為ORB-SLAM_YOLOv5_with_LK,對比ORB-SLAM_YOLOv5與ORB-SLAM_YOLOv5_with_LK兩算法的運行時間。

本次實驗配置,CPU為英特爾i7-8565U,顯卡為Nvidia GeForce MX230,內存為12GB,Ubuntu20.04系統下運行。

5.1 ORB-SLAM_YOLOv5對比

表1所示結果,ORB-SLAM_YOLOv5在TUM數據集(fr3_walking_xyz、fr3_walking_half、fr3_walking_rpy、fr3_walking_static、fr2_desk_with_people)上與DS-SLAM、ORB-SLAM2進行絕對誤差對比。

表1 絕對軌跡誤差ATE

表2所示結果,ORB-SLAM2、DS-SLAM分別與ORB-SLAM_YOLOv5提升率計算。

表2 絕對軌跡誤差提升率

表1與表2中數據集序列分別簡寫為xyz、half、rpy、static、desk.RMSE為均方根誤差,是重要的評分標準,Mean為均值,Med為中值數,S.D.為標準差。

表2提升率公式:

從上述表格中的DS-SLAM數據采用其論文中的數據,可以看出本算法對于ORB-SLAM2系統的提升比較大,除了在低運動場景下的fr2_desk_with_person數據集提升率為70%左右,其余數據集提升率均為95%左右。對于DS-SLAM,在高動態fr3_walking_rpy數據集上DS-SLAM誤差達到0.444 2,不能滿足使用要求,本文提出的算法誤差為0.031 6,提升率在92.88%,本文算法在其它數據集上也表現良好,這與采用不同閾值的動態點剔除和使用YOLOv5模型有很大的關系,雖然在fr3_walking_static下誤差與DS-SLAM有所提升,但其RMSE在提升后仍舊為0.009 2,依舊滿足可用性。

總體來說ORB-SLAM_YOLOv5系統在TUM數據集上的效果整體要強于ORB-SLAM2和DS-SLAM.

5.2 增加LK光流實驗結果

由于fr2_desk_with_people屬于低動態場景并且ORB-SLAM2的效果已經很好了,沒有動態場景下的代表性,所以在以下4個數據集對比ORB-SLAM_YOLOv5增加LK光流和不加LK光流的情況。

下述時間的計算公式:

Total Tracking Time是追蹤線程總的運行時間,n是運行的圖片的總數,Mean Tracking Time是每一張圖片平均運行時間。

圖6可以看出增加LK光流后運行時間均有減少,而RMSE則有升高有降低,基本平穩,在誤差變動不大的情況下,大大的縮少了時間,例如,fr3_walking_static數據集有910張圖片,每一張圖片縮少0.017 3 s,總體縮小時間在15.743 s,大大縮小了運行時間。由于LK光流只是加在在關鍵幀之間的普通幀,若一個數據集頻繁的插入關鍵幀,那么本算法減少的時間并沒有那么可觀,所以對于關鍵幀插入的條件要求較高,需要在不影響誤差的情況下,加快運行速度。從以上4個數據集中可以看出誤差變化并不大,甚至在fr3_walking_rpy與fr3_walking_static還略有下降,說明了增加LK算法的可取性。

圖6 增加LK光流后的誤差與時間對比

6 結論

為了增加在高動態環境中SLAM系統的魯棒性,本文在ORB-SLAM2系統的基礎上增加了語義分割模型與LK光流,語義信息將圖片中的點與點的關系變為檢測框與檢測框的關系,利用對極約束的運動一致性檢測剔除動態框的特征點,保留靜態點。在關鍵幀與關鍵幀之間使用LK光流追蹤普通幀,省去提取特征點的時間,大大的加快運行速度。LK光流追蹤中使用RansacPnP計算位姿,誤差與不加光流的情況保持平穩。本文在TUM數據集上對算法驗證,在高動態場景數據集下與ORB-SLAM2對比普遍優化在90%以上,與DS-SLAM對比,在fr2_walking_rpy數據集下優化90%以上。

但是算法依舊有不足,深度學習網絡在標注與訓練時會花費大量時間,時間效率不能與傳統方法相比,另外由于檢測框采用矩形,在剔除特征點時會剔除過多特征點,之后可以采用顯著性區域設置掩碼圖以減少特征點剔除。在后續的研究中考慮增加IMU傳感器,使其在無紋理、光照條件差等情況下增加算法適用性與魯棒性。