張彥鐸， 袁 博*， 李 迅

(1.武漢工程大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 武漢 430025； 2.武漢工程大學(xué) 智能機(jī)器人湖北省重點(diǎn)實(shí)驗(yàn)室， 武漢 430025)

基于改進(jìn)ICP算法的室內(nèi)環(huán)境三維地圖創(chuàng)建研究

張彥鐸1，2， 袁 博1，2*， 李 迅1，2

(1.武漢工程大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 武漢 430025； 2.武漢工程大學(xué) 智能機(jī)器人湖北省重點(diǎn)實(shí)驗(yàn)室， 武漢 430025)

提出一種基于離散選取機(jī)制的改進(jìn)特征點(diǎn)ICP算法，并設(shè)計(jì)了基于該算法的三維地圖創(chuàng)建方法.該方法分為3個階段，首先提取并匹配相機(jī)運(yùn)動過程中采集的RGB彩色圖像中的SURF特征點(diǎn)；然后結(jié)合RANSAC算法進(jìn)行初始配準(zhǔn)，優(yōu)化特征點(diǎn)集初始位姿、去除誤匹配，并結(jié)合基于離散選取機(jī)制的特征點(diǎn)ICP算法進(jìn)行精確配準(zhǔn)；最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀實(shí)現(xiàn)對相機(jī)運(yùn)動軌跡的優(yōu)化，減少累計(jì)誤差，并將相機(jī)采集到的點(diǎn)云數(shù)據(jù)根據(jù)相機(jī)當(dāng)前位姿構(gòu)建三維點(diǎn)云地圖.經(jīng)過在5個公開數(shù)據(jù)集環(huán)境下進(jìn)行實(shí)驗(yàn)對比，證明本方法的可行性和有效性，在相機(jī)運(yùn)動長度為15.989 m的情況下誤差僅為0.059 m，且能夠準(zhǔn)確地創(chuàng)建實(shí)驗(yàn)環(huán)境的三維地圖.

離散選取機(jī)制； 改進(jìn)特征點(diǎn)ICP算法； RANSAC； g2o； 關(guān)鍵幀

同時定位與地圖創(chuàng)建(SimultaneousLocalizationAndMapping，SLAM)[1]是移動機(jī)器人的一項(xiàng)重要任務(wù).隨著傳感器技術(shù)的發(fā)展和進(jìn)步，研究者們提出了基于各種傳感器的地圖創(chuàng)建方法，RGB-D相機(jī)由于其深度圖像提供的距離信息為創(chuàng)建稠密的三維地圖提供了便利，且三維地圖相對于二維地圖包含了更多有價值的形狀、環(huán)境信息.因此隨著以Kinect為代表的RGB-D相機(jī)的問世，基于RGB-D相機(jī)的三維地圖創(chuàng)建逐步成為近年來的研究熱點(diǎn).

為了得到完整的三維地圖信息，關(guān)鍵問題是將相機(jī)從不同視角獲得的數(shù)據(jù)通過坐標(biāo)變換配準(zhǔn)到同一坐標(biāo)系下，求解坐標(biāo)變換參數(shù)的過程被稱為運(yùn)動估計(jì).針對這個問題，研究者們提出了各種解決方案，其中最常用的是1992年由Besl和McKay提出的迭代最近點(diǎn)(IterativeClosestPoint，ICP)算法[2].然而該算法雖然具有較為精確的配準(zhǔn)效果，但仍存在較多不足：首先，ICP算法是一種點(diǎn)集對點(diǎn)集的配準(zhǔn)方法，因此隨著空間點(diǎn)數(shù)量的增加，算法效率嚴(yán)重降低.其次，點(diǎn)集間的初始位姿對算法效果起重要作用，不合適的初始位姿將使ICP算法陷入局部最優(yōu)，嚴(yán)重影響配準(zhǔn)效果.

