基于ORB-SLAM點(diǎn)云拼接設(shè)計(jì)*

李宗周 劉明芹,2

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

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

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

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

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

2.1 關(guān)鍵幀位姿

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)方案設(shè)計(jì)和實(shí)施

3.1 實(shí)驗(yàn)設(shè)置

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

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

3.2 點(diǎn)云拼接效果驗(yàn)證

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

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

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

4 結(jié)論

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