一种全局优化与多约束条件迭代的3D彩色点云配准方法与流程

本发明属于3d点云数据处理技术领域，具体涉及一种全局优化与多约束条件迭代的3d彩色点云配准方法。

背景技术：

随着人工智能的发展，图像配准技术逐渐从二维图像过度到三维图像，而3d点云作为三维图像的典型代表之一，已逐渐被广泛应用。3d点云配准是首先利用计算机对3d点云进行预处理，然后采用合适的算法对于处理后的数据进行粗配准，最后再选取合适的方法对粗配准后的结果进行精配准。其点云数据配准的整个过程中，滤波器的选取、粗配准准则及精配准约束条件的选取直接决定点云数据最终的配准结果。

近些年，3d点云数据配准技术应用越来越广泛，引起了一些学者的关注。这种配准方法的基本思想是：首先先去一定的滤波器对原点云数据进行预处理，其次采取一定的算法对预处理结果进行粗配准，最后对其结果进行精配准。目前，点云数据的配准方法虽然由很多，可以较好的进项点云数据的配准处理，但仍旧存在一些缺陷：第一，滤波器选取得好坏直接影响噪声、离群点和离群点簇剔除的结果，从而决定其粗配准的精度高低；第二，在粗配准的过程中，往往会陷入局部最优解，使得粗配准的误差较大；第三，在精配准的过程中，约束条件的单一化选择导致精配准后的误差较大，配准精度较低。

技术实现要素：

本发明的目的是提供一种全局优化与多约束条件迭代的3d彩色点云配准方法，解决了现有3d彩色点云配准方法中配准精度较低，配准时间较长的问题。

本发明所采用的技术方案是，一种全局优化与多约束条件迭代的3d彩色点云配准方法，具体按照以下步骤实施：

步骤1，采用基于标记的多状态离群点剔除原点云数据中的噪声、离群点及离群点簇；

步骤2，经过步骤1后，采用pso粒子群优化算法对滤除后源点云数据和目标点云数据进行粗配准处理；

步骤3，经过步骤2后，采用蚁群优化算法对其进行全局优化处理，得到更加精准的粗配准结果；

步骤4，经过步骤3后，采用改进的多约束条件迭代最近点精确配准算法对其进行精配准处理。

本发明的特点还在于，

步骤1中，具体按照以下步骤实施：

步骤1.1，用k-d树进行点云的邻域搜索；

步骤1.2，经步骤1.1后，用移动最小二乘法局部表面拟合方法对点云法向量进行计算；

步骤1.3，经步骤1.2后，对不同点云数据设置不同的阈值，以点法线差值的正交分量的比值与设置阈值进行比较，以作为孤立点的判断条件，标记离群点；

步骤1.4，经步骤1.3后，以改进的dbscan聚类方法对标记离群点进行聚类；

步骤1.5，经步骤1.4后，统计每个聚类中标记离群点的数量占该聚类中点云总数的比值，当该比值大于设定的比例阈值时，判定并删除离群点簇；

步骤1.6，重复步骤1.5，直到删除所有的离群点簇，同时删除剩余的标记离群点；得到滤除后源点云数据和目标点云数据。

步骤2中，具体按照以下步骤实施：

步骤2.1，初始化pso粒子群算法的迭代次数m＝80，随机生成平移变量tx,ty,tz以及旋转变量rx,ry,rz，初始化学习因子c1＝2、c2＝2；

步骤2.2，经过步骤2.1后，采用单位四元数法求出源点云p和目标点云q的旋转变换矩阵r和平移矩阵t，并计算目标点云q旋转、平移后的矩阵q′；具体按照以下步骤实施：

步骤2.21，计算源点云p与目标点云q的重心，如式(1)所示；

式(1)中，pi和qi是源点云与目标点云的点，i为其下标，表示第i个点，n为每个点云的总数，μp和μq分别为源点云p与目标点云q的重心；

步骤2.22，利用公式(2)计算其协方差矩阵m；

步骤2.23，根据协方差矩阵m，利用公式(3)构造4阶矩阵s；

步骤2.24，计算矩阵s的特征值和特征向量，并将最大特征值对应的特征向量作为所求的旋转变量r；

