基于特征点法向量的点云配准算法

孙培芪，卜俊洲，陶庭叶，房兴博，贺 晗，冯佳琪

(合肥工业大学土木与水利工程学院，安徽 合肥 230009)

三维激光扫描随着其技术的发展，正不断应用到各行各业中。利用三维激光扫描仪对被测物体进行多站、多次扫描，得到不同视角下的三维点云数据，将不同站的多片点云数据进行配准，即可得到完整的被测物体三维点云模型。目前点云配准已经在三维重建、科研医学、文物保护、测绘工程等领域得到了广泛的应用。

应用最广的点云配准算法是文献[1]中提出的迭代最近点算法(lerative closest point,ICP),该方法是目前点云配准的算法基础，后人在该算法的基础上进行改进、延伸。文献[2]提出了基于八叉树结构对ICP算法点云配准方法进行改进，加快了点云的搜索速度；文献[3]提出了基于对偶四元数的三维空间坐标转换点云配准方法，更加适用于大角度变换的点云配准工作；文献[4]提出了基于采样一致性(SAC- IA)的点云配准技术，加快了点云配准的收敛性。但是，上述方法并没有解决配准时容易陷入局部最优这一问题。

综上，本文提出一种基于特征点法向量之间的欧氏距离及夹角的点云配准方法。该方法在粗配准阶段，首先利用SIFT算法提取特征点；其次根据特征点的法向量之间的欧氏距离对两片点云的特征点进行自动匹配；然后根据拓扑关系，即特征点法向量之间的夹角对特征点对进行提纯；最后利用单位四元数法将两片点云进行旋转、平移，为后面的精配准工作提供良好的初始位置，可以有效解决配准时陷入局部最优的问题，也缩短了精配准的时间。

1 粗配准

在进行精配准之前，需要先将待配准的两片点云进行粗配准，以使其获得良好的初始位置。

1.1 SIFT算法提取特征点

SIFT算法是David Lowe于1999年提出的局部特征描述子，并于2004年进行了更深入的发展和完善[5]。SIFT特征是基于尺度不变性的图像局部特征，对平移、旋转、尺度缩放、亮度变化、遮挡和噪声等具有良好的不变性，对视觉变化、仿射变换也保持一定的稳定性，特别适用于在海量数据库进行快速、准确的匹配。

SIFT算法的原理[6]可以简单描述为：一幅图像在尺度不同的尺度空间L(x,y,σ)定义为一个变化尺度的高斯函数G(x,y,σ)与原图像I(x,y)的卷积，即

L(x,y,σ)=G(x,y,σ)*I(x,y)

(1)

式中，(x,y)表示图像的像素位置；*为卷积运算；σ是尺度空间因子。使用高效的高斯差分算子D(x,y,σ)进行极值检测，以此来选择出尺度空间中的稳定关键点,具体如下

D(x,y,σ)=(G(x,y,kσ)-G(x,y,σ))*I(x,y)=

L(x,y,kσ)-L(x，y,σ)

(2)

式中，k为一个常量。特征点是由DoG空间的局部极值点组成的，通过同一组内各DoG相邻两层图像之间比较完成特征点的探查。为了寻找DoG函数的极值点，每一个像素点要与同尺度的8个相邻点和上下相邻尺度对应的18个点共26个点进行比较，如果该像素点的DoG算子值在这26个邻域中为极值，则将该点定义为特征点。目前得到的特征点是离散空间的极值点，还要通过拟合三维曲面来精确确定特征点的位置和尺度，同时去除低对比度的特征点和不稳定的边缘点，以增强匹配稳定性，提高抗噪声能力。

利用DoG函数在尺度空间的泰勒二次展开式进行最小二乘拟合，公式下

(3)

式中，X=(x,y,σ)T。求导并让方程等于0，可以得到极值点的偏移量为

(4)

1.2 特征点匹配

两片点云公共部分的特征点提取出之后，要将特征点进行两两匹配，采用特征点的法向量之间的欧氏距离作为两片点云中特征点相似性判定度量[7]。选取源点云中某一特征点，计算出该点的法向量ni与目标点云的所有特征点的法向量nj之间的欧氏距离，并相互比较选取出欧氏距离最小的两个点。如果最小距离与次最小距离相比小于预先设定的某个阈值τ(τ>0)，则目标点云中与源点云中特征点法向量欧氏距离最近点为一对特征点对。若降低阈值，则匹配点对数量会减少，但是会增加匹配的稳定性，故剔除一部分错误匹配点对。

需要注意的是，每个特征点的法向量是具有正负之分的[7]。因此，为保证扫描对象表面法向量方向的一致性，对所有求得的法向量进行方向调整，使之满足ni·nj<0(i≠j)，使得特征点对的匹配更加准确。