研究者們針對ICP算法的特點(diǎn)進(jìn)行了大量的研究，提出了各種改進(jìn)算法.文獻(xiàn)[3]利用Kinect相機(jī)設(shè)計(jì)了一種結(jié)合GPU的實(shí)時三維重建和交互系統(tǒng)，針對ICP算法計(jì)算大規(guī)模數(shù)據(jù)時時間和空間復(fù)雜度高的缺點(diǎn)，利用GPU加速提高了算法效率，但該方法對硬件性能要求高，且由于內(nèi)存的限制無法完成大規(guī)模場景的重建.文獻(xiàn)[4]提出了一種利用Kinect相機(jī)進(jìn)行3D地圖創(chuàng)建的RGBDSLAM系統(tǒng)，該系統(tǒng)利用RANSAC方法結(jié)合關(guān)鍵幀進(jìn)行運(yùn)動估計(jì)并利用非線性優(yōu)化方法優(yōu)化相機(jī)位姿，并使用多個不同場景的數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)評估了系統(tǒng)的性能.文獻(xiàn)[5]提出了一種基于特征點(diǎn)的改進(jìn)ICP方法，與傳統(tǒng)ICP算法不同，基于特征點(diǎn)的ICP算法使用彩色圖片中提取的特征點(diǎn)進(jìn)行配準(zhǔn)，相比于傳統(tǒng)ICP算法大大減少了計(jì)算量，提高了算法效率，但由于特征點(diǎn)之間誤匹配的存在，導(dǎo)致該方法存在配準(zhǔn)精度低，無法收斂等缺點(diǎn).文獻(xiàn)[6]針對基于特征點(diǎn)的ICP算法精度低，易配準(zhǔn)失敗的問題，提出一種以RANSAC算法為策略，求解基于對應(yīng)特征點(diǎn)的ICP算法.該算法隨機(jī)選擇特征點(diǎn)進(jìn)行配準(zhǔn)，采用RANSAC算法策略計(jì)算ICP算法模型，提高了算法的準(zhǔn)確性和魯棒性.但I(xiàn)CP算法模型中隨機(jī)選取的特征點(diǎn)可能出現(xiàn)過于緊密的情況，使得配準(zhǔn)結(jié)果無法滿足全局的配準(zhǔn)要求，從而導(dǎo)致算法陷入局部最優(yōu)造成配準(zhǔn)效果不穩(wěn)定.

綜上所述，雖然文獻(xiàn)[6]提出的算法既提高了實(shí)時性又兼顧了準(zhǔn)確性，但仍存在隨機(jī)選取特征點(diǎn)易使算法陷入局部最優(yōu)從而導(dǎo)致配準(zhǔn)效果不穩(wěn)定的問題.

1本文工作

在某些復(fù)雜環(huán)境中，提取的特征點(diǎn)較為稠密，以RANSAC算法為策略，求解基于隨機(jī)特征點(diǎn)的ICP算法其特征點(diǎn)選取的隨機(jī)性，使得選取的特征點(diǎn)往往出現(xiàn)過于緊密的情況，在后續(xù)的求解幀間坐標(biāo)變換參數(shù)的過程中，導(dǎo)致三維配準(zhǔn)的結(jié)果陷入局部最優(yōu)從而影響配準(zhǔn)精度.

本文提出的基于離散選取機(jī)制的特征點(diǎn)ICP算法，在選取特征點(diǎn)的過程中引入了特征點(diǎn)之間的三維空間歐氏距離作為閾值，既待選取的特征點(diǎn)和任意一個已選取的特征點(diǎn)之間的三維空間歐氏距離必須大于閾值，使選取的特征點(diǎn)離散化.有效地改進(jìn)了其缺陷，進(jìn)一步提高了算法的魯棒性.

1.1算法流程和結(jié)構(gòu)

本文首先提取彩色圖像上的SURF特征點(diǎn)；針對傳統(tǒng)ICP算法中配準(zhǔn)點(diǎn)集初始位姿和誤匹配影響配準(zhǔn)精度的問題，采用RANSAC算法對特征點(diǎn)集進(jìn)行初始配準(zhǔn)，優(yōu)化點(diǎn)集間的初始位姿并去除誤匹配；采用基于離散選取機(jī)制的特征點(diǎn)ICP算法進(jìn)行精確配準(zhǔn)，計(jì)算精確的幀間配準(zhǔn)關(guān)系；最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀實(shí)現(xiàn)對相機(jī)軌跡的優(yōu)化，并創(chuàng)建地圖.算法流程如圖1所示.