步骤2.25，利用公式(4)计算平移矩阵t；

t＝μp-rμq(4)；

步骤2.26，经过步骤2.24和2.25后，获得旋转矩阵r和平移矩阵t，并利用r和t及公式(5)计算目标点云旋转、平移后的矩阵q′；

q′＝rq+t(5)；

步骤2.3，利用公式(6)计算适应度函数fitness；

fitness＝|rt(q-q′)|，i＝1,2,3,...,m(6)；

式(6)中，变换矩阵rt包含6个参数，平移变量tx,ty,tz以及旋转变量rx,ry,rz，q’为变换后的点云集；

步骤2.4，利用公式(7)计算源点云和目标点云对应点对的局部最小距离fpbest，表示粒子当前最佳位置；

f(pi，qi)＝|p′i-q′i|(7)；

式(7)中，p′i和q′i表示待配准点对pi和qi的k邻域的重心；

步骤2.5，令fp＝fpbest，fitnessbest＝fitness，rt＝rt(r0,t0)，fp表示粒子历史最佳位置，fitnessbest表示所有粒子的最佳位置函数，rt表示最佳变换矩阵，rt(r0,t0)表示初始化的变换矩阵，实现源点云和目标点云粒子群粗配准；具体按照以下步骤实施：

步骤2.51，设置r1i＝rand()，r2i＝rand()，其中r1i和r2i都是伪随机数，rand()是随机函数，在(0,1)的均匀分布中生成随机数；

步骤2.52，以空间矩阵rt代替速度v，并利用公式(8)更新rt，对目标点云q进行空间矩阵变换；

式(8)中，d为维数，v为粒子i的速度，pi为粒子i在一次迭代中的最佳位置，c1和c2为加速器，设置为2；r1和r2是伪随机数，设置为0-1；

步骤2.53，利用公式(6)计算此次变换的适应度函数，并利用公式(7)计算源点云p与目标点云q的各点对的局部距离函数，并确定其中最小值fpbest；

步骤2.54，比较fp和fpbest，如果fpbest小于fp，则继续比较fitnessbest和fitness，如果fitness小于fitnessbest，则fitnessbest＝fitness，fp＝fpbest；否则，fitnessbest不变；如果fpbest大于fp，则fp不变；

步骤2.55，确定满足实验数据的最佳迭代次数m，如果迭代m次后，标记此时的最近点对集合；否则，重复步骤2.51至步骤2.55。

步骤3中，具体按照以下步骤实施：

步骤3.1，对目标点集中的点与点之间赋予初始信息素值c＝2，并在目标点云上随机选择n个点作为“蚂蚁”的初始位置；

步骤3.2，利用公式(9)计算每个“蚂蚁”点在其邻域内的状态转移概率并进行状态转移；

式(9)中，pij^k表示编号为k的蚂蚁从i位置向j位置的“转移”的概率；τij(t)表示t时刻位置i与位置j的信息素值；ηij通常设为i位置和j位置之间距离的倒数；jk(i)表示i位置上的蚂蚁k未访问的邻近位置点的集合；α,β为启发式因子，其中α＝0.9,β＝1.06，分别表示确定信息素浓度τij和距离间的评价参数；

步骤3.3，对所有“蚂蚁”点搜索到一条路径后，遍历目标点集中所有点；

步骤3.4，利用公式(10)进行信息素迭代计算；

式(10)中，ρ为信息素蒸发参数，ρ＝0.1，0＜ρ＜1；δτ^kij表示蚂蚁k在i位置和j位置之间信息素的浓度，并利用公式(11)计算获得；

式(11)中，和是选择的对应点对的法向量，且两法向量相似度高时，k表示第k个粒子的搜索过程；λ＝8；

步骤3.5，确定最终比较的阈值，若路径存在更新的信息素值τij大于预先设定的阈值，则搜索结束，该路径(点集合)与源点云组成的点对作为配准的点对；否则重复步骤3.2至步骤3.4；

步骤3.6，利用单位四元数法求解最终选择的点对之间的空间变换矩阵，完成粗配准。

