基于Kinect 的三维物体重建 55页

'郑郑郑州州州大大大学学学毕毕毕业业业论论论文文文题目：基于Kinect的三维物体重建指导教师：柳朝阳职称：教授学生姓名：杨红庄学号：20092100337专业：数学与应用数学院（系）：数学与统计学院完成时间：2013年5月24日2013年5月24日 摘要三维重建技术是计算机视觉、人工智能、虚拟现实世界等前沿领域的难点和热点。主要是物体的三维重建技术可以快速的将日常生活中的物品数字化，并可将其在计算机中以数字信息形式展现；同时，三维打印机的出现，这也对物体三维重建技术发展起了推动作用，也越来越显示出物体的三维重建的重要性。本文利用微软公司开发销售的Kinect体感传感器进行深度信息的获取，再通过使用OpenNI2SDK将获取的深度信息转化为世界坐标，即为获得三维数据点云，然后再通过对获得的点云数据进行去噪、重建等一系列步骤，最终获得物体的三维模型。本文章主要从Kinect点云获取及噪声处理、ICP（IterativeClosestPoint）算法及其各个变种和ICP算法点云匹配结果三个主要部分讲解三维重建技术。关键词：三维重建；Kinect；ICP ABSTRACTThethree-dimensionalreconstructiontechnologyisthehottopicinthefieldofcom-putervision,artificialintelligence,virtualrealityworldandothercuttingedgefields.Thistechnologycanquicklydigitizetheitemsofourdailylifeanddemonstratetheinformationindigitalform.Atthesametime,withtheadventofthree-dimensionalprinter,whichplaysaroleinpromotingthethree-dimensionalreconstructiontechnology,theimportanceofthistechnologybecomemoreandmoreobvious.ThedataofthisarticleisobtainedbytheKinectsomatosensorysensorwhichisdevelopedbyMicrosoftCorporation.ThenweusetheOpenNI2SDKtotransferthedataintoworldcoordinate,namelythethree-dimensionalpointcloud.Byde-noising,reconstructingthethree-dimensionalpointcloudweultimatelyobtainthethree-dimensionalmodelofanitem.Thispaperanalyzethree-dimensionalre-constructiontechnologymainlyfromthreeaspects:theKinectpointcloudacquisition,theICPalgorithmanditsvariants,andthecloudmatchingresultsofICPalgorithm.Keywords：3Dreconstruction；Kinect；ICP 目录前言11Kinect点云的获取及去噪21.1Kinect工作原理..................................21.2三维物体点云的获取及分割...........................21.3点云的去噪....................................21.4点云法向量的拓扑评估..............................41.5K-最近点的搜索.................................52ICP算法及其变种72.1问题描述.....................................72.2标准ICP算法...................................72.3标准ICP算法特性分析..............................92.4ICP变种......................................102.4.1控制点的选择...............................102.4.2距离特征.................................102.4.3对应点对权重的选择...........................102.4.4点对的剔除................................112.5三维物体的重建..................................113ICP算法点云的匹配结果123.1具有一一对应关系点云匹配...........................123.2无一一对应点云匹配...............................133.3Kinect数据匹配..................................134总结16致谢17参考文献18附录19matlab点云生成源程序.................................19Kinect点云获取源程序.................................20双边滤波去噪源程序..................................22法向量的拓扑评估源程序...............................29拟合平面法向量评估源程序...........................29拟合曲面法向量评估源程序...........................33K-最近邻搜索源程序..................................36 标准的ICP算法源程序.................................38ICP算法变种源程序..................................46均匀采样源程序..................................47随机采样源程序..................................47剔除n%源程序..................................47标准差的若干部剔除源程序...........................48增加权重的源程序................................49方向量夹角剔除源程序..............................49点到面距离的源程序...............................50 前言三维重建技术在计算机视觉等学科中已经研究很久，一直都是这些领域的热点与难点，但目前仅仅只存在于实验室中，没有得到广泛的普及应用，其主要原因是由于大部分数据的获取途径是经过结构光来获取物体的数字化数据，但结构光所需仪器设备价格很高，无法广泛普及。本文利用价格实惠的Kinect来获取的数据的深度信息，然后对这些获取的深度信息进行去噪、转化、重建等操作，最终获得三维物体模型。由于Kinect体感控制的价格便宜及广泛的普及，越来越多的家庭已经具有Kinect设备，这就使一些普通用户也能较方便的获得日常生活用品的数字化信息，很方便的将信息世界与现实世界建立了桥梁，也使越来越多的人们感受到了数字化的优越性，为实现数字化的普及产生的很大的影响。1 1Kinect点云的获取及去噪1.1Kinect工作原理利用红外投影机主动投影的近红外线光谱，当将其照射到粗糙物体或者穿透毛玻璃后，光谱会发生扭曲，这样就会形成随机的反射斑点，进而被红外摄像头获取，分析获取红外线光谱，则可创建可视范围内的物体及人体的深度图像，这就是Kinect工作的主要原理。对于上述光源，一般称其为激光散斑[1]，其具有高度的随机性，而且会随着距离的不同而变化图案，这就是说空间中任意两处的散斑图案都是不同的。只要在空间打上这样的结构光，整个空间就会被标记，如果把一个物体放到这样的空间，只要分析照射到物体上面的散斑图案，就可以知道物体上面点的具体位置了。1.2三维物体点云的获取及分割涉及到Kinect的工作原理及生产商为了降低成本而采用廉价设备，导致Kinect获取的原始数据的噪声很大，所以应该在获取点云之前需要对原始数据进行处理，尽量的消除噪声的影响。考虑到Kinect更新数据的速率是每秒钟30帧，多帧所消耗的时间少，故可以先采用多帧求平均方法对原始数据进行处理，这个过程就是所谓的均值法去噪，这样就可以将明显的错误信息过滤掉。根据经验总结，采用5帧平均能过达到较好的效果。对于采集的数据，我们一般是希望仅获得我们所感兴趣的区域，因而需要对数据进行分割，由于实验工作的场景是宽敞，能够保证物体与背景有一定的距离，故可以直接控制深度信息把感兴趣区域直接分割出来。又空间中任一点均可由深度坐标表示，且存在一一对应的关系，故可以直接将深度信息转化到世界坐标系中，这里采用了OpenNI2SDK库中的函数ConvertDepthToWorld进行转换，这样初始的点云就获取成功，获取的结果在3.3Kinect数据的匹配中展示。点云获取的程序见附录。1.3点云的去噪通常来说，可将噪声可分为大尺度的噪声和小尺度的噪声来处理：对于大尺度的噪声，采用的就是上述的均值法进行处理；对于小尺度的噪声，这里采用的是点云的双边滤波去噪来处理，这样既可以保持模型的尖锐特征又可以去除小尺度的噪声。按照通常的理解，点云的双边滤波去噪[2,3]更新点云坐标的基本思想是：q:=q+n(1)其中:q是点云的数据坐标，n是q点处的法向量，是q点处双边滤波因子。对于双边滤波因子：∑Wc(∥qi−kij∥)Ws(⟨ni;qi−kij⟩)⟨ni;qi−kij⟩kij2N(qi)=∑(2)Wc(∥qi−kij∥)Ws(⟨ni;qi−kij⟩)kij2N(qi)2 其中：N(qi)是数据点qi的邻居点，即是与qi距离相距最近的若干个点，一般情况下，这里选取25个最近点；Wc(x)是光顺滤波函数，其为标准的高斯函数，定义为：2xW(x)=e2c2(3)cWs(x)是特征保权重函数，其定义类似于光顺滤波，也为标准的高斯函数，定义为：2xW(x)=e2s2(4)s参数c是数据点qi到其邻居点距离对数据点qi的影响因子，参数s是数据点qi到其邻近点距离向量在数据点qi的法向量ni上的投影对数据点qi的影响因子。双边滤波算法实现数据点更新的基本步骤：(a)对于任意一数据点qi求出该点的m个邻近点kij;j=1;:::;m(b)对于每一个邻近点kij,求出x=||qi−kij||;y=⟨niqi−kij⟩(c)对于每一个邻近点kij,将上一步求的的x;y,代入光顺滤波函数与特征保权重函数求得Wc(x);Ws(x)(d)将Wc(x);Ws(x)代入式子(2)求的i(e)更新qi:=qi+aini对于我的代码的实现，其输入文件的待去噪的点云数据（这里要求读取的文件类型是*.obj），输出的文件是经双边滤波去噪后的数据（输出的文件类型是*.obj）。首先对*.obj格式做一下了解，obj文件是一种标准的3D模型文件格式，很适合用于3D软件模型见的互导，可以直接利用写字板打开进行查看和编辑修改。其常用的格式主要包括:•v:表示几何体顶点，后面跟点的坐标。•f:表示面，后面跟此面所包含的点的索引。其次，双边滤波代码实现的整体流程图为：图1:双边滤波去噪流程图3 最后，将结合双边滤波去噪的代码的实现步骤与其整体流程图相结合，得到其相对应的源代码程序。源程序见附录。1.4点云法向量的拓扑评估对于1.3节利用双边滤波去噪处理时，需要获取每个数据点处的法向量，但利用Kinect设备获得点云数据，仅仅只获取点的坐标，是不能获取每个数据点处的法向量，因而就需要我们自己去估计每一点的法向量。实现过程中主要利用每个数据点的周围信息（即是邻居点）来做评估，其主要思想：利用点与点之间的拓扑关系，找到每一点处与其最近的k个点，利用这k个点估计出一个平面方程或者二次曲面方程，然后获得估计出来平面的法向量或者曲面在此点的法向量，就记此法向量为该点的法向量。考虑任意一点q0=(x0;y0;f(x0;y0))，利用最近邻搜索搜索出qi点的最近的k个点,将其设为：qi=(xi;yi;f(xi;yi));i=1;:::;k由于点q=(x;y;f(x;y))在点q0处进行多元Taylor展开：12f(x;y)=f(x0;y0)+fx(x0;y0)(x−x0)+fy(x0;y0)(y−y0)+fxx(x0;y0)(x−x0)2(5)122+fxy(x−x0)(y−y0)+fyy(y−y0)+O(||(x−x0;y−y0)||)2则所找到的最近点qi均可由点q0表出。首先考虑拟合出平面方程，故只需考虑多元Taloy展开的线性部分，即f(x;y)=f(x0;y0)+(x−x0)+(y−y0)(6)其中：@f(x0;y0)=@x@f(x0;y0)=@y由于点的坐标是已知的，故Taloy展开式中仅有两个未知变量、,若估计出这两个未知函数，即可得到平面的方程,亦可获得平面上任意点的法向量。对于上述的任意一点q0=(x0;y0;f(x0;y0))及其k个最近邻点qi=(xi;yi;f(xi;yi))，i=1;:::k，任务就是估计出这k+1个点的最佳平面，这就转化为了一个最小二乘问题∑k2min||f(xi;yi)−f(x0;y0)−(xi−x0)−(yi−y0)||(7)i=1利用求极小值的方法进行求解，可转化为求解线性方程组：Ax=b(8)其中:[∑∑]k(x−x)2k(x−x)(y−y)A=i=1i0i=1i0i0∑k∑k2i=1(xi−x0)(yi−y0)i=1(yi−y0)4 [∑∑]Tkkb=−i=1(zi−z0)(xi−x0)−i=1(zi−z0)(yi−y0)此时q0点处的法向量可以表示为：(;;−1)。源程序见附录。其次考虑拟合二次曲面方程，由上述的多元Taloy展开式可以知道，需要估计出来fx;fy;fxx;fxy;fyy五个变量，同拟合平面方程类似，最终也可转化为最小二乘问题：∑k11222min||∆zi−a∆xi−b∆yi−c∆xi−d∆xi∆yi−e∆yi||(9)22i=1其等价于2min||Ax−b||(10)与拟合平面方程类似，也利用极小值方法，可以转化为求解线性方程组：TTAAx=Ab(11)其中：a=fx;b=fy;c=fxx;d=fxy;e=fyy∆x∆y1∆x2∆x∆y1∆y211211121..........A=.....∆x∆y1∆x2∆x∆y1∆y2nn2nnn2nn5[]Tb=∆z1∆z2···∆zn∆xi=xi−x0;∆yi=yi−y0;∆zi=zi−z0此时q点的处的法向量可以表示为：(a;b;−1)。源程序见附录。1.5K-最近点的搜索在双边滤波去噪的与法向量的评估实现中，均涉及到了寻找每个点的最近的k个点，即均需要实现k−最近点的搜索问题，这是一个很复杂的问题，如果按照一般的思想去搜索的话，就需要对数据集中的每个点计算出到其它任意点的距离，然后做从小到大排序处理，获取前k点，依此循环下去即可。这样思想往往仅对于小型、低维的数据集合是可行的，但对于大型数据集是行不通的，可以考虑一个包含10万个点的数据集，如果要获得此数据集中每个点的最近一个点（这样就不用考虑排序时的计算量），要得到我们想要的结果就需要循环100亿次，这个还是没有考虑排序时的时间消耗，若要求的最近k个最近点，计算量是不可估计的，在此处消耗的时间也是不可估算的，程序的可执行度是很差的。为了避免时间的大量消耗，我在这里学习了ANN（ApproximateNearestNeighbor）库，即k−最近邻搜索库，其原理是基于叉树进行搜索的如图1:5 图2:二维情况下ANN最近点搜索示意图这样大大的加快了搜索的效率。特别对于大型数据集和高维数据集加速的效果是非常明显的。在这里做了一个关于三维数据点集搜索的测试，分别测试了数据集点的个数为500、2000，10000三种情况，其结果见表格1。表1:ANN算法搜索最近点与直接搜索比较数据集点的个数ANN算法运行的时间（/ms）直接计算运行的时间（/ms）500617262000223154310000106|源程序见附录。6 2ICP算法及其变种2.1问题描述在计算机视觉领域，点云配准（PointCloudRegistration）是一个非常重要的中间步骤，它在三维物体表面的重建及三维物体的识别等问题中有着极其重要的应用。对于这样的配准问题，研究者提出了很多解决方案，如点标记（PointSignature）法，自旋图像（SpinImage）、主曲率（PrincipalCurvature）方法、遗传算法（Generic）、随机采样一致性算法（RANSAC）等等，这些算法各有特色，在许多情况下是能够解决配准问题。但应用最为广泛的，影响最为卓著的还是1992年由Besl和Mckay提出迭代最近点算法（IterativeClosestPoint，ICP）[4]，它可以直接对点云数据进行匹配，无需进行无关表面的三维数据处理，也不需要对物体的特征进行假设和分割，因而取得了非常广泛的应用。对于Kinect物体重建，目的要重建出一个完整的物体，但由于Kinect的局限性，只能扫描到对应的物体正面信息，因而需要对物体进行多视角的扫描，为了实现这一想法，主要是通过Kinect固定或者物体固定，将其中一个围绕这另一个进行转动的方法实现的，但总的来讲，可以认为目标是不动的，而Kinect的位置变动，这样就能够获得物体的各个面的数据信息，对于这些扫描出的结果，主要区别是坐标系发生了旋转和平移，这其中涉及的参数称为运动参数，准确的计算出运动参数，就可以准确的计算出旋转矩阵和平移向量，从而得到精确的配准结果，这个就是配准的核心任务，一般运动参数的获得主要是利用重叠区域的数据来进行估计的，对于这些待定的运动参数可以用6个自由度来表示（旋转矩阵3个与平移向量3个），现在的任务就是找到一个全局最优的6维向量，从本质讲就是一个基于最小二乘的非线性优化问题，通过最小化两个点集最近点距离的平方和来获得旋转矩阵，最后再利用旋转矩阵获得平移向量。2.2标准ICP算法对于点云P和Q的匹配问题（将点集P向点集Q上匹配），就需要使这两片点云相对应的点欧式距离总和达到最小，因而其对应的目标函数就是一个最小化问题：∑Np12minerror=||qi−Rpi−T||(12)Npi=1其中：R是旋转矩阵，T是平移向量。对于这个最小化问题，可采用上述提出的一个基于四元数[10]的ICP算法,此外还有三种种方法可用做ICP求解[14]其中的奇异值分解也是一个重要的方法[11],这里先采用基于四元数的ICP算法，其主要思想是：通过四元数与旋转矩阵的关系，首先获得四元数，从而获得对应的旋转矩阵。其主要步骤：(a)分别求得点集P和点集Q的重心，即是：∑Np−→1−→p=pi(13)Npi=17 ∑Nq−→1−→q=qi(14)Nqi=1−→−→(b)利用点集P;Q的坐标和式(13),式(14)计算出来的重心p;q计算这两个集合的协方差矩阵:1∑N−→−→−→−→Σpq=(pi−p)(qi−q)Ni=11∑N−→−→−→−→−→−→−→−→=(piqi−pqi−piq+pq)Ni=1(15)1∑N1∑N1∑N−→−→−→−→−→−→−→−→=piqi−pqi−qpi+pqNNNi=1i=1i=11∑N−→−→−→−→=piqi+pqNi=1(c)利用协方差矩阵Σpq获得自对阵矩阵：TAij=(Σpq−Σpq)ij(16)(d)根据步骤(c)获得自对阵矩阵构造列向量：T∆=[A23A31A12](17)(e)利用协方差矩阵Σpq和列向量∆构造出4×4对矩阵:()tr(Σ)∆TpqQ(Σpq)=(18)∆Σ+ΣT−tr(Σ)Ipqpqpq3(f)求出步骤(5)所获得矩阵Q(Σpq)所对应的最大特征值所对应的单位特征向量：−→TqR=[q0q1q2q3](19)−→(g)根据单位特征向量qR(即四元数)，由四元数与旋转矩阵的关系获得旋转矩阵q2+q2−q2−q22(qq−qq)2(qq+qq)012312031302R=2(q1q2+q0q3)q2−q2+q2−q22(q2q3−q0q1)(20)01232(qq−qq)2(qq+qq)q2−q2−q2+q2130223010123(h)利用获得的旋转矩阵R求出平移向量T：−→−→T=q−Rp(21)对于代码的实现，为了达到较精确匹配的效果，需要多次进行计算，就涉及到循环，故需要终止条件，这里设置的条件是第k步的待匹配点云位置相对于第k−1步相对应点云位置的欧式距离的和之差不超过1012，故其算法程序实现的流程图可以表示为：8 图3:点到面匹配结果对于ICP算法变种的实现，将会在2.4节其在流程图中实现的位置一一说明。源程序见附录。2.3标准ICP算法特性分析在图像的配准中，ICP算法应用的非常广泛，主要是由于以下优点：可以获得较精确的效果，对数据点集形状没有要求，可以对任意形状进行配准，在较好的初值情况下，算法是的收敛性可以得到保证。ICP算法虽然取得了很好的应用，但是它也有其局限性，主要是由于该算法那些优点一般都是体现在其中一个点集是另一个点集的子集、或者是一个点集配准到其任意旋转的位置，但这个条件很多时候是无法满足的；其次是最近点的获取，计算量很大，并且在计算最近点的时候使用了一个最基本的假设：欧氏距离下的最近点就是满足条件的两个点云间的对应点，这个有点武断，会产生一定数量的错误的对应点，因而这个情况就要求我们必须有一个较好的初始的运动参数假设。但对于那些不是完全对应的，也不是一个点集完全包含另一个点集，仅仅有部分重叠区域的点云，直接利用标准的ICP算法是行不通的，因而，研究者提出了ICP算法的一些变种。基本上在ICP算法的每一步中都有一些优化，为了更好的展现这些算法的差异性，这里将其分为控制点的选择，距离特征，9 对应点权重的对应，点对的剔除五个部分。下面将从这个五个方面讲述ICP的变种。2.4ICP变种2.4.1控制点的选择ICP算法主要是用于点云的匹配，而且大部分情况是用于不完全重叠的点云，如果直接去利用标准的ICP算法，则需要对待匹配的数据集中每一点都要找到对应点，对于那些没有在重叠区域的点，就找到的不是很有用的对应点，往往这样的点还会带来很大影响，导致结果不准确，偶尔也会出现不收敛的情况。同时，这样也增加了很多不必要的计算量，使程序运行时间增加很多。因而提出了采样的方法，在这里我实现的主要有一致采样方法和随机采样两种。对于一致采样，主要是自己固定要选取的点的索引，这个对于有标记的点云，这是一个很有效的方法。对于那些未知重叠区域，就采用随机采样策略，增加选取点在重叠区域的概率。其在流程图的采样这一步中实现的，相对于标准的ICP算法，主要是增加均匀采点或者随机采点[5]的筛选过程。2.4.2距离特征考虑标准ICP的目标函数:∑Np12minerror=||qi−Rpi−T||(22)Npi=1这里直接采用了点到点的欧式距离。程序实现的还有一种点到面的距离[8]，这里所谓的点到面的距离就是在每一点pi到其最近对应点qi处所拟合出来面的距离，即是点到点的−→距离在其最近对应点qi处的法相量ni的投影，此时目标函数转化为：∑Np122minerror=||qi−Rpi−T||cos(23)Npi=1−→其中：是piqi与ni的夹角这一过程是流程图中计算点对的协方差这一步骤中实现的，与标准ICP算法的区别在于是否相乘cos2。2.4.3对应点对权重的选择标准的ICP算法采用的是一个非零常数作为权重，即点与点之间是没有区别的，这样使得那些点对之间距离很大的点对造成很大的影响，Godin[7]采用策略：距离越远的点对，赋给它的权值越小。Rusinkiewicz[9]实现这一想法采用了:Dist(pi;pj)weigth(i;j)=1−(24)Distmax其中：pj是pi所对应的最近点，Distmax是所有点对的距离最大值。在实现的过程中采用的也是这个思想。这个还隐含着剔除距离最大的点。10 这一过程是流程图中计算点对的协方差这一步骤中实现的，与标准ICP算法的区别在于是否相乘weigth(i;j)，即是目标函数转化为：∑Np12minerror=weigth(i;j)||qi−Rpi−T||(25)Npi=1实现结果表明:匹配的结果有所提高。2.4.4点对的剔除考虑不是全部点的都在重叠区域，这样在重叠区域外部的会找到错误的对应，如果把这样的点对也加入总目标函数，这样会产生很大影响，所以剔除一些很差的点对是必要的，最直接的给定一个阈值，当点对间的距离大于这个阈值时，就剔除该点对，Masuda[5]提出利用所有点对距离的标准差的若干倍作为阈值，经验值一般是采取标准差的2.5倍，Pulli[6]提出了剔除点对距离最大的n%,经验值一般采用剔除10%，同时也有利用点对的法向量的夹角进行剔除[6]，经验值一般为45°，即如果某一点对的法向量的夹角超过45°时就剔除。经过这样的预处理，然后在增加其它策略就会得到较好的结果。这一过程是流程图中寻找最近点这一步骤中实现的，与标准ICP算法不同的是在这一步骤中增加了阈值剔除、剔除n%、法向量剔除或者利用标准差若干倍剔除这一过程。2.5三维物体的重建Kinect数据获取，主要是获取的深度信息，故只能获得正向的深度信息，这里固定Kinect不动，将物体转动，获得物体各个角度的数据信息，对数据进行处理，然后在利用ICP算法将各个角度的数据拼接到一起，即固定第一片扫出来的数据，然后依次将此片与其相邻的上一片做ICP匹配，则可获得完整的三维物体的点云数据，结果在3.3节Kinect数据匹配中展示。11 3ICP算法点云的匹配结果3.1具有一一对应关系点云匹配这里利用matlab生成光滑曲面作为目标点云，将其进行旋转和平移得到待匹配云。然后分别使用点到点、点到面的方法及增加一些策略进行匹配。这样就能够很好的验证ICP算法对一一对应电云的效果，原始图形下图：图4:点云的原始位置(将蓝色匹配到红色)距离特征采用点到点，其匹配的结果如下：图5:点到点及剔除n%匹配结果对于点到点的其他策略，其结果与剔除n%的结果基本上是相同的，这里就没有一一的展示。距离特征采用点到面，其匹配的结果如下：图6:点到面匹配结果12 由于点到面直接进行匹配已经达到较理想的结果，故在这就没有增加其他策略对点对进行进一步的选择及剔除，以下章节也仅展示点到面进行匹配的结果。3.2无一一对应点云匹配如2.3节所考虑的问题，一一对应的点云匹配的情况是很少的，大部分情况是点云仅有部分相交，为了模拟这种情况，这里的原始数据也是利用matlab生成的，设定x;y的范围使得所得两片点云不是一一对应的，即设定变量的范围不是完全一样，有部分是相交的。这样得到的原始图形如下：图7:点云的原始位置(将蓝色匹配到红色)距离特征为点到面的匹配结果：图8:点到面匹配结果)3.3Kinect数据匹配由于3.1节与3.2节的点云数据均是由matlab生成的，其基本上是不含有噪音的，为了更好的与实际情况结合，也为了更加方便的重建出常见的三维物体，这里利用kinect扫描图像识别中常用的人体模型，见下图：图9:点云的原始位置(待重建的三维物体)13 由于Kinect的工作原理，Kinect仅仅只能获取物体的正面信息（正对这Kinect的部分），其获取的上述人体模型正面点云如下图:图10:Kinect获取的点云数据为了获取一个完整的模型，这里采取了八个角度进行数据获取，视角间隔基本上是45"，其获得点云结果放到一起效果如下图：图11:Kinect多视角获取点云位置可以明显的看出来不同角度点云间有很大的误差，经过ICP算法处理后的效果如下图：14 图12:经ICP算法处理后Kinect多视角点云位置将面的情况加入到经过ICP算法点云中，其效果见下图：图13:经ICP算法处理后Kinect多视角添加网格的点云位置15 4总结本文涉及的是利用Kinect进行三维物体的重建，不需要借助昂贵的设备，操作步骤简单，能够在日常生活中直接利用，已经能够得到物体大概的形状，其中主要步骤：获取深度数据，将其转化为点云坐标，经双边滤波处理，转换物体的角度，获取多视角的数据，经ICP算法处理，得到三维物体的点云结果。对于上述结果，只是获得了物体的点云结果，对于获得物体的表面，可以利用Poisson表面重建[13]计算获得，但结果误差较大，若想获得更加精确的结果，可以将重建的结果先经meshlab处理，后经3DMax或者Maya等专业模型修改工具处理和修改，这样基本上能满足要求的。16 致谢毕业设计是毕业生对所学及所感兴趣专业知识的一个总结、归纳、深化的过程，其中包含了我们需要时时加强的东西：诸如分析解决问题能力、查阅文献的能力、统筹编辑的能力和总结经验的能力等等。在毕业设计的数周里，我们得到了指导老师的细心指导和关怀，特别是在设计的仿真设计和论文的修改过程中，对其中遇到的各种问题和疑惑，指导老师都花费了大量的时间和精力一一为我解答，让我在这个设计过程中对Kenect及C语言编程等多方面的知识有了一次新的深入和全面的提升，并能够逐渐将课本上所学的理论知识与生活中的实际问题相结合，从而为以后步入社会解决工作中遇到的实际问题打下良好的基础。在此，谨向指导老师表示最衷心的感谢！在课题的研究设计期间，同学的鼓励和支持也给我们很大的帮助，在这里一同表示感谢：没有你们的支持、帮助和鼓励，我们很难拿出这样的一份最终作品。当然由于时间和知识水平所限，设计中不可避免会出现纰漏和错误，恳请各位审阅老师悉心指正，在此我们不胜感激17 参考文献[1]余涛.Kinect应用开发实战.北京：机械工业出版社，2013年1月.26-96.[2]杜小燕,姜晓峰,郝传刚等.点云模型的双边滤波去噪算法.计算机应用与软件,2010,27(007):245-246[3]TongJ,ZhouJ,LiuL,etal.Scanning3dfullhumanbodiesusingkinects.VisualizationandComputerGraphics,IEEETransactionson,2012,18(4):643-650[4]BeslPJ,MckayHD.AmethodforRegistrationof3-DShapes.TransactionsonPatternAnalysisandMachineIntelligence.1992.14(2):239-256[5]MasudaT,YokoyaN.Arobustmethodforregistrationandsegmentationofmultiplerangeimages.CAD-BasedVisionWorkshop,1994,Proceedingsofthe1994Second.IEEE,1994:106-113.[6]PulliK.Multiviewregistrationforlargedatasets.3-DDigitalImagingandModeling,1999.Proceedings.SecondInternationalConferenceon.IEEE,1999:160-168[7]GodinG,RiouxM.Three-dimensionalregistrationusingrangeandintensityinforma-tion.PhotonicsforIndustrialApplications.InternationalSocietyforOpticsandPho-tonics,1994:279-290.[8]YangC,MedioniG.Objectmodelingbyregistrationofmultiplerangeimages.Imageandvisioncomputing,1992,10(3):145-155.[9]RusinkiewiczS,LevoyM.EfficientvariantsoftheICPalgorithm.3-DDigitalImagingandModeling,2001.Proceedings.ThirdInternationalConfeRenceon.IEEE,2001:145-152.[10]HomBKP.Closed-formsolutionofabsoluteorientationusingunitquaternions.JOSAA,1987,4(4):629-642.[11]ArunKS,HuangTS,BlosteinSD.Least-squaresfittingoftwo3-Dpointsets.PatternAnalysisandMachineIntelligence,IEEETransactionson,1987(5):698-700.[12]DoraiC,WangG,JainAK,etal.RegistrationandintegrationofmultipleObjectviewsfor3Dmodelconstruction.PatternAnalysisandMachineIntelligence,IEEETransactionson,1998,20(1):83-89.[13]KazhdanM,BolithoM,HoppeH.Poissonsurfacereconstruction.ProceedingsofthefourthEurographicssymposiumonGeometryprocessing.2006.[14]EggertDW,LorussoA,FisherRB.Estimating3-Drigidbodytransformation:acom-parisonoffourmajoralgorithms18 附录matlab点云生成源程序dataGen.mfunction[patch0整片曲面,...patch1,...第片1patch2,...第片2patch2_t第片变换后...2]=dataGen(stepx,stepy)%func=@(x,y).3*(1-x).^2.*exp(-(x.^2)-(y+1).^2)...%-1*(x/5-x.^3-y.^5).*exp(-x.^2-y.^2)...%-1/3*exp(-(x+1).^2-y.^2);func=@(x,y).3*(1-x).^2.*exp(-(x.^2)-(y+1).^2);%选取范围%patch0x0Range=[-33];y0Range=[-33];%patch1x1Range=[-33];y1Range=[-31];%patch2x2Range=[-33];y2Range=[-23];%旋转平移rot_x=pi/12;rot_y=pi/12;rot_z=0;T=[0-.50]";%wholepatchpatch0=GetMeshData(x0Range,y0Range,stepx,stepy,func);patch1=GetMeshData(x1Range,y1Range,stepx,stepy,func);patch2=GetMeshData(x2Range,y2Range,stepx,stepy,func);%patch2-transformRx=[100;0cos(rot_x)-sin(rot_x);0sin(rot_x)cos(rot_x)];Ry=[cos(rot_y)0sin(rot_y);010;-sin(rot_y)0cos(rot_y)];Rz=[cos(rot_z)-sin(rot_z)0;sin(rot_z)cos(rot_z)0;001];19 R=Rx*Ry*Rz;patch2_t=R*patch2+repmat(T,1,length(patch2));patch2=patch2";patch2_t=patch2_t";fid1=fopen("origin.obj","wt");fid2=fopen("Roration.obj","wt");n=size(patch2,1);fori=1:nfprintf(fid1,"v%f%f%fn",patch2(i,:));fprintf(fid2,"v%f%f%fn",patch2_t(i,:));endendfunctionpatch=GetMeshData(xRange,yRange,xStep,yStep,func)x=xRange(1):xStep:xRange(2);y=yRange(1):yStep:yRange(2);[x,y]=meshgrid(x,y);z=func(x,y);patch=[x(:)y(:)z(:)]";endKinect点云获取源程序main.cpp#include#include#include"OpenNI.h"#include"opencv.hpp"usingnamespacestd;usingnamespaceopenni;usingnamespacecv;intmain(){//初始化环境OpenniOpenNI::initialize();//声明并打开设备DeviceDevicedevAnyDevice;devAnyDevice.open(ANY_DEVICE);//创建深度数据流VideoStreamstreamDepth;streamDepth.create(devAnyDevice,SENSOR_DEPTH);//设置深度图像视频模式VideoModemModeDepth;//分辨率大小mModeDepth.setResolution(640,480);20 //每秒帧30mModeDepth.setFps(30);//像素格式mModeDepth.setPixelFormat(PIXEL_FORMAT_DEPTH_1_MM);streamDepth.setVideoMode(mModeDepth);//打开深度和图像数据流streamDepth.start();//循环读取数据流信息并保存在中VideoFrameRefinti,j,k,l,it;VideoFrameRefframeDepth;streamDepth.readFrame(&frameDepth);intWidth=frameDepth.getWidth();intHeight=frameDepth.getHeight();DepthPixel*Sum;int*m_com;Sum=newDepthPixel[Width*Height];m_com=newint[Width*Height];Point3f*Finalcoordinate=newPoint3f[Width*Height];for(inti=0;i0){Sum[k]=Sum[k]+pDepth[k];m_com[k]=m_com[k]+1;}}}DepthPixeldepth=2030;l=0;for(j=0;j0){if(Sum[k]/m_com[k]>filename;vectorP,NP;CNormalcnormal;CBilateralcbilateral;CReadWritecreadwrite;P=creadwrite.ReadPoint3D(filename);cnormal.Caculatenormal(P,NP);cbilateral.UpdateCoordinate(P,NP);creadwrite.WriteFile(P);return0;}ReadWrite.h#ifndefREADWRITE_H22 #include#include#include"cv.h"usingnamespacestd;usingnamespacecv;classCReadWrite{public:vectorReadPoint3D(char*filename);voidWriteFile(vectorP);};#endifReadWrite.cpp#include"ReadWrite.h"vectorCReadWrite::ReadPoint3D(char*filename){ifstreamfin;fin.open(filename);chartr;vectorP;Point3dp;while(!fin.eof()){fin>>tr;if(tr=="v"){fin>>p.x;fin>>p.y;fin>>p.z;P.push_back(p);}}fin.close();returnP;}voidCReadWrite::WriteFile(vectorP){charfilename[30]="Bilateral_filtering.obj";ofstreamsavefile;savefile.open(filename);vector::iteratorit;it=P.begin();for(;it!=P.end();it++){savefile<<"v"<<"";savefile<x<<"";savefile<y<<"";savefile<z<<"n";}}23 CNormal.h#ifndefCNORMAL_H#include#include#include"cv.h"#include"ANN.h"usingnamespacestd;usingnamespacecv;classCNormal{public:voidCaculatenormal(vector&P,vector&NP);protected:voidCalculateMeanPoint3D(vector&nNeighboor,Point3d&mean);voidCalculateMatrixEig(double*m,double*eigen,double*q,intn);Point3dCaculate_normal(vector&nNeighboor);};#endifCNormal.cpp#include"CNormal.h"voidCNormal::CalculateMeanPoint3D(vector&nNeighboor,Point3d&mean){vector::iteratorit;mean.x=0;mean.y=0;mean.z=0;for(it=nNeighboor.begin();it!=nNeighboor.end();it++){mean.x+=it->x;mean.y+=it->y;mean.z+=it->z;}mean.x=mean.x/(double)nNeighboor.size();mean.y=mean.y/(double)nNeighboor.size();mean.z=mean.z/(double)nNeighboor.size();}voidCNormal::CalculateMatrixEig(double*m,double*eigen,double*q,intn){double*vec,*eig;vec=newdouble[n*n];eig=newdouble[n];CvMatm_m=cvMat(n,n,CV_64F,m);CvMatm_vec=cvMat(n,n,CV_64F,vec);CvMatm_eig=cvMat(n,1,CV_64F,eig);24 cvEigenVV(&m_m,&m_vec,&m_eig);//需要赋值*eigen=eig[n-1];for(inti=(n-1)*n;i&nNeighboor){Point3dmean,p;Point3dnnormal;CalculateMeanPoint3D(nNeighboor,mean);for(inti=0;i!=nNeighboor.size();i++){nNeighboor[i].x=nNeighboor[i].x-mean.x;nNeighboor[i].y=nNeighboor[i].y-mean.y;nNeighboor[i].z=nNeighboor[i].z-mean.z;}doubleA[9];for(inti=0;i<9;i++){A[i]=0;}for(inti=0;i!=nNeighboor.size();i++){p.x=nNeighboor[i].x;p.y=nNeighboor[i].y;p.z=nNeighboor[i].z;A[0]=A[0]+p.x*p.x;A[1]=A[1]+p.x*p.y;A[2]=A[2]+p.x*p.z;A[4]=A[4]+p.y*p.y;A[5]=A[5]+p.y*p.z;A[8]=A[8]+p.z*p.z;}A[3]=A[1];A[6]=A[2];A[7]=A[5];doubleeig,qr[3];CalculateMatrixEig(A,&eig,qr,3);nnormal.x=qr[0];nnormal.y=qr[1];nnormal.z=qr[2];returnnnormal;}voidCNormal::Caculatenormal(vector&P,vector&NP){vector::iteratorit,it1;25 NP.clear();intmaxPts=P.size();intk=6;intdim=3;doublee=0;ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it=P.begin();it!=P.end();it++){dataPts[iter][0]=it->x;dataPts[iter][1]=it->y;dataPts[iter][2]=it->z;iter++;}kdTree=newANNkd_tree(dataPts,maxPts,dim);Point3dp;vectornNeighboor;for(it=P.begin();it!=P.end();it++){queryPt[0]=it->x;queryPt[1]=it->y;queryPt[2]=it->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);for(inti=0;ix;p.y=it1->y;p.z=it1->z;nNeighboor.push_back(p);}Point3dnnormal;nnormal=Caculate_normal(nNeighboor);NP.push_back(nnormal);nNeighboor.clear();}}CBilateral.h#ifndefCBILATERAL_H26 #include#include#include"cv.h"#include"ANN.h"usingnamespacecv;classCBilateral{public:voidUpdateCoordinate(vector&P,vector&NP);protected:doubleCalculateInnerProduct(Point3dq,Point3dk,Point3dn);doubleCalculateSigmaC(doublex);doubleCalculateSigmaS(doubley);doubleCalculateDistance(Point3dq,Point3dk);};#endifCBilateral.cpp#include"CBilateral.h"voidCBilateral::UpdateCoordinate(vector&P,vector&NP){vector::iteratorit;intmaxPts=P.size();intk=25;intdim=3;doublee=0;ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it=P.begin();it!=P.end();it++){dataPts[iter][0]=it->x;dataPts[iter][1]=it->y;dataPts[iter][2]=it->z;iter++;}kdTree=newANNkd_tree(dataPts,maxPts,dim);vectoralpha;for(it=P.begin();it!=P.end();it++){27 queryPt[0]=it->x;queryPt[1]=it->y;queryPt[2]=it->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);doublem_sum1=0,m_sum2=0;for(inti=1;i#include#include#include"cv.h"#include"ANN.h"usingnamespacestd;typedefstruct_Point3D{doublex,y,z;}Point3D;vectorReadPoint3D(char*filename){ifstreamfin;fin.open(filename);chartr;vectorP;Point3Dp;while(!fin.eof()){fin>>tr;if(tr=="v"){fin>>p.x;fin>>p.y;fin>>p.z;P.push_back(p);}}fin.close();29 returnP;}voidCalculateMeanPoint3D(vector&nNeighboor,Point3D&mean){vector::iteratorit;mean.x=0;mean.y=0;mean.z=0;for(it=nNeighboor.begin();it!=nNeighboor.end();it++){mean.x+=it->x;mean.y+=it->y;mean.z+=it->z;}mean.x=mean.x/(double)nNeighboor.size();mean.y=mean.y/(double)nNeighboor.size();mean.z=mean.z/(double)nNeighboor.size();}voidCalculateMatrixEig(double*m,double*eigen,double*q,intn){double*vec,*eig;vec=newdouble[n*n];eig=newdouble[n];CvMatm_m=cvMat(n,n,CV_64F,m);CvMatm_vec=cvMat(n,n,CV_64F,vec);CvMatm_eig=cvMat(n,1,CV_64F,eig);cvEigenVV(&m_m,&m_vec,&m_eig);//需要赋值*eigen=eig[n-1];for(inti=(n-1)*n;i&nNeighboor){Point3Dmean,p;Point3Dnnormal;CalculateMeanPoint3D(nNeighboor,mean);for(inti=0;i!=nNeighboor.size();i++){nNeighboor[i].x=nNeighboor[i].x-mean.x;nNeighboor[i].y=nNeighboor[i].y-mean.y;nNeighboor[i].z=nNeighboor[i].z-mean.z;}doubleA[9];30 for(inti=0;i<9;i++){A[i]=0;}for(inti=0;i!=nNeighboor.size();i++){p.x=nNeighboor[i].x;p.y=nNeighboor[i].y;p.z=nNeighboor[i].z;A[0]=A[0]+p.x*p.x;A[1]=A[1]+p.x*p.y;A[2]=A[2]+p.x*p.z;A[4]=A[4]+p.y*p.y;A[5]=A[5]+p.y*p.z;A[8]=A[8]+p.z*p.z;}A[3]=A[1];A[6]=A[2];A[7]=A[5];doubleeig,qr[3];CalculateMatrixEig(A,&eig,qr,3);nnormal.x=qr[0];nnormal.y=qr[1];nnormal.z=qr[2];returnnnormal;}voidCaculatenormal(vector&P,vector&NP){vector::iteratorit,it1;NP.clear();intmaxPts=P.size();intk=6;intdim=3;doublee=0;ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it=P.begin();it!=P.end();it++){dataPts[iter][0]=it->x;dataPts[iter][1]=it->y;dataPts[iter][2]=it->z;iter++;}31 kdTree=newANNkd_tree(dataPts,maxPts,dim);Point3Dp;vectornNeighboor;for(it=P.begin();it!=P.end();it++){queryPt[0]=it->x;queryPt[1]=it->y;queryPt[2]=it->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);for(inti=0;ix;p.y=it1->y;p.z=it1->z;nNeighboor.push_back(p);}Point3Dnnormal;nnormal=Caculatnnornal(nNeighboor);NP.push_back(nnormal);nNeighboor.clear();}}voidmain(){char*filename;filename=newchar[20];cout<<"请输入文件名:";cin>>filename;vectorP,NP;P=ReadPoint3D(filename);Caculatenormal(P,NP);ofstreamsavefile;savefile.open("normal.obj");vector::iteratorit1,it2;Point3Dnp,p;it2=P.begin();for(it1=NP.begin();it1!=NP.end();it1++,it2++){np.x=it1->x;np.y=it1->y;np.z=it1->z;p.x=it2->x;p.y=it2->y;p.z=it2->z;savefile<<"vn"<<"";savefile<#include#include#include"cv.h"#include"ANN.h"usingnamespacestd;typedefstruct_Point3D{doublex,y,z;}Point3D;vectorReadPoint3D(char*filename){ifstreamfin;fin.open(filename);chartr;vectorP;Point3Dp;while(!fin.eof()){fin>>tr;if(tr=="v"){fin>>p.x;fin>>p.y;fin>>p.z;P.push_back(p);}}fin.close();returnP;}voidMatrixTran(double*src,double*src_T,intm,intn){for(inti=0;i&nNeighboor){33 Point3Dnormol;double*A,*A_T,*b;A=newdouble[(nNeighboor.size()-1)*5];b=newdouble[nNeighboor.size()-1];A_T=newdouble[(nNeighboor.size()-1)*5];for(inti=1;i!=nNeighboor.size();i++){nNeighboor[i].x=nNeighboor[i].x-nNeighboor[0].x;nNeighboor[i].y=nNeighboor[i].y-nNeighboor[0].y;nNeighboor[i].z=nNeighboor[i].z-nNeighboor[0].z;A[(i-1)*5+0]=nNeighboor[i].x;A[(i-1)*5+1]=nNeighboor[i].y;A[(i-1)*5+2]=0.5*nNeighboor[i].x*nNeighboor[i].x;A[(i-1)*5+3]=nNeighboor[i].x*nNeighboor[i].y;A[(i-1)*5+4]=0.5*nNeighboor[i].y*nNeighboor[i].y;b[i-1]=nNeighboor[i].z;}MatrixTran(A,A_T,nNeighboor.size()-1,5);double*ATA,*ATb,*x;ATA=newdouble[25];ATb=newdouble[5];x=newdouble[5];CvMatm_A=cvMat(nNeighboor.size()-1,5,CV_64F,A);CvMatm_A_T=cvMat(5,nNeighboor.size()-1,CV_64F,A_T);CvMatm_b=cvMat(nNeighboor.size()-1,1,CV_64F,b);CvMatm_ATA=cvMat(5,5,CV_64F,ATA);CvMatm_ATb=cvMat(5,1,CV_64F,ATb);CvMatm_x=cvMat(5,1,CV_64F,x);cvMatMul(&m_A_T,&m_A,&m_ATA);cvMatMul(&m_A_T,&m_b,&m_ATb);cvSolve(&m_ATA,&m_ATb,&m_x,CV_LU);doubledist=sqrt(x[0]*x[0]+x[1]*x[1]+1);normol.x=x[0]/dist;normol.y=x[1]/dist;normol.z=-1/dist;delete[]A;delete[]A_T;delete[]b;delete[]ATA;delete[]ATb;delete[]x;returnnormol;}voidCaculatenormal(vector&P,vector&NP){vector::iteratorit,it1;NP.clear();intmaxPts=P.size();intk=10;intdim=3;doublee=0;ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;34 ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it=P.begin();it!=P.end();it++){dataPts[iter][0]=it->x;dataPts[iter][1]=it->y;dataPts[iter][2]=it->z;iter++;}kdTree=newANNkd_tree(dataPts,maxPts,dim);Point3Dp;vectornNeighboor;for(it=P.begin();it!=P.end();it++){queryPt[0]=it->x;queryPt[1]=it->y;queryPt[2]=it->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);for(inti=0;ix;p.y=it1->y;p.z=it1->z;nNeighboor.push_back(p);}Point3Dnnormal;nnormal=Caculatnnornal(nNeighboor);NP.push_back(nnormal);nNeighboor.clear();}}voidmain(){char*filename;filename=newchar[20];cout<<"请输入文件名:";cin>>filename;vectorP,NP;P=ReadPoint3D(filename);Caculatenormal(P,NP);ofstreamsavefile;35 savefile.open("normal.obj");vector::iteratorit1,it2;Point3Dnp,p;it2=P.begin();for(it1=NP.begin();it1!=NP.end();it1++,it2++){np.x=it1->x;np.y=it1->y;np.z=it1->z;p.x=it2->x;p.y=it2->y;p.z=it2->z;savefile<<"vn"<<"";savefile<#include#include#include"cv.h"#include"highgui.h"#includeusingnamespacestd;typedefstruct_Point3D{doublex,y,z;}Point3D;vectorReadPoint3D(char*filename){ifstreamfin;fin.open(filename);chartr;vectorP;Point3Dp;while(!fin.eof()){fin>>tr;if(tr=="v"){fin>>p.x;fin>>p.y;fin>>p.z;36 P.push_back(p);}}fin.close();returnP;}intk=1;intdim=3;doublee=0.0;intmain(){clock_ttime;charfilename1[10]="1.obj";charfilename2[10]="2.obj";vectorData,Model;Data=ReadPoint3D(filename1);Model=ReadPoint3D(filename2);vector::iteratorit1,it2;intmaxPts=Model.size();ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it2=Model.begin();it2!=Model.end();it2++){dataPts[iter][0]=it2->x;dataPts[iter][1]=it2->y;dataPts[iter][2]=it2->z;iter++;}kdTree=newANNkd_tree(dataPts,maxPts,dim);time=clock();for(it1=Data.begin();it1!=Data.end();it1++){queryPt[0]=it1->x;queryPt[1]=it1->y;queryPt[2]=it1->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);}time=clock()-time;cout<<"程序运行的时间是:"<#include"ICP.h"voidmain(){//存储数据点数据的路径与模型点数据的路径的变量分别为filename1,filename2char*filename1,*filename2;filename1=newchar[20];filename2=newchar[20];cout<<"请输入数据点数据的路径名称:";cin>>filename1;cout<<"请输入模型点数据的路径名称:";cin>>filename2;vectorData;vectorModel;doubleR[9],T[3],e=0.0001;CICPpIcp;Data=pIcp.ReadPoint3D(filename1);Model=pIcp.ReadPoint3D(filename2);pIcp.ICP(Model,Data,R,T,e);Data.clear();Model.clear();delete[]filename1;delete[]filename2;}ICP.h#ifndefICP_H#include#include"cv.h"#include"highgui.h"#include#includeusingnamespacestd;typedefstruct_Point3D{doublex,y,z;}Point3D;classCICP{public:vectorReadPoint3D(char*filename);voidFindClosestPointSet(vector&model,vector&data,vector&Y);voidCalculateMeanPoint3D(vector&P,Point3D&mean);voidICP(vector&model,vector&data,double*R,double*T,doublee);38 voidVectorMul(double*p,double*y,double*mul,intm1,intn1,intm2,intn2);voidMulMatrixAdd(double*a,double*b,intm,intn);voidMulMatrixDiv(double*a,double*b,intm,intn);voidMatrixTran(double*src,double*src_T,intm,intn);doubleMatrixTrace(double*m,intn);voidCalculateMatrixEig(double*m,double*eig,double*q,intn);voidCalculateRoration(double*q,double*R);voidCalculateTransPoint(vector&src,vector&dst,double*R,double*T);voidWriteFile(char*finame,vector&cloud);};#endifICP.cpp#include"ICP.h"vectorCICP::ReadPoint3D(char*filename){ifstreamfin;fin.open(filename);chartr;vectorP;Point3Dp;doublex,y,z;while(!fin.eof()){fin>>tr;if(tr=="v"){fin>>x;fin>>y;fin>>z;p.x=x;p.y=y;p.z=z;P.push_back(p);}}fin.close();returnP;}voidCICP::ICP(vector&model,vector&data,double*R,double*T,doublee){vectorY,P,P1;vectorS_P;vector::iteratorit1,it2;doubled1=0.0,d2=10000.0;//循环上一步误差与当前步误差的变量表示为d1,d2P=data;intround=0;//循环的圈数do{d1=d2;intk=0;Point3Dp;for(it1=P.begin();it1!=P.end();it1++)39 {k++;if(k%1==0){p.x=it1->x;p.y=it1->y;p.z=it1->z;S_P.push_back(p);}}//找到对应最近点,将结果存储在变量YFindClosestPointSet(model,S_P,Y);//计算数据集和点集的均值Point3DP_mean,Y_mean;CalculateMeanPoint3D(S_P,P_mean);CalculateMeanPoint3D(Y,Y_mean);doublem[9];//存储互协方差矩阵计算时叠加矩阵总和的各个元素for(inti=0;i<9;i++){m[i]=0;}//计算数据集和模型集的互协方差矩阵it1=S_P.begin();it2=Y.begin();for(;it1!=S_P.end();it1++,it2++){doublep[3],y[3],mul[9];p[0]=it1->x;p[1]=it1->y;p[2]=it1->z;y[0]=it2->x;y[1]=it2->y;y[2]=it2->z;VectorMul(p,y,mul,3,1,1,3);//将计算出来的矩阵进行叠加MulMatrixAdd(m,mul,3,3);}//求均值for(inti=0;i<9;i++){m[i]=m[i]/(double)S_P.size();}//将均值向量的乘积加入到里面doublep_mean[3],y_mean[3],mul[9];p_mean[0]=P_mean.x;p_mean[1]=P_mean.y;p_mean[2]=P_mean.z;y_mean[0]=Y_mean.x;y_mean[1]=Y_mean.y;40 y_mean[2]=Y_mean.z;VectorMul(p_mean,y_mean,mul,3,1,1,3);MulMatrixDiv(m,mul,3,3);//计算出互协方差矩阵的转置doublem_t[9];MatrixTran(m,m_t,3,3);//计算反对称矩阵AdoubleA[9];for(inti=0;i<9;i++){A[i]=m[i];}MulMatrixDiv(A,m_t,3,3);doubledelta[3];delta[0]=A[5];delta[1]=A[6];delta[2]=A[1];//获得矩阵Qdoubletr=MatrixTrace(m,3);MulMatrixAdd(m,m_t,3,3);doubleI[9]={tr,0,0,0,tr,0,0,0,tr};MulMatrixDiv(m,I,3,3);doubleQ[16];Q[0]=tr;for(inti=0;i<3;i++){Q[i+1]=delta[i];}for(inti=1;i<4;i++){Q[i*4]=delta[i-1];}for(inti=1;i<4;i++){Q[i*4+1]=m[3*(i-1)+0];Q[i*4+2]=m[3*(i-1)+1];Q[i*4+3]=m[3*(i-1)+2];}//求解最大特征值所对应的特征向量doubleeig,qr[4];CalculateMatrixEig(Q,&eig,qr,4);//单位化特征向量doubles_qr=0.0;for(inti=0;i<4;i++){s_qr=s_qr+qr[i]*qr[i];}s_qr=sqrt(s_qr);for(inti=0;i<4;i++){qr[i]=qr[i]/s_qr;}41 //计算旋转矩阵doubleR1[9];CalculateRoration(qr,R1);//计算平移向量doubleT1[3],qt[3];for(inti=0;i<3;i++){qt[i]=y_mean[i];}doublemull[3];VectorMul(R1,p_mean,mull,3,3,3,1);MulMatrixDiv(qt,mull,3,1);for(inti=0;i<3;i++){T1[i]=qt[i];}S_P.clear();Y.clear();round++;if(round>1){cout<x-it2->x)*(it1->x-it2->x)+(it1->y-it2->y)*(it1->y-it2->y)+(it1->z-it2->z)*(it1->z-it2->z);}d2=sqrt(d2);cout<<"abs(d2-d1):"<e);//将变换过的数据输出到*.objchar*filename;filename=newchar[20];cout<<"请输入结果输出文件的名称：";cin>>filename;WriteFile(filename,P);delete[]filename;}voidCICP::FindClosestPointSet(vector&model,vector&data,vector&Y){vector::iteratorit1,it2;Y.clear();intmaxPts=model.size();intk=1;intdim=3;doublee=0.0;ANNpointArraydataPts;ANNpointqueryPt;ANNidxArraynnIdx;ANNdistArraydists;ANNkd_tree*kdTree;queryPt=annAllocPt(dim);dataPts=annAllocPts(maxPts,dim);nnIdx=newANNidx[k];dists=newANNdist[k];intiter=0;for(it2=model.begin();it2!=model.end();it2++){dataPts[iter][0]=it2->x;dataPts[iter][1]=it2->y;dataPts[iter][2]=it2->z;iter++;}kdTree=newANNkd_tree(dataPts,maxPts,dim);Point3Dp;43 for(it1=data.begin();it1!=data.end();it1++){queryPt[0]=it1->x;queryPt[1]=it1->y;queryPt[2]=it1->z;kdTree->annkSearch(queryPt,k,nnIdx,dists,e);it2=model.begin()+nnIdx[0];p.x=it2->x;p.y=it2->y;p.z=it2->z;Y.push_back(p);}delete[]nnIdx;delete[]dists;deletekdTree;annClose();}voidCICP::CalculateMeanPoint3D(vector&P,Point3D&mean){vector::iteratorit;mean.x=0;mean.y=0;mean.z=0;for(it=P.begin();it!=P.end();it++){mean.x+=it->x;mean.y+=it->y;mean.z+=it->z;}mean.x=mean.x/(double)P.size();mean.y=mean.y/(double)P.size();mean.z=mean.z/(double)P.size();}voidCICP::VectorMul(double*p,double*y,double*mul,intm1,intn1,intm2,intn2){CvMatm_p=cvMat(m1,n1,CV_64F,p);CvMatm_y=cvMat(m2,n2,CV_64F,y);CvMatm_mul=cvMat(m1,n2,CV_64F,mul);//计算m_p*的乘积，将结果存储在中m_ym_mulcvMatMul(&m_p,&m_y,&m_mul);}voidCICP::MulMatrixAdd(double*a,double*b,intm,intn){intk;for(inti=0;i&src,vector&dst,double*R,double*T){vector::iteratorit;dst.clear();for(it=src.begin();it!=src.end();it++){doublep[3],r[3];p[0]=it->x;p[1]=it->y;p[2]=it->z;VectorMul(R,p,r,3,3,3,1);MulMatrixAdd(r,T,3,1);Point3Dpoint;point.x=r[0];point.y=r[1];point.z=r[2];dst.push_back(point);}}voidCICP::WriteFile(char*filename,vector&cloud){ofstreamsavefile;savefile.open(filename,ios::ate);vector::iteratorit;it=cloud.begin();for(;it!=cloud.end();it++){savefile<<"v"<<"";savefile<x<<"";savefile<y<<"";savefile<z<<"n";}savefile.close();}46 ICP算法变种源程序均匀采样源程序for(it1=P.begin();it1!=P.end();it1++){k++;if(k%1==0){p.x=it1->x;p.y=it1->y;p.z=it1->z;S_P.push_back(p);}}控制程序中k%m==0的m的值，均匀的选取样本点。随机采样源程序doubleCICP::random(doublestart,doubleend){returnstart+(end-start)*rand()/(RAND_MAX+1.0);}voidCICP::SelectControlPointSet(vector&P,vector&S_P,intm_num){srand((unsigned)time(NULL));vector::iteratorit1,it2;it1=P.begin();for(inti=0;ix;p.y=it2->y;p.z=it2->z;S_P.push_back(p);}}通过控制输入的mnum值的大小来控制随机选取的控制点的个数。利用时直接将此成员函数加入到ICP.cpp程序中即可。剔除n%源程序首先将其按照对应点对的距离从小到大的顺序排序，选择出距离最大的n%,这里取n=90。其源程序：voidCICP::CalculateSort(vector&dist,vector&Index){Dist*m_p;m_p=newDist[dist.size()];for(inti=0;i!=dist.size();i++){m_p[i].Data=dist[i];m_p[i].Index=i;}qsort(m_p,dist.size(),sizeof(m_p[0]),cmp1);inti=(int)(dist.size()*0.9);for(;i!=dist.size();i++){47 Index.push_back(m_p[i].Index);}delete[]m_p;}其次将索引按照从大到小的顺序排序，其源程序：voidCICP::Calculate_Sort(vector&Index){int*m_q;intk=Index.size();m_q=newint[k];for(inti=0;i!=k;i++){m_q[i]=Index[i];}qsort(m_q,k,sizeof(m_q[0]),cmp2);Index.clear();for(inti=0;im_dist;vectorIndex;//找到对应最近点,将结果存储在中YFindClosestPointSet(model,S_P,Y,m_dist);CalculateSort(m_dist,Index);Calculate_Sort(Index);vector::iteratoriter1;vector::iteratoriter2;for(inti=0;i!=Index.size();i++){iter1=S_P.begin();iter2=Y.begin();iter1=iter1+Index[i];iter2=iter2+Index[i];S_P.erase(iter1);Y.erase(iter2);}Index.clear();m_dist.clear();标准差的若干部剔除源程序doubledist_sdv;dist_sdv=CalculateSDV(m_dist);dist_sdv=2.5*dist_sdv;vector::iteratoriter1;vector::iteratoriter2;for(inti=m_dist.size()-1;i>=0;i--){if(m_dist[i]>dist_sdv){iter1=S_P.begin();iter2=Y.begin();iter1=iter1+i;48 iter2=iter2+i;S_P.erase(iter1);Y.erase(iter2);}}m_dist.clear();利用标准差剔除时，直接将上述程序插入到主函数中即可。增加权重的源程序voidCICP::Weigth(vector&dist,vector&m_weigth){doubled_max=0;for(size_ti=0;i!=dist.size();i++){if(d_maxm_weigth;Weigth(m_dist,m_weigth);m_dist.clear();p[0]=(p[0]-p_mean[0])*m_weigth[m_i];p[1]=(p[1]-p_mean[1])*m_weigth[m_i];p[2]=(p[2]-p_mean[2])*m_weigth[m_i];方向量夹角剔除源程序voidCICP::CalculateExcluding(vector&S_P,vector&Y,vector&NS_P,vector&NY){intk;vector::iteratoriter1,iter2;k=NY.size()-1;doubleangel;for(;k!=-1;k--){angel=CalculateAngel(NS_P[k],NY[k]);if(angel<0.707){iter1=S_P.begin();iter2=Y.begin();iter1=iter1+k;iter2=iter2+k;S_P.erase(iter1);Y.erase(iter2);}}}49 这里设置的是对应点对的方向量的夹角大于45゜就剔除此点对。点到面距离的源程序首先先在找到的最近点中计算出来对应点对连接成的向量与此点对中模型集点的法向量的夹角余弦值，后在计算协方差矩阵时乘上对应的余弦值。p[0]=(p[0]-p_mean[0])*m_cos[m_i]*m_cos[m_i];p[1]=(p[1]-p_mean[1])*m_cos[m_i]*m_cos[m_i];p[2]=(p[2]-p_mean[2])*m_cos[m_i]*m_cos[m_i];50'