1.2相機(jī)模型和運(yùn)動模型

(1)

從而進(jìn)一步得到像素點(diǎn)從圖像坐標(biāo)系UV投影到相機(jī)坐標(biāo)系XYZ的坐標(biāo)變換公式為：

(2)

1.2.2運(yùn)動模型 通常用剛性變換矩陣T描述相機(jī)在不同位姿間的運(yùn)動：

(3)

Pi+1=Ti+1，iPi.

(4)

假設(shè)相機(jī)初始位姿為P0，根據(jù)公式4可知，P0與i時刻相機(jī)位姿Pi的關(guān)系為：

(5)

1.3特征點(diǎn)提取與匹配

目前最常用的特征點(diǎn)提取方法包括FAST[8]、ORB[9]、SIFT[10]、SURF[11]等.

FAST算法和ORB算法特征點(diǎn)提取速度快，但特征點(diǎn)不具有尺度不變性，因此魯棒性較差.SIFT算法提取的特征點(diǎn)穩(wěn)定，但提取效率低.而SURF算法繼承了SIFT算法的穩(wěn)定性，又改進(jìn)了其速度慢、特征點(diǎn)提取效率低的問題.因此，鑒于室內(nèi)環(huán)境相對復(fù)雜的特點(diǎn)，本文采用SURF算法進(jìn)行特征點(diǎn)提取，在一定程度上既保證了算法的魯棒性又兼顧了實(shí)時性.實(shí)驗(yàn)中特征點(diǎn)提取方法效果如表1所示，可以看出SURF算法同時兼顧了特征點(diǎn)的提取數(shù)量和算法的運(yùn)行效率.

特征點(diǎn)匹配階段本文采用雙向相似最近鄰FLANN算法進(jìn)行匹配，并根據(jù)對應(yīng)特征點(diǎn)之間的歐氏距離判斷其匹配是否成功.假設(shè)U1V1和U2V2分別代表兩幅彩色RGB圖像Irgb1和Irgb2中匹配的對應(yīng)特征點(diǎn)P1和P2在圖像坐標(biāo)系UV的坐標(biāo)，有兩點(diǎn)間的歐氏距離計(jì)算公式：

(6)

歐氏距離越小代表特征點(diǎn)的差異程度越低.首先計(jì)算特征點(diǎn)對之間的最小歐氏距離distmin，接下來計(jì)算每個特征點(diǎn)匹配對之間的歐氏距離dist，若dist≤α×distmin則認(rèn)為該特征點(diǎn)匹配對匹配成功，本文實(shí)驗(yàn)中參數(shù)α設(shè)為4.

1.4運(yùn)動估計(jì)

1.4.1RANSAC算法初始配準(zhǔn) 針對基于特征點(diǎn)的ICP算法中特征點(diǎn)集初始位姿和特征點(diǎn)之間的誤匹配影響配準(zhǔn)精度的缺點(diǎn).本文采用RANSAC算法進(jìn)行初始配準(zhǔn)，優(yōu)化初始位姿.RANSAC算法通過估計(jì)圖片上對應(yīng)特征點(diǎn)的單應(yīng)性重投影誤差，將匹配的特征點(diǎn)劃分為內(nèi)點(diǎn)和野點(diǎn)，消除了錯誤匹配.本文中將對應(yīng)特征點(diǎn)二維投影誤差閾值μ設(shè)定為3個像素，即特征點(diǎn)和匹配的特征點(diǎn)對應(yīng)的極線坐標(biāo)誤差超過3個像素則認(rèn)為是錯誤匹配.

由圖2可以看出經(jīng)RANSAC算法進(jìn)行初始配準(zhǔn)之后，有效的去除了特征點(diǎn)中的誤匹配.

(7)

