基于ORB-SLAM點云拼接設計*

李宗周 劉明芹,2

(1.江蘇海洋大學機械工程學院,江蘇連云港 222005;2.中國礦業大學機電學院,江蘇徐州 221116)

1 ORB-SLAM開源改進及地圖融合國內外現狀

對于ORB-SLAM的功能改進,高翔博士修改增加了點云模塊,在程序中,添加了pointcloudmapping的相關程序,Pangolin使ORB-SLAM在運行數據集的時候能直接顯示點云圖。對傳統ORB算法特征點分布不均勻,容易出現簇集的問題和Qtree_ORB算法特征點過均勻等問題,提出了一種基于四叉樹改進的ORB特征提取算法。針對ORBSLAM2系統中隨機抽樣一致(RANSAC)算法在誤匹配剔除時因其算法本身的隨機性而導致效率較低的問題和在ORB-SLAM2系統里未能構建稠密點云地圖的問題,采用漸進一致采樣(PROSAC)算法來改進ORB-SLAM2系統中的誤匹配剔除(如圖1),并在系統中添加稠密點云地圖和八叉樹地圖構建線程[1]。

圖1 ORB-SLAM2算法圖Fig.1 ORB-SLAM2 algorithm diagram

2 關鍵幀位姿圖的優化

2.1 關鍵幀位姿

在點云圖像保存過程中,關鍵幀的信息是保存內容的極為重要的。根據原作者[2],以位姿圖優化中的節點表示相機位姿,以T1,…,Tn來表達,邊是兩個位姿節點之間相對運動的估計,這可來自于特征點法或者直接法。假設估計了Ti和Tj之間的一個運動ΔTij。取一種比較自然的表達方式：

按照圖優化的思路,實際中該等式不會精確的成立,因此設立最小二乘誤差,構建誤差eij：

優化過程中的雅克比求導簡而言之就是所有的位姿頂點和位姿,構成了一個圖優化,本質上是一個最小二乘問題,優化變量為各個頂點的位姿,邊來自于位姿觀測約束。那么總體目標函數為：

2.2 基于 ICP 算法的配準

利用ORB-SLAM的ICP迭代算法,采用ICP方法進行幀與幀之間的配準,得到當前幀與上一幀間的變換關系,建立位姿約束因子。那么,暫且不考慮移動軌跡的信息因素,將有重疊部分的兩個點云假設成兩個具有更多點云信息的幀,從而進行迭代。對其有重疊的部分進行配準,使用雙向距離方法來判斷重疊區域內的點[3],并利用加權最小二乘來求解最優轉換參數,從而使點云旋轉平移,形成一片完整的點云。

給定2個部分重疊的點云,分別為源點云P和目標點云Q,通過對源點云P中的點兩次應用最近鄰居搜索來建立雙向距離關系。P中的任意點pi,可以從點云Q中搜索其最近鄰點qi。

假設待匹配的兩片點云為Pt={pt1,pt2,…,p'tm}和Qt={qt1,qt2,…,q'tm},m’和 n’分別為點云P和Q中的點的數目,R,T分別為旋轉和平移矩陣,迭代原理為最小化點云間距離：

根據ICP算法的第k步的已知剛體變換Rk和Tk,將點云P進行Rkpi+Tk變換,同時建立每個點的二次搜索對應關系,則點云P和Q之間的數學關系為：

在最小二乘的條件約束下計算出點云P和Q的剛體變換,求得最佳的旋轉平移矩陣為：

3 實驗方案設計和實施

3.1 實驗設置

本文是在ORB-SLAM2的基礎上進行實驗,使用自己錄制的數據集,并對ORB-SLAM進行一些改進,添加保存加載功能。再參考點云數據集的官網源代碼,從bin格式文件轉換成pcd格式的點云文件,并且使用pcl顯示查看點云圖。

保存模塊最多修改的部分在map.h以其對應的src文件中,保存的部分包括地圖點的數目以及內容、關鍵幀的數目以及內容、關鍵幀的生長樹節點、詞袋向量、共視圖和關聯關系,添加保存函數SaveMap等和mpKeyFrame Database等指針。在原有程序中加入新接口,實現保存功能。使用原本的開源ORB-SLAM運行自己的單目數據集,得到點云數據。

3.2 點云拼接效果驗證

本文得到的特征點以桌子為基準,分別從45°到90°和90°到135°的位置,拍攝得到的數據集,具有一定部分的重疊部分,這樣可以擁有較高的點云配準速度,之后再經過濾波,減少點云有效配準的參與個數。由觀察可發現,點云表面主要有平坦區域和邊緣區域,在平坦區域,法向量分布密集,變化程度較小；相反,在邊緣區域,法向量變化越大。由圖2可看出,電腦左邊及右邊的特征點組成的兩面立體較直觀地展示了系統建圖的效果。從點云融合效果來看,點云中左側的筆記本電腦邊緣和右側的位置能夠對應起來,最終的融合有效果。

圖2 各方向誤差對比圖Fig.2 Error comparison chart in each direction

利用配準后兩組點云中對原點云對應點對的距離作為誤差度量,各組實驗結果如圖2所示。當采用本文方法對點云執行配準時,其最大配準誤差分別為6.26mm、10.52mm和7.50mm,平均配準誤差分別為0.36mm、0.38mm和0.65mm。由此可見,X軸配準誤差平均值及標準差均較小,結果分布較平穩；YZ軸受深度影響產生的誤差較大。由實驗數據可知,在一定程度上表明本文方法具有較好的穩定性和可重復性。實際上,配準精度會受到多方面因素的影響。通過實驗看出,由于ORB特征點對于法線變化大的邊緣處非常明顯,而對于特征點稀少的平面所能追蹤的點云稀少且不是很精準。處于邊緣和直角的點云比較平滑連續,特征點追蹤較為順利；當在較為空曠的桌面和電腦平面上特征點的點云稍微有些稀少且有誤差,特征點追蹤相比邊緣的時候有些欠缺。可見,在不同高度下的配準誤差有所不同。在5cm到20cm的高度上時,誤差大致呈遞減趨勢,此時目標點云中所保留的樣本數較多；當2cm到5cm時,誤差驟增至10.52mm,對目標點云中噪聲樣本就越多,此時誤差對噪聲的干擾較敏感,從而造成整體配準精度降低；當0cm到2cm時,誤差變化再次趨于平穩,但此時各組配準誤差均較大,尤其是Z軸誤差有明顯的距離,產生這種現象的原因很可能是單目點云的深度出現了在一定區域中的整體性的較大偏差。

4 結論

本文介紹了ORB-SLAM在開源本體的使用過程中,因缺少圖像保存模塊,只能進行關鍵幀和導航軌跡的操作。通過在系統中添加接口,通過保存函數在整個項目的各處作用,通過關鍵幀的點云數據保存成為點云數據的bin格式文件。轉換pcd格式文件。這個設計只針對單目測試使用,沒有對雙目和RGB-D進行測試優化,同時也存在一定的不穩定性。將點云數據應用于基于深度學習的3D目標檢測的研究日益廣泛。由于采用的是普通的點云融合方法,在運行效率上有所缺陷,未來將考慮采用更好的數據融合方案來提升,如采用語義識別[4]或是深度學習能更有效率計算特征點從而融合多個點云實現重構。未來計劃是點云融合前的預處理使用語義分割。根據幾何特征對分割塊進行初步分類,然后通過主成分分析提取分割塊,最后基于分割塊合并策略實現語義分割。