步骤4中，在多约束条件的icp精配准算法中，对源点云p与目标点云q预先设置最大迭代次数m＝200，目标函数阈值δ＝10^-6m，利用公式(12)表示迭代配准中的目标函数；

其中，ns为点云数量，r和t为两点云配准中的旋转矩阵和平移矩阵，pi和qi分别为源点云与目标点云中的点；

具体按照以下步骤实施：

步骤4.1，搜索源点云p和目标点云q，确定对应点对；具体按照以下步骤实施：

步骤4.11，搜索源点云p和目标点云q，使得欧式距离‖pi-qi‖＝min最小的点对为初始对应点对；

步骤4.12，利用公式(13)计算所有点对中每点的曲率，并将这些点对存储到集合ts{(pi,qi),i＝1,2,3,...,n′}中，该集合包含初始点对及这些点对的曲率信息；

式(13)中，k1(pi)、k2(pi)分别是点曲率的最大值和最小值，也称为点的主曲率；

步骤4.13，利用公式(22)计算每对点对的曲率相似度ψ，并判断误差是否在取值区间之内，若在，保留该点对作为待配准的对应点对；若不在，搜索目标点qi的k邻域，并利用公式(13)计算邻域内各点的曲率信息；

ψ＝|s(pi)-s(qi)|(22)；

步骤4.14，计算点对中点pi和qi邻域各点的曲率相似度及欧式距离，将满足欧式距离最小且曲率相似度符合误差区间[0,0.08]的邻域点作为新的qi；若找不到满足以上两条约束的邻域点，则将该点对从集合ts中删除；

步骤4.15，将最终确定的点对在源点云与目标点云集合进行标记；

步骤4.2，目标函数最小化，并利用奇异值分解法计算此次迭代过程中的旋转矩阵rk、平移矩阵tk，利用公式(23)计算此次误差函数d；

其中，计算旋转矩阵rk、平移矩阵tk的具体步骤为：

步骤4.21，利用公式(24)表示空间变换矩阵函数；

步骤4.22，利用公式(25)计算源点云与目标点云的重心坐标，并利用公式(26)确定去重心化后的两个点集p′和q′，则利用公式(27)表示新的变换矩阵函数；

p′＝p-μp,q′＝q-μq(26)；

步骤4.23，对目标函数的总误差进行化简，化简后的函数由公式(28)表示，并对其进一步表示为公式(29)；

步骤4.24，要使目标函数变为最小，即使得tranc(rh)最小化；线性代数中，m是一个对称矩阵且b是正交矩阵，则有tranc(m)≥tranc(bm)，对h进行奇异值分解，得到公式(30)；

h＝u∑v^t(30)

式(30)中，∑为3阶非负元素对角矩阵；

步骤4.25，令旋转矩阵r＝vu^t时，rh为对称正定矩阵；此时的r为最优旋转矩阵，并计算平移矩阵t；

步骤4.3，利用公式(31)更新目标点集；

q＝rkp+tk(31)；

步骤4.4，再次计算新的对应点对使之满足欧式距离最小，并判断点对是否被标记；若标记，则直接根据标记索引，在集合ts中找到该点对；否则按照步骤4.12计算点对的曲率信息并对这些点对进行归类或者删除；

步骤4.5，重复迭代并计算两次误差的差值|d-d’|，若满足|d-d’|＜δ，则算法结束；否则继续迭代，直至达到最大迭代次数。

任意一点p的主曲率，具体实现步骤为：

设二次平面的一般方程，如公式(14)所示；

z＝f(x,y)＝a0x²+a1y²+a2x+a2y+a4(14)；

在目标点pi的k邻域内，根据最小二乘原理利用公式(15)该曲面方程的最小值；

由公式(14)和公式(15)求出该曲面的拟合曲面方程，拟合曲面方程由公式(16)表示；

r(x,y)＝(x,y,a0+a1x+a2y+a3x²+a4y²)(16)；

计算拟合曲面r(x,y)的偏导rx，ry，rxx，ryy，rxy，并利用公式(17)计算曲面的单位法矢量；

利用公式(18)和(19)分别计算平均曲率和高斯曲率；

其中，e＝rx·rx,f＝rx·ry,g＝ry·ry,

利用公式(20)和公式(21)计算点p的主曲率；