本文為解決文獻(xiàn)[6]算法陷入局部最優(yōu)導(dǎo)致配準(zhǔn)效果不穩(wěn)定的問題，在隨機(jī)選取特征點(diǎn)的基礎(chǔ)上引入了特征點(diǎn)之間的歐氏距離β×distmax作為閾值，對特征點(diǎn)進(jìn)行離散選取.選取規(guī)則如下：

(8)

假設(shè)有兩幅RGB彩色圖片Is和It，算法步驟如下：

第1步：定義離散選取的特征點(diǎn)集Fs、Ft和配準(zhǔn)點(diǎn)集Ps、Pt，點(diǎn)集為空.定義選取的特征點(diǎn)對數(shù)量N以及最大迭代次數(shù)count，并計(jì)算Is中特征點(diǎn)之間的最大歐氏距離distmax，本文中N設(shè)置為10，count設(shè)置為50；

第2步：隨機(jī)在圖像Is中選取一組歐氏距離dist>β×distmax的特征點(diǎn)放入Fs，并將其在圖像It中匹配的對應(yīng)特征點(diǎn)放入Ft；

第3步：隨機(jī)在圖像Is中選取一個特征點(diǎn)p，若該點(diǎn)到Fs中任意一點(diǎn)的歐氏距離dist均大于閾值β×distmax，則將該點(diǎn)及其在圖像It中匹配的對應(yīng)特征點(diǎn)分別放入Fs、Ft；

第4步：重復(fù)步驟3，直至選擇出N對特征點(diǎn)放入Fs、Ft，將點(diǎn)集Fs、Ft投影到以相機(jī)為原點(diǎn)的三維空間相機(jī)坐標(biāo)系XYZ中，分別為配準(zhǔn)點(diǎn)集Ps、Pt；

第5步：結(jié)合公式(7)，根據(jù)Ps、Pt計(jì)算剛性變換矩陣T，若公式(7)中的目標(biāo)函數(shù)小于閾值λ則配準(zhǔn)成功，退出精確配準(zhǔn)算法.否則返回步驟2，清空點(diǎn)集Fs、Ft和Ps、Pt，并將迭代次數(shù)加1，本文中λ設(shè)置為0.05；

第6步：若算法迭代次數(shù)達(dá)到最大迭代次數(shù)，則配準(zhǔn)失敗，退出精確配準(zhǔn)算法，丟棄當(dāng)前幀.

1.5優(yōu)化與地圖創(chuàng)建

ICP算法的思想是通過優(yōu)化兩個點(diǎn)集之間對應(yīng)點(diǎn)的歐氏距離計(jì)算剛性變換矩陣的最優(yōu)解，因此不可避免的的存在誤差，而在地圖創(chuàng)建過程中的誤差累計(jì)會嚴(yán)重影響地圖的準(zhǔn)確性.因此本文采用g2o圖優(yōu)化算法減少累計(jì)誤差，實(shí)現(xiàn)對地圖的快速優(yōu)化.

在基于圖優(yōu)化的SLAM系統(tǒng)中，將相機(jī)的位姿看作結(jié)點(diǎn)，而不同結(jié)點(diǎn)之間的剛性變換矩陣看作是聯(lián)結(jié)對應(yīng)結(jié)點(diǎn)的邊.圖優(yōu)化過程首先需要利用關(guān)鍵幀構(gòu)造閉環(huán)，具體方法是對不相鄰的關(guān)鍵幀進(jìn)行配準(zhǔn)，即在對應(yīng)關(guān)鍵幀結(jié)點(diǎn)之間添加一條邊，從而形成閉環(huán)，然后使用g2o算法進(jìn)行優(yōu)化.本文根據(jù)當(dāng)前幀相對于上一幀關(guān)鍵幀之間的偏移量ΔP作為關(guān)鍵幀的選取標(biāo)準(zhǔn)，若偏移量ΔP大于0.3則將當(dāng)前幀選取為關(guān)鍵幀.偏移量ΔP由兩幀之間的旋轉(zhuǎn)矩陣R和平移向量t的范數(shù)表示，計(jì)算方式如下：

