李宗周 劉明芹,2

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

1 ORB-SLAM開源改進及地圖融合國內(nèi)外現(xiàn)狀

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

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

2 關(guān)鍵幀位姿圖的優(yōu)化

2.1 關(guān)鍵幀位姿

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

按照圖優(yōu)化的思路,實際中該等式不會精確的成立,因此設(shè)立最小二乘誤差,構(gòu)建誤差eij：

優(yōu)化過程中的雅克比求導(dǎo)簡而言之就是所有的位姿頂點和位姿,構(gòu)成了一個圖優(yōu)化,本質(zhì)上是一個最小二乘問題,優(yōu)化變量為各個頂點的位姿,邊來自于位姿觀測約束。那么總體目標(biāo)函數(shù)為：

2.2 基于 ICP 算法的配準(zhǔn)

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

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

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

根據(jù)ICP算法的第k步的已知剛體變換Rk和Tk,將點云P進行Rkpi+Tk變換,同時建立每個點的二次搜索對應(yīng)關(guān)系,則點云P和Q之間的數(shù)學(xué)關(guān)系為：

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

3 實驗方案設(shè)計和實施

3.1 實驗設(shè)置

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

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

3.2 點云拼接效果驗證

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

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

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

4 結(jié)論

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