本发明的有益效果是，

该方法采用基于标记的多状态离群算法有效剔除噪声、离群点及离群点簇后，首先利用pso粒子群优化算法对其进行初步粗配准处理；其次，使用蚁群优化算法对其进行全局优化处理；最后，采用全局多约束条件迭代最近点精确配准算法对其进行精配准处理。在粗配准过程中，由于pso粒子群优化算法的处理过程收敛速度快，容易陷入局部最优解，导致错误配准，再利用蚁群优化算法对其结果进行全局优化，不仅减少粗配准的错误配对，还为后续的精配准处理初始化一个精度较高的初始值，进而缩短整个配准的时间；在精配准过程中，采用了对应点之间欧氏距离和曲率双约束条件icp迭代精配准处理，使得配准的精度得到一定的提高。

附图说明

图1是本发明一种全局优化与多约束条件迭代的3d彩色点云配准方法的流程图；

图2a是人工加噪的斯坦福bunny3d点云数据图；

图2b是人工加噪的斯坦福bunny3d点云数据滤波后的结果图；

图2c是使用kinect相机采集得点云数据图；

图2d是使用kinect相机采集得点云数据滤波后的结果图；

图3a是本实施例中待配准源点云和目标点云数据图；

图3b是现有基于sac-ia粗配准(正面)结果图；

图3c是现有基于pso粒子群的粗配准(正面)结果图；

图3d是本实施例中进行粗配准(正面)结果图；

图3e是现有基于sac-ia粗配准(侧面)结果图；

图3f是现有基于pso粒子群的粗配准(侧面)结果图；

图3g是本实施例中进行粗配准(侧面)结果图；

图4a是现有3dndt算法精配准(正面)结果图；

图4b是现有icp算法精配准(正面)结果图；

图4c是本实施例中进行精配准(正面)结果图；

图4d是现有3dndt算法精配准(侧面)结果图；

图4e是现有icp算法精配准(侧面)结果图；

图4f是本实施例中进行精配准(侧面)结果图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明一种全局优化与多约束条件迭代的3d彩色点云配准方法，如图1所示，具体按照以下步骤实施：

步骤1，采用基于标记的多状态离群点剔除原点云数据中的噪声、离群点及离群点簇；具体按照以下步骤实施：

步骤1.1，用k-d树进行点云的邻域搜索；

步骤1.2，经步骤1.1后，用移动最小二乘法局部表面拟合方法对点云法向量进行计算；

步骤1.3，经步骤1.2后，对不同点云数据设置不同的阈值，以点法线差值的正交分量的比值与设置阈值进行比较，以作为孤立点的判断条件，标记离群点；本发明的两个实例的阈值是bunny模型取值0.25，椅子模型取0.3；

步骤1.4，经步骤1.3后，以现有改进的dbscan聚类方法对标记离群点进行聚类；

步骤1.5，经步骤1.4后，统计每个聚类中标记离群点的数量占该聚类中点云总数的比值，当该比值大于经多次实验调整设定的比例阈值时，判定并删除离群点簇；本发明的两个实例的阈值是bunny模型取值0.2，椅子模型取0.33；

步骤1.6，重复步骤1.5，直到删除所有的离群点簇，同时删除剩余的标记离群点；得到滤除后源点云数据和目标点云数据；

图2a是人工加噪的斯坦福bunny3d点云数据，图2b是滤波后的结果，图2c是使用kinect相机采集得点云数据，图2d是滤波后的结果，由处理结果可以看出，该滤波方法不仅能够滤除人工噪音，而且还能有效滤除相机采集得3d点云数据。

步骤2，经过步骤1后，采用pso粒子群优化算法对滤除后源点云数据和目标点云数据进行粗配准处理；具体按照以下步骤实施：

步骤2.1，初始化pso粒子群算法的迭代次数m＝80，随机生成平移变量tx,ty,tz以及旋转变量rx,ry,rz，初始化学习因子c1＝2、c2＝2；

步骤2.2，经过步骤2.1后，采用单位四元数法求出源点云p和目标点云q的旋转变换矩阵r和平移矩阵t，并计算目标点云q旋转、平移后的矩阵q′；

具体按照以下步骤实施：