(9)

2實(shí)驗(yàn)設(shè)計(jì)與分析

2.1實(shí)驗(yàn)設(shè)計(jì)

本文實(shí)驗(yàn)部分在搭載Intel(R) Core i5-5250U 1.60GHz處理器、4GB內(nèi)存和Xubuntu15.04操作系統(tǒng)的計(jì)算機(jī)上運(yùn)行.分別在Computer Vision Group[4，13]提供的5個不同數(shù)據(jù)集環(huán)境下進(jìn)行實(shí)驗(yàn).數(shù)據(jù)集包含由Kinect傳感器采集的分辨率為640×480的彩色圖像和深度范圍在2至5米的深度圖像序列、相機(jī)的內(nèi)參以及由高精度運(yùn)動捕捉系統(tǒng)獲取的相機(jī)真實(shí)運(yùn)動軌跡，且提供了工具計(jì)算相機(jī)真實(shí)位姿和算法估計(jì)位姿之間的均方根誤差(RSME)，從而有效的評估算法的準(zhǔn)確性.實(shí)驗(yàn)首先從算法的準(zhǔn)確性和實(shí)時性兩個方面對比本文算法、文獻(xiàn)[6]算法和RGBD SLAM算法；接下來通過對比不同算法構(gòu)建地圖的效果和相機(jī)運(yùn)動軌跡進(jìn)一步驗(yàn)證本文算法的優(yōu)越性.實(shí)驗(yàn)過程中具體參數(shù)和數(shù)據(jù)集詳細(xì)信息如表2和表3所示.

2.2實(shí)驗(yàn)結(jié)果分析

2.2.1算法效果分析 為評估算法性能，本文在fr1/room、fr1/desk、fr1/desk2、fr1/xyz和fr2/xyz等5個公開數(shù)據(jù)集環(huán)境下進(jìn)行實(shí)驗(yàn).其中fr1/room數(shù)據(jù)集環(huán)境最為復(fù)雜，且相機(jī)的運(yùn)動軌跡形成了閉環(huán)，而fr1/xyz和fr2/xyz數(shù)據(jù)集環(huán)境則相對較為簡單，相機(jī)運(yùn)動軌跡僅包含水平和垂直方向的運(yùn)動.

本節(jié)從準(zhǔn)確性和實(shí)時性兩個方面對比評估本文算法、文獻(xiàn)[6]算法和RGBD SLAM算法的性能，文獻(xiàn)[4]詳細(xì)記錄了RGBD SLAM算法在各數(shù)據(jù)集環(huán)境下的實(shí)驗(yàn)數(shù)據(jù).表4中算法誤差為各算法運(yùn)動估計(jì)計(jì)算的相機(jī)位姿和數(shù)據(jù)集提供的相機(jī)真實(shí)位姿之間的RMSE(均方根誤差).

在準(zhǔn)確性方面，由表4可以看出，本文算法和文獻(xiàn)[6]算法運(yùn)動估計(jì)計(jì)算的相機(jī)位姿誤差均小于RGBD SLAM算法，本文算法的誤差相對于RGBD SLAM算法和文獻(xiàn)[6]算法分別平均減少了57.7%和33.1%.尤其在fr1/room、fr1/desk和fr1/desk2三個相對復(fù)雜的數(shù)據(jù)集環(huán)境中本文算法相對于文獻(xiàn)[6]算法計(jì)算的誤差平均降低了48%，而在fr1/xyz和fr2/xyz數(shù)據(jù)集中誤差僅僅降低10.7%.這是由于相對復(fù)雜的環(huán)境中提取的特征點(diǎn)較稠密，文獻(xiàn)[6]方法隨機(jī)選取的特征點(diǎn)過于緊密導(dǎo)致算法陷入局部最優(yōu)從而使算法誤差增大，而本文方法提出的基于離散選取機(jī)制的特征點(diǎn)ICP算法有效的解決了這個問題，因此本文算法在復(fù)雜環(huán)境下依然表現(xiàn)出了較好的準(zhǔn)確性.而在fr1/xyz和fr2/xyz等相對簡單的環(huán)境中，算法提取的特征點(diǎn)較稀疏，文獻(xiàn)[6]方法隨機(jī)選取的大部分特征點(diǎn)已經(jīng)較為分散，在一定程度上了掩蓋了其缺陷，因此文獻(xiàn)[6]算法的準(zhǔn)確性在特征點(diǎn)較稀疏的環(huán)境中和本文算法差距不大，但仍然優(yōu)于RGBD SLAM算法.綜上所述，本文算法在準(zhǔn)確性方面明顯優(yōu)于RGBD SLAM算法和文獻(xiàn)[6]算法，提出的基于離散選取機(jī)制的ICP算法有效的解決了文獻(xiàn)[6]算法中在特征點(diǎn)較稠密的復(fù)雜環(huán)境中易陷入局部最優(yōu)從而影響算法準(zhǔn)確性的問題，通過實(shí)驗(yàn)證明在各種環(huán)境下本文算法都表現(xiàn)出了良好的配準(zhǔn)精度.