1.2.1 求解特征点法向量[8]

某一点法向量的求解是基于该点所在的曲面而获得的，而点云表征的是一个个离散点，点云数据所记录的信息是每个独立点的三维坐标。因此，为了求得每个特征点所对应的法向量，取该点附近一定半径内的点进行曲面拟合，在拟合面的基础上求取目标点的法向量。求解过程如下：

(1) 待拟合的n个扫描点(xi,yi,zi),i=1,2,…，n，拟合的平面方程为

ax+by+cz=dd≥0

(5)

式中，a、b、c应满足a2+b2+c2=1。且任一点(xi,yi,zi)到该平面的距离为

di=|axi+byi+czi-d|

(6)

(7)

(3) 将f分别对4个未知参数d、a、b、c求偏导，从而求出未知参数d，即

(8)

(4) 此时任一点到平面的距离可以改写为

(9)

(10)

(5) 对式(10)继续求关于a、b、c的偏导

(11)

(12)

同理

(13)

(14)

(6) 将式(12)—式(14)合并改写为

(15)

求解出(a,b,c)T即为该点在该平面的法向量。

1.2.2 求解特征点法向量之间的欧氏距离

从源点云中选取一个特征点，其法向量为(ai,bi,ci)T。从目标点云中选取一个特征点，其法向量为(aj,bj,cj)T，则公式为两个法向量之间的欧氏距离计算

(16)

1.3 特征点对提纯

在特征点配对时设置的阈值，并不能将特征点完全正确地一一对应，误匹配的现象始终无法避免。在理论上，只要提取出3对正确的匹配点对，即可求解出准确的两片点云之间的旋转、平移矩阵[9]。但若其中1对特征点对存在误匹配，则计算出的变换矩阵与实际变换矩阵之间便会存在很大的差异。因此，为了保证求解出源点云与目标点云之间的旋转与平移矩阵的准确性，需要将配对后的特征点对进行更深一步的提纯工作。

目前，匹配点提纯算法常用的是随机抽样一致性算法(random sample consensus，RANSAC)[10]，采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。数据分为有效数据与无效数据两种。随机抽样一致性算法就是根据不同的数据建立不同的模型，满足该模型的数据为有效数据，不满足该模型的数据为无效数据，从而剔除无效数据，将误匹配的特征点对进行排除。从结果上来看，该算法获得的模型数据稳健性较强，估算出的参数精度相对较高。但是，该算法每次针对不同的数据，所拟合的模型均不相同，故不适用于复杂的点云场景。并且，计算模型参数的迭代次数没有上限，如果设置迭代次数的上限，得到的结果可能不是最优结果，甚至可能会得到错误的结果。故本文采取一种根据特征点对之间法向量的夹角作为评判量的匹配点提纯算法。

考虑源点云与目标点云虽然处于不同的空间坐标系，但其空间拓扑关系应该保持一致，相对应的点云之间除了有平移变化，还应有旋转关系，特征点对的法向量之间的夹角应满足一定的数学关系[11]。因此，预先对特征点对法向量的夹角设置一个阈值G，用于对误匹配点进行剔除，从而达到提纯的效果。

对于任意特征点对A和B，它们的法向量分别为nA和nB，它们之间的法向量差别越大，其夹角的余弦值越小，即nA·nB就越小。因此，根据式(17)对法向量之间的夹角余弦设置阈值G，将法向量的乘积小于G的点对视为误匹配点，并将其剔除。

nA·nB≥G

(17)

1.4 点云坐标转换

当源点云与目标点云的特征点完成一一对应后，便要利用特征点对求解源点云到目标点云的平移和旋转矩阵。常用的三维坐标转换方法是利用布尔沙模型求得七参数[12]，其中包括3个坐标平移参数、3个角度旋转参数及1个尺度参数。由于在点云配准问题中，不涉及点云尺度的大小变换，因此仅使用前6个参数对源点云进行平移、旋转。但是，该方法含有线性化后产生的不稳定误差，仅在小角度转换情况下，可以将转换参数的误差忽略，而在坐标转换角度较大时，其精度则达不到转换的要求。故本文选用单位四元数法，该方法更适用于大角度三维坐标转换。

四元数由哈密顿于1843年提出，其实质是1种简单的超复数，由1个实部单位和3个虚部单位构成。使用单位四元数法进行点云三维坐标转换时，必须找到两片点云中的公共部分，即求得源点云与目标点云的重叠部分的旋转、平移矩阵，从而应用到源点云整体部分的转换[13]。其使用的具体条件为：①分别选取源点云与目标点云的公共部分A和B，且A和B两片点云中的点云数量相等；②A和B两片点云中的点云要满足一一对应的关系，即Ai与Bj代表着在不同坐标系下的相同点位。在前文中，将源点云与目标点云公共部分的特征点进行一一匹配，获得的匹配点对即可满足单位四元数法的使用要求。