步骤2.21，计算每个源点云p与目标点云q的重心，如式(1)所示；

式(1)中，pi和qi是源点云与目标点云的点，i为其下标，表示第i个点，n为每个点云的总数，μp和μq分别为源点云p与目标点云q的重心；

步骤2.22，利用公式(2)计算每个源点云p和目标点云q的协方差矩阵m；

步骤2.23，根据协方差矩阵m，利用公式(3)构造4阶矩阵s；

步骤2.24，计算矩阵s的特征值和特征向量，并将最大特征值对应的特征向量作为所求的旋转变量r；

步骤2.25，利用公式(4)计算源点云p和目标点云q的平移矩阵t；

t＝μp-rμq(4)；

步骤2.26，经过步骤2.24和2.25后，获得旋转矩阵r和平移矩阵t，并利用r和t及公式(5)计算目标点云旋转、平移后的矩阵q′；

q′＝rq+t(5)；

步骤2.3，利用公式(6)计算适应度函数fitness；

fitness＝|rt(q-q′)|，i＝1,2,3,...,m(6)；

式(6)中，变换矩阵rt包含6个参数，平移变量tx,ty,tz以及旋转变量rx,ry,rz，q’为变换后的点云集；

步骤2.4，利用公式(7)计算源点云和目标点云对应点对的局部最小距离fpbest，表示粒子当前最佳位置；

f(pi，qi)＝|p′i-q′i|(7)；

式(7)中，p′i和q′i表示待配准点对pi和qi的k邻域的重心；

步骤2.5，令fp＝fpbest，fitnessbest＝fitness，rt＝rt(r0,t0)，fp表示粒子历史最佳位置，fitnessbest表示所有粒子的最佳位置函数，rt表示最佳变换矩阵，rt(r0,t0)表示初始化的变换矩阵，实现源点云和目标点云粒子群粗配准；

具体按照以下步骤实施：

步骤2.51，设置r1i＝rand()，r2i＝rand()，其中r1i和r2i都是伪随机数，rand()是随机函数，在(0,1)的均匀分布中生成随机数；

步骤2.52，以空间矩阵rt代替速度v，并利用公式(8)更新rt，对目标点云q进行空间矩阵变换；

式(8)中，d为维数，v为粒子i的速度，pi为粒子i在一次迭代中的最佳位置，c1和c2为加速器，设置为2；r1和r2是伪随机数，设置为0-1；

步骤2.53，利用公式(6)计算此次变换的适应度函数，并利用公式(7)计算源点云p与目标点云q的各点对的局部距离函数，并确定其中最小值fpbest；

步骤2.54，比较fp和fpbest，如果fpbest小于fp，则继续比较fitnebest和fitness，如果fitness小于fitnessbest，则fitnessbest＝fitness，fp＝fpbest；否则，fitnessbest不变；如果fpbest大于fp，则fp不变；

步骤2.55，经过多次实验调整，确定满足实验数据的最佳迭代次数m，如果迭代m次后，标记此时的最近点对集合；否则，重复步骤2.51至步骤2.55；

步骤3，经过步骤2后，得到初配准的初始配准点对，再采用蚁群优化算法对其进行全局优化处理，得到更加精准的粗配准结果；具体按照以下步骤实施：

步骤3.1，对目标点集中的点与点之间赋予初始信息素值c＝2，并在目标点云上随机选择n个点作为“蚂蚁”的初始位置；不同的点云数据，n的取值不同，本发明的两个实例n的值分别为：bunny模型n＝20，椅子n＝38；

步骤3.2，利用公式(9)计算每个“蚂蚁”点在其邻域内的状态转移概率并进行状态转移；

式(9)中，pij^k表示编号为k的蚂蚁从i位置向j位置的“转移”的概率；τij(t)表示t时刻位置i与位置j的信息素值；ηij通常设为i位置和j位置之间距离的倒数；jk(i)表示i位置上的蚂蚁k未访问的邻近位置点的集合；α,β为启发式因子，其中α＝0.9,β＝1.06，分别表示确定信息素浓度τij和距离间的评价参数；

步骤3.3，对所有“蚂蚁”点搜索到一条路径后，遍历目标点集中所有点；