在實(shí)時性方面，由于RGBD SLAM算法在實(shí)驗(yàn)中加入了多線程技術(shù)，其運(yùn)算速度提高了2～2.5倍[4]，因此RGBD SLAM算法在實(shí)時性方面優(yōu)于文獻(xiàn)[6]算法和本文算法.本文算法在fr1/xyz以及fr2/xyz數(shù)據(jù)集環(huán)境下耗時分別達(dá)到了文獻(xiàn)[6]算法的130%和150%，而在其他數(shù)據(jù)集環(huán)境下實(shí)時性則略優(yōu)于文獻(xiàn)[6]算法.這是由于在特征點(diǎn)較稀疏的環(huán)境中，文獻(xiàn)[6]方法隨機(jī)選取的特征點(diǎn)本身較為分散，因此配準(zhǔn)過程可以迅速得到收斂，本文算法則需要進(jìn)行初始配準(zhǔn)以及特征點(diǎn)離線選取的工作，影響了算法效率，因此在文獻(xiàn)[6]算法可以迅速收斂的簡單環(huán)境中，本文算法在配準(zhǔn)速度上要慢于文獻(xiàn)[6]方法.而在特征點(diǎn)較稠密的復(fù)雜環(huán)境下，文獻(xiàn)[6]算法隨機(jī)選取的特征點(diǎn)過于緊密，導(dǎo)致配準(zhǔn)結(jié)果無法收斂，雖然該方法在單次迭代過程中耗時小于本文算法，但由于配準(zhǔn)過程中需要反復(fù)選取特征點(diǎn)進(jìn)行迭代計(jì)算，整個精確配準(zhǔn)過程中算法總耗時達(dá)到了本文方法的105%～110%.而本文算法選取出的離散特征點(diǎn)在復(fù)雜環(huán)境下依然可以迅速收斂，無須對特征點(diǎn)進(jìn)行反復(fù)選取迭代，因此在復(fù)雜的環(huán)境中實(shí)時性優(yōu)于文獻(xiàn)[6]算法.兩種算法在特征點(diǎn)稀疏和稠密兩種不同環(huán)境中配準(zhǔn)過程耗時具體分布如表5所示.

圖3為本文算法和文獻(xiàn)[6]方法在數(shù)據(jù)集fr1/room和fr2/xyz環(huán)境下實(shí)驗(yàn)結(jié)果和相機(jī)真實(shí)位姿之間的平移誤差，x軸為每一幀圖片在數(shù)據(jù)集圖片序列中的時間戳，y軸為算法運(yùn)動估計(jì)過程中計(jì)算的相機(jī)位姿和真實(shí)位姿之間的平移誤差，圖中藍(lán)線為本文算法誤差，紅線為文獻(xiàn)[6]算法誤差.有圖可以看出，本文算法相對文獻(xiàn)[6]算法誤差更穩(wěn)定，文獻(xiàn)[6]算法在fr1/room數(shù)據(jù)集環(huán)境中誤差波動較大，在某段時間會出現(xiàn)較大的誤差.這是由于在特征點(diǎn)較稠密的環(huán)境中算法我陷入局部最優(yōu)導(dǎo)致配準(zhǔn)效果不穩(wěn)定造成的.而在fr2/xyz數(shù)據(jù)集環(huán)境中算法提取到的特征點(diǎn)較稀疏，因此文獻(xiàn)[6]算法誤差波動不明顯.

