基于改進ICP算法的室內環境三維地圖創建研究

張彥鐸， 袁 博*， 李 迅

(1.武漢工程大學 計算機科學與工程學院， 武漢 430025； 2.武漢工程大學 智能機器人湖北省重點實驗室， 武漢 430025)

基于改進ICP算法的室內環境三維地圖創建研究

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

(1.武漢工程大學 計算機科學與工程學院， 武漢 430025； 2.武漢工程大學 智能機器人湖北省重點實驗室， 武漢 430025)

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

離散選取機制； 改進特征點ICP算法； RANSAC； g2o； 關鍵幀

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

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

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

綜上所述，雖然文獻[6]提出的算法既提高了實時性又兼顧了準確性，但仍存在隨機選取特征點易使算法陷入局部最優從而導致配準效果不穩定的問題.

1本文工作

在某些復雜環境中，提取的特征點較為稠密，以RANSAC算法為策略，求解基于隨機特征點的ICP算法其特征點選取的隨機性，使得選取的特征點往往出現過于緊密的情況，在后續的求解幀間坐標變換參數的過程中，導致三維配準的結果陷入局部最優從而影響配準精度.

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

1.1算法流程和結構

本文首先提取彩色圖像上的SURF特征點；針對傳統ICP算法中配準點集初始位姿和誤匹配影響配準精度的問題，采用RANSAC算法對特征點集進行初始配準，優化點集間的初始位姿并去除誤匹配；采用基于離散選取機制的特征點ICP算法進行精確配準，計算精確的幀間配準關系；最后利用g2o圖優化算法結合關鍵幀實現對相機軌跡的優化，并創建地圖.算法流程如圖1所示.

1.2相機模型和運動模型

(1)

從而進一步得到像素點從圖像坐標系UV投影到相機坐標系XYZ的坐標變換公式為：

(2)

1.2.2運動模型 通常用剛性變換矩陣T描述相機在不同位姿間的運動：

(3)

Pi+1=Ti+1，iPi.

(4)

假設相機初始位姿為P0，根據公式4可知，P0與i時刻相機位姿Pi的關系為：

(5)

1.3特征點提取與匹配

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

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

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

(6)

歐氏距離越小代表特征點的差異程度越低.首先計算特征點對之間的最小歐氏距離distmin，接下來計算每個特征點匹配對之間的歐氏距離dist，若dist≤α×distmin則認為該特征點匹配對匹配成功，本文實驗中參數α設為4.

1.4運動估計

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

由圖2可以看出經RANSAC算法進行初始配準之后，有效的去除了特征點中的誤匹配.

(7)

本文為解決文獻[6]算法陷入局部最優導致配準效果不穩定的問題，在隨機選取特征點的基礎上引入了特征點之間的歐氏距離β×distmax作為閾值，對特征點進行離散選取.選取規則如下：

(8)

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

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

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

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

第4步：重復步驟3，直至選擇出N對特征點放入Fs、Ft，將點集Fs、Ft投影到以相機為原點的三維空間相機坐標系XYZ中，分別為配準點集Ps、Pt；

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

第6步：若算法迭代次數達到最大迭代次數，則配準失敗，退出精確配準算法，丟棄當前幀.

1.5優化與地圖創建

ICP算法的思想是通過優化兩個點集之間對應點的歐氏距離計算剛性變換矩陣的最優解，因此不可避免的的存在誤差，而在地圖創建過程中的誤差累計會嚴重影響地圖的準確性.因此本文采用g2o圖優化算法減少累計誤差，實現對地圖的快速優化.

在基于圖優化的SLAM系統中，將相機的位姿看作結點，而不同結點之間的剛性變換矩陣看作是聯結對應結點的邊.圖優化過程首先需要利用關鍵幀構造閉環，具體方法是對不相鄰的關鍵幀進行配準，即在對應關鍵幀結點之間添加一條邊，從而形成閉環，然后使用g2o算法進行優化.本文根據當前幀相對于上一幀關鍵幀之間的偏移量ΔP作為關鍵幀的選取標準，若偏移量ΔP大于0.3則將當前幀選取為關鍵幀.偏移量ΔP由兩幀之間的旋轉矩陣R和平移向量t的范數表示，計算方式如下：

(9)

2實驗設計與分析

2.1實驗設計

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

2.2實驗結果分析

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

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

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

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

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

2.2.2地圖創建分析 相機位姿計算的越精確，那么根據由相機拍攝的圖片所創建的地圖效果就越精確.本節將進一步對比本文算法和文獻[6]算法在fr1/room數據集環境下的建圖效果和相機運動估計，論證本文算法的優越性.

fr1/room數據集是由一臺手持的Kinect相機在封閉的室內環境中錄制的，環境中包含了很多易導致配準錯誤的相似物體，環境非常復雜，且相機的運動軌跡形成了閉環.因此該數據集對算法的可靠性有很高的要求.

圖4為本文算法和文獻[6]算法三維地圖的構建效果，左邊為本文方法創建的三維地圖，右邊為文獻[6]方法創建的三維地圖.由圖4可以看出，本文算法獲得了一致性較好的地圖，有效的恢復了實驗場景，且輪廓清晰，建立了與數據集環境基本一致的三維點云地圖；而文獻[6]算法創建的地圖雖然也較完整的恢復了場景，但在某些細節出現了明顯的偏移.因此，本文算法相對于文獻[6]算法創建的地圖更有效的恢復了場景，精確度更高.

圖5為兩種算法在數據集fr1/room環境下運動估計計算的相機軌跡和相機真實軌跡的比較.圖中黑線為相機真實運動軌跡，藍線為算法估計軌跡，紅色部分為兩者之間的誤差.由圖5可見，本文算法運動估計計算的軌跡與真實軌跡更接近，算法結果更精確.

3結論

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

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

[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相機的移動機器人三維SLAM[J]. 華中科技大學學報(自然科學版)， 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計劃項目(2013AA12A202);國家自然科學基金項目(41501505).

1000-1190(2017)02-0264-09

P283.49

A

*通訊聯系人. E-mail: 304033314@qq.com.