步骤3.4，利用公式(10)进行信息素迭代计算；

式(10)中，ρ为信息素蒸发参数，ρ＝0.1，0＜ρ＜1；δτ^kij表示蚂蚁k在i位置和j位置之间信息素的浓度，并利用公式(11)计算获得；

式(11)中，和是选择的对应点对的法向量，且两法向量相似度高时，k表示第k个粒子的搜索过程；λ＝8为一个常数，是为保证信息素浓度函数的取值符合算法要求，否则信息素浓度函数会永远存在于区间[0,1]之间，无法进行后续算法实施；

步骤3.5，根据本算法中信息素浓度更新函数的具体实验结果及具体数据模型的多次实验结果进行调整，并确定最终比较的阈值；若路径存在更新的信息素值τij大于预先设定的阈值，则搜索结束，该路径(点集合)与源点云组成的点对作为配准的点对；否则重复步骤3.2至步骤3.4；不同的点云数据，其阈值设置不同，本发明的两个实例阈值分别设置为：bunny模型，阈值＝470，椅子模型，阈值＝760；

步骤3.6，利用单位四元数法求解最终选择的点对之间的空间变换矩阵，完成粗配准；配准结果如图3所示；

图3a是源点云和目标点云滤除后待配准的数据，图3b和图3e是采用基于sac-ia粗配准算法结果的正面图和侧面图，由该图可以看出，在模型坐垫部分融合比较杂乱，有较大程度的分离；图3c和图3f是采用基于pso粒子群的粗配准结果的正面图和侧面图，由该图可以看出，采集的数据模型在正面上平移误差较sac-ia粗配准算法结果大，但椅背处该方法的配准效果明显优于sac-ia粗配准算法结果，但从侧面的配准结果显示，该算法旋转误差更小，总的分析，pso粒子群算法粗配准在该模型效果优于基于sac-ia的粗配准结果；图3d和图3g是采用本发明的粗配准方法结果的正面图和侧面图，由该图可以看出，在模型坐垫部分层次分明且整齐，分离程度更小，源点云与目标点云的位姿调整的更合理，更有利于精确配准。

步骤4，经过步骤3后，得到优化后的粗配准结果，再采用改进的多约束条件迭代最近点精确配准算法对其进行精配准处理；

在多约束条件的icp精配准算法中，对源点云p与目标点云q预先设置最大迭代次数m＝200，目标函数阈值δ＝10^-6m，利用公式(12)表示迭代配准中的目标函数；

其中，ns为点云数量，r和t为两点云配准中的旋转矩阵和平移矩阵，pi和qi分别为源点云与目标点云中的点。

具体按照以下步骤实施：

步骤4.1，搜索源点云p和目标点云q，确定对应点对；具体按照以下步骤实施：

步骤4.11，搜索源点云p和目标点云q，使得欧式距离‖pi-qi‖＝min最小的点对为初始对应点对；

步骤4.12，利用公式(13)计算所有点对中每点的曲率，并将这些点对存储到集合ts{(pi,qi),i＝1,2,3,...,n′}中，该集合包含初始点对及这些点对的曲率信息；

式(13)中，k1(pi)、k2(pi)分别是点曲率的最大值和最小值，也称为点的主曲率；

任意一点p的主曲率具体实现步骤为：

设二次平面的一般方程，如公式(14)所示；

z＝f(x,y)＝a0x²+a1y²+a2x+a3y+a4(14)；

1.在目标点pi的k邻域内，根据最小二乘原理利用公式(15)该曲面方程的最小值；

2.由公式(14)和公式(15)求出该曲面的拟合曲面方程，拟合曲面方程由公式(16)表示；

r(x,y)＝(x,y,a0+a1x+a2y+a2x²+a4y²)(16)；

3.计算拟合曲面r(x,y)的偏导rx，ry，rxx，ryy，rxy，并利用公式(17)计算曲面的单位法矢量；

4.利用公式(18)和(19)分别计算平均曲率和高斯曲率；

其中，e＝rx·rx,f＝rx·ry,g＝ry·ry,

5.利用公式(20)和公式(21)计算点p的主曲率；