2.2.2地圖創(chuàng)建分析 相機(jī)位姿計(jì)算的越精確，那么根據(jù)由相機(jī)拍攝的圖片所創(chuàng)建的地圖效果就越精確.本節(jié)將進(jìn)一步對比本文算法和文獻(xiàn)[6]算法在fr1/room數(shù)據(jù)集環(huán)境下的建圖效果和相機(jī)運(yùn)動估計(jì)，論證本文算法的優(yōu)越性.

fr1/room數(shù)據(jù)集是由一臺手持的Kinect相機(jī)在封閉的室內(nèi)環(huán)境中錄制的，環(huán)境中包含了很多易導(dǎo)致配準(zhǔn)錯誤的相似物體，環(huán)境非常復(fù)雜，且相機(jī)的運(yùn)動軌跡形成了閉環(huán).因此該數(shù)據(jù)集對算法的可靠性有很高的要求.

圖4為本文算法和文獻(xiàn)[6]算法三維地圖的構(gòu)建效果，左邊為本文方法創(chuàng)建的三維地圖，右邊為文獻(xiàn)[6]方法創(chuàng)建的三維地圖.由圖4可以看出，本文算法獲得了一致性較好的地圖，有效的恢復(fù)了實(shí)驗(yàn)場景，且輪廓清晰，建立了與數(shù)據(jù)集環(huán)境基本一致的三維點(diǎn)云地圖；而文獻(xiàn)[6]算法創(chuàng)建的地圖雖然也較完整的恢復(fù)了場景，但在某些細(xì)節(jié)出現(xiàn)了明顯的偏移.因此，本文算法相對于文獻(xiàn)[6]算法創(chuàng)建的地圖更有效的恢復(fù)了場景，精確度更高.

圖5為兩種算法在數(shù)據(jù)集fr1/room環(huán)境下運(yùn)動估計(jì)計(jì)算的相機(jī)軌跡和相機(jī)真實(shí)軌跡的比較.圖中黑線為相機(jī)真實(shí)運(yùn)動軌跡，藍(lán)線為算法估計(jì)軌跡，紅色部分為兩者之間的誤差.由圖5可見，本文算法運(yùn)動估計(jì)計(jì)算的軌跡與真實(shí)軌跡更接近，算法結(jié)果更精確.

3結(jié)論

本文提出一種基于離散選取機(jī)制的特征點(diǎn)ICP算法進(jìn)行運(yùn)動估計(jì)的三維地圖創(chuàng)建方法.首先提取SURF特征點(diǎn)，在運(yùn)動估計(jì)過程中采用RANSAC算法進(jìn)行初始配準(zhǔn)去除特征點(diǎn)中的誤匹配，優(yōu)化初始位姿，并提出了基于離散選取機(jī)制的特征點(diǎn)ICP算法進(jìn)行精確配準(zhǔn)計(jì)算相機(jī)運(yùn)動軌跡，最后利用g2o圖優(yōu)化算法結(jié)合關(guān)鍵幀減少累計(jì)誤差，實(shí)現(xiàn)對相機(jī)運(yùn)動軌跡的優(yōu)化.通過在5個數(shù)據(jù)集環(huán)境下完成了地圖的創(chuàng)建并評估算法性能.從準(zhǔn)確性和實(shí)時性兩個方面對比了文本算法、文獻(xiàn)[6]算法和RGBD SLAM算法，驗(yàn)證了本文算法在準(zhǔn)確性方面優(yōu)于文獻(xiàn)[6]算法和RGBD SLAM算法，而在實(shí)時性方面，由于本文算法未使用多線程和GPU加速等方法，因此配準(zhǔn)效率僅能達(dá)到2幀每秒，無法滿足實(shí)時建圖的需要.