单位四元数法的具体算法如下[14]：

(1) 分别计算A和B点云的重心，重心坐标为ZA与ZB

(18)

式中，NA与NB分别为A与B中点云的数量。本文要求两片点云中的点云数量相等，即NA=NB。

(2) 求A与B的协方差矩阵，即

(19)

由式(19)所获得的协方差矩阵，构成一个4×4对称阵Q(R)

(20)

(21)

(4) 求得旋转矩阵后，即可求解出平移矩阵qT

qT=ZA-R(qR)ZB

(22)

旋转矩阵R(qR)与平移矩阵qT即为源点云与目标点云之间的变换关系。利用该旋转、平移矩阵可调整源点云的三维坐标，为后面的精配准工作提供了较好的初始位置。

1.5 粗配准算法整体流程

(1) 找出源点云与目标点云的公共重叠部分A与B。

(2) 使用SIFT算法提取出A与B的公共点。

(3) 计算每个特征点的法向量，并统一方向。

(4) 将A中某一特征点的法向量与B中所有特征点的法向量计算欧氏距离并选取出距离最近的两个点，其距离比值若小于阈值，则距离最小的两个点进行匹配。

(5) 根据点云的空间拓扑关系，计算已配对的特征点之间的法向量夹角。根据数学关系，若大于设定的阈值，则视为误匹配点，予以剔除。

(7) 将求得的旋转、平移矩阵应用于源点云。

2 精配准

经过粗配准以后，源点云与目标点云已经获得了较好的初始位置，在此基础上采用ICP迭代算法进行精配准。ICP算法根据源点云与目标点云之间的几何特征求解其运动参数(即旋转与平移)，再代入这些运动参数对数据进行转化，得到新的目标点云与源点云，从而继续确定配准点云之间新的对应关系，不断重复上述过程。当源点云与目标点云经过不断旋转与平移，从而达到目标函数最小，即满足最小二乘定理时，那么配准效果达到最优[15- 16]。目标函数表示如下

(23)

式中，T、R分别表示平移与旋转参数；Pi与Qi分别代表目标点云与源点云。

3 试验分析

为验证上述方法的点云配准效果，本文采用经典的斯坦福兔子作为试验素材。将两个不同视角的斯坦福兔子点云数据分别作为源点云与目标点云，其中亮色的为源点云，暗色的为目标点云。本文算法使用C++编程实现，程序运行环境为Intel(R) Core i5- 7500处理器，内存16 GB。为了验证在交换角度较大时本文算法的可靠性，设置其初始位置如图1所示。

在本次试验中，根据试验数据将特征点匹配的阈值τ1设定为0.05，将匹配点对提纯的阈值τ2设定为0.8。试验中，每个兔子点云有35 947个点，提取出的特征点为1280个点。在此基础上，满足阈值要求，最终匹配成功的特征点对为209对。将特征点对代入四元数法后，求得旋转、平移矩阵，并应用于源点云，完成粗配准，如图2所示。

粗配准后精配准结果如图3所示，直接ICP配准结果如图4所示。将精配准所用时间、迭代次数、精度与直接ICP所用时间、迭代次数、精度进行对比，见表1。

表1 精配准与直接ICP精度对比

从图4中可以看出，在大角度转换的情况下进行直接ICP配准，使得两片点云陷入了局部最优。而先经过粗配准后，再使用ICP算法，配准结果得到了很大的改善，两片点云几乎完全重叠。另外，由于为ICP算法提供了较好的初始位置，因此相应的迭代次数与迭代时间都进一步缩短，配准误差进一步降低。

4 结 语

本文首先使用SIFT算法提取源点云与目标点云的特征点，并根据特征点的法向量之间的空间拓扑关系对特征点进行配对、提纯；接着利用单位四元数法，根据匹配成功的特征点对，计算出源点云与目标点云的旋转、平移矩阵，为两片点云进行粗配准。粗配准的结果为精配准提供了良好的初始位置，改善了ICP算法对于大角度转换情况下的配准工作容易陷入局部最优的问题。从过程与结果来看，本文算法不仅缩短了ICP配准时间，还降低了误差。同时，需要注意的是，虽然ICP配准时间得到了改善，但是粗配准时使用SIFT算法进行提取特征点仍需要耗费一定的时间，这也是日后该算法需要优化的地方。