步骤4.13，利用公式(22)计算每对点对的曲率相似度ψ，并判断误差是否在取值区间之内，若在，保留该点对作为待配准的对应点对；若不在，搜索目标点qi的k邻域，并利用公式(13)计算邻域内各点的曲率信息；

ψ＝|s(pi)-s(qi)|(22)；

步骤4.14，计算点对中点pi和qi邻域各点的曲率相似度及欧式距离，将满足欧式距离最小且曲率相似度符合误差区间[0,0.08]的邻域点作为新的qi；若找不到满足以上两条约束的邻域点，则将该点对从集合ts中删除；

步骤4.15，将最终确定的点对在源点云与目标点云集合进行标记；

步骤4.2，目标函数最小化，并利用奇异值分解法计算此次迭代过程中的旋转矩阵rk、平移矩阵tk，利用公式(23)计算此次误差函数d；

其中，计算旋转矩阵rk、平移矩阵tk的具体步骤为：

步骤4.21，利用公式(24)表示空间变换矩阵函数；

步骤4.22，利用公式(25)计算源点云与目标点云的重心坐标，并利用公式(26)确定去重心化后的两个点集p′和q′，则利用公式(27)表示新的变换矩阵函数；

p′＝p-μp,q′＝q-μq(26)；

步骤4.23，对目标函数的总误差进行化简，化简后的函数由公式(28)表示，并对其进一步表示为公式(29)；

步骤4.24，要使目标函数变为最小，即使得tranc(rh)最小化；线性代数中，m是一个对称矩阵且b是正交矩阵，则有tranc(m)≥tranc(bm)，对h进行奇异值分解，得到公式(30)；

h＝u∑v^t(30)

式(30)中，∑为3阶非负元素对角矩阵；

步骤4.25，令旋转矩阵r＝vu^t时，rh为对称正定矩阵。此时的r为最优旋转矩阵，再利用公式(4)求出平移矩阵t；

步骤4.3，利用公式(31)更新目标点集；

q＝rkp+tk(31)；

步骤4.4，再次计算新的对应点对使之满足欧式距离最小，并判断点对是否被标记；若标记，则直接根据标记索引，在集合ts中找到该点对；否则按照步骤4.12计算点对的曲率信息并对这些点对进行归类或者删除；

步骤4.5，重复迭代并计算两次误差的差值|d-d’|，若满足|d-d’|＜δ，则算法结束；否则继续迭代，直至达到最大迭代次数；精配准执行的结果如图4所示；

图4a和图4d分别是采用3dndt算法精配准结果的正面图和侧面图，由该图可以看出，源点云与目标点云在采集数据的椅背处有明显的分离现象，扶手和坐垫处重合不完全，该算法精确配准误差较大；图4b和图4e是采用icp算法精配准结果的正面图和侧面图，由该图可以看出，该算法最终的配准结果误差比3dndt算法误差小，源点云与目标点云在采集数据的扶手、椅背顶端及坐垫处的垂直方向有分离的情况，该算法精确配准误差较小；图4c和图4f是采用本发明的精配准方法执行结果的正面图，由该图可以看出，该发明最终的配准结果误差最小，源点云与目标点云在采集数据的精确配准上，所有位置均得到更好的配准效果，源点云与目标点云的重合度最高。

将现有的粗配准方法和精配准方法和本发明的方法在多个性能方面进行了对比，对比结果如表1、表2所示。表1是将本发明的粗配准方法与现有的粗配准方法在运行时间和配准精度方面进行了对比。由表1可知，本发明粗配准算法比基于局部特征描述子fpfh结合sac-ia算法的配准时间更短，说明本发明的计算效率高于传统粗配准算法的配准效率。而且，从配准误差结果分析，可以确定本发明配准的精度比传统粗配准有较大提高，能够为精确配准提供更准确的初值。表2是将本发明的精配准方法与现有的精配准方法进行了对比，结果表明，本发明的配准误差都是最小的；在配准时间上，由于3dndt算法的机制，该算法耗时更短，该算法耗时更长，但与原始icp算法比较，本算法用时更短。综合分析，本发明3d彩色点云配准方法更优异。

表1各种粗配准算法的性价对比

表2各种精配准算法性价对比