未來的研究工作將加入詞袋模型(bag of words)進(jìn)行閉環(huán)檢測.并加入多線程技術(shù)提高算法實(shí)時性，使之滿足實(shí)時建圖的需要.

[1] SMITH R C， CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. The International Journal of Robotics Research， 1986， 5(4): 56-68．

[2] BESL P J， MCKAY N D. A method for registration of 3-D shapes[J]//IEEE Transactions on Pattern Analysis and Machine Intelligence， 1992， 14(2): 239-256．

[3] IZADI S， KIM D， HILLIGES O， et al. Kinect fusion: real-time 3D reconstruction and interaction using a moving depth camera[C]//24th Annual ACM Symposium on User Interface Software and Technology， Santa Barbara: Association for Computing Machinery， 2011: 559-568．

[4] ENDRES F， HESS J， ENGELHARD N， et al. An evaluation of the RGB-D SLAM system[C]// IEEE International Conference on Robotics and Automation. 2012: 1691-1696．

[5] MITRA N J， GELFAND N， POTTMANN H， et al. Registration of point cloud data from a geometric optimization perspective[C]// Euro Graphics Symposium on Geometry Processing， 2004: 23-32．

[6] 賈松敏， 王 可， 李秀智，等. 基于RGB-D相機(jī)的移動機(jī)器人三維SLAM[J]. 華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)， 2014: 103-109．

[7] OUROSHK K ， SANDER OUDE E. Accuracy and resolution of kinect depth data for indoor mapping applications[J]. Sensors， 2012， 12(2): 1437-1454．

[8] ROSTEN E， DRUMMOND T. Machine learning for high-speed corner detection[J]. ECCV， 2006， 3951: 430-443．

[9] ETHAN R， VINCENT R， KURT K， et al. ORB: an efficient alternative to SIFT or SURF[J]. IEEE International Conference on Computer Vision， 2011， 58(11): 2564-2571．

[10] LOWE D， DAVID G. Object recognition from local scale-invariant features[J]. IEEE International Conference on Computer Vision， 2001， 2(3): 1150-1157.

[11] HERBERT B， TINNE T， ANDREAS E. SURF: speeded up robust features[J]. ECCV， 2006， 110(3): 404-417．

Reconstructing 3D map in indoor environment based on an improved ICP algorithm

ZHANG Yanduo1,2， YUAN Bo1,2， LI Xun1,2

(1.School of Computer Science and Engineering， Wuhan Institute of Technology， Wuhan 430025; 2.Hubei Key Laboratory of Intelligent robot, Wuhan Institute of Technology, Wuhan 430025)

In this paper， an improved Iterative Closest Point (ICP) algorithm is proposed based on features with the discrete selection mechanism for motion estimation to reconstruct 3D map in indoor environment. Firstly， SURF features in consecutive RGB images are extracted and matched. Then， Random Sample Consensus (RANSAC) algorithm is used for initial registration to optimize the initial pose of features and remove the outliers. Furthermore， secondary registration is applied to calculate the refined transformation between point-clouds in the different coordinate systems combining with the Iterative Closest Point algorithm which based on features with the discrete selection mechanism. Finally， the trajectory of the moving camera is optimized using General Gragh Optimization (g2o) framework combining with key frames， and 3D map is reconstructed through projecting 3D points cloud observed by the camera into global map according to current camera poses. The performance of our proposed algorithm in five public datasets is tested. The results demonstrate that the algorithm is feasible and effectively with the translational error just of 0.059m and ability to generate the 3D map of environment accurately．

discrete selection mechanism; improved ICP algorithm; RANSAC; g2o; key frames

2016-10-12.

國家863計(jì)劃項(xiàng)目(2013AA12A202);國家自然科學(xué)基金項(xiàng)目(41501505).

1000-1190(2017)02-0264-09

P283.49

A

*通訊聯(lián)系人. E-mail: 304033314@qq.com.