基于改进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.