基于特征点对优化筛选的点云初始配准算法

郭 敏， 孙 梦， 吕源治， 李贞兰

(1.长春工业大学 计算机科学与工程学院， 吉林 长春 130102;2.中国科学院长春光学精密机械与物理研究所， 吉林 长春 130033；3.吉林大学， 吉林 长春 130012)

0 引 言

随着物联网技术和三维立体视觉的不断发展，三维点云数据处理技术在人工智能、虚拟重建、机器视觉等领域得到广泛应用[1-3]。点云配准技术是点云处理技术的关键部分，点云配准质量是影响三维重建效果的重要因素。

自动配准、手动配准和依赖仪器配准是点云配准方式的三种形式[4]。目前主要采用自动配准技术，最常用的方法是由 Besl P J等[5]提出的迭代最近点(Iterative Closest Point, ICP)算法，它是当前点云配准过程中应用最广泛的算法。虽然该迭代算法精确度较高，但对两组点云的相对初始位置要求较高且容易陷入局部最优解[6]。针对此问题，目前点云配准主要采用先进行初始配准再进行精配准的方式[7]。初始配准算法主要包括主成分分析法、基于特征的配准算法以及基于投票机制的配准方法。基于特征的配准是指利用物体自身的点、线、面等几何特征或纹理特征解计算变换参数[8]，它是目前应用最为广泛的初始配准算法，主要分为特征检测、特征描述和特征匹配三个步骤。特征描述步骤中应用最为广泛的是Rusu R B等[9]在2009年提出的一种FPFH用于描述点云特征。在特征匹配阶段，RANSAC算法是一种应用较为广泛的特征匹配方法[10]，但存在有限次随机性带来的不稳定性和计算量较大等弊端。当输入的点云中相似特征点较多时，仅依靠特征描述的相似程度搜索对应点[11]，会出现较多的错误匹配点对，导致后续的点云配准结果较差。

针对上述问题，提出一种基于特征点对优化的初始配准算法，该算法能够在优化对应点集的同时，规范化阈值的选取，减少了对初始配准结果的影响，为点云的精配准提供精确度较高的初始对应点集，从而实现对三维点云数据配准的目标。同时文中又基于三维坐标转换的基本理论，罗德里格矩阵在迭代求解点云配准参数中的应用。

1 配准原理

三维点云初始配准的关键在于获取较高精度的初始位置和精确的对应点对。为此，文中采用基于点云特征实现初始配准的算法流程如图1所示。

图1 点云初始配准算法流程

首先，对输入的两组点云进行法线估计；其次，基于点云的曲率实现特征点的提取；再次，根据特征点建立点云的快速特征直方图；然后，基于已经得到的直方图实现初始对应点集的建立，并进行优化选取；最后，利用罗德里格斯旋转公式求解初始变换位姿，以实现点云初始配准的目标。

1.1 计算点云法向量

采用主成分分析(Principal Component Analysis, PCA)[12]算法实现点云的法向量估计，首先，构建点云的KD树并计算其邻域内的中心点坐标;其次，构造点云的协方差矩阵,并计算该矩阵的特征值和特征向量;然后，将所求得的特征值按照从小到大的顺序进行排序，最小特征值对应的特征向量就是点云的法向量；最后，对求得的法向量进行检验与调整，使得所有点云数据的法向量方向一致。

1.2 提取特征点

曲率能够描述曲线或者曲面的弯曲程度，在三维点云中可以用来描述多个点所构成曲面的局部性质[13]，常用的曲率有高斯曲率、平均曲率和主曲率。三维空间中某点的平均曲率表示曲面在该点的平均弯曲程度，文中基于平均曲率实现特征点的提取。

首先，将点云中第c个点Pc(c=1,2,…,N)(N表示点的个数)的曲率估计值为

(1)

式中：λ1,λ2,λ3----第c个点与其e个邻域点所建立协方差矩阵的特征值，满足λ1≤λ2且λ1≤λ3。

然后，设定曲率阈值

(2)

最后，根据曲率估计值dc与阈值T大小的比较，得到特征点集与非特征点集：曲率估计值大于阈值的点标记为特征点；否则，标记为非特征点，所有特征点组成特征点集,非特征点组成非特征点集。

1.3 建立快速点特征直方图

快速点特征直方图(FPFH)是一种能够描述特征点邻域内几何特征的描述子，利用多个不同维度的直方图来描述特征点的法向量特征，从而描述特征点邻域内的几何特征信息。在三维空间中，空间内任意一点与其K邻域内的每一个点两两连接，将点与点之间的几何特征定量化表示，基于点云的三维坐标信息与法向量之间的相互作用关系，描述出点云的几何特征。查询点Pq与其K邻域点的FPFH计算关系如图2所示。

图2 查询点Pq与其邻域点FPFH描述子计算原理图

FPFH描述子仅在查询点与邻域点之间建立几何关系，为了更好地描述任意两点Ps与Pt对应法向量之间的关系，选择其中一点作为坐标原点，在两点间构造局部坐标系，如图3所示。

图3 两点间局部坐标系

首先分别查询两点的法向量，其次以法向量为其中一个坐标轴，然后根据法向量求剩下两个坐标轴的单位向量，再根据向量求其夹角。三个单位向量u，v,w计算如下:

(3)

根据三个单位向量u，v,w可计算法向量与局部坐标系各坐标轴之间的夹角。

(4)

α,φ,θ这三元组也被称为简化点特征直方图，记作SPFH。对于某个查询点Pq来说，其FPFH的建立过程如下：

1)求该点的简化点特征直方图SPFH(Pq)。

2)利用已经得到的SPFH(Pq)并根据下列公式计算FPFH特征

FPFH(Pq)=SPFH(Pq)+

(5)

式中：ωk----权重;

k----邻域点的个数。

1.4 优化筛选特征点对

基于点云的特征直方图寻找源点云与目标点云间的对应点对，采用最邻近搜索方式查找源点云与目标点云中特征直方图相似的点作为初始对应点集，设H(P)表示源点云的特征直方图，H(Q)表示目标点云的特征直方图。对于源点云中的一点p在H(Q) 中能找到与H(p) 极为相似的最邻近点, 同理，对于目标点云中的一点q在H(P) 中能找到与H(q)极为相似的最邻近点，组成初始的对应点集A1。

由于此时对应点集的误差较大，所以要对其进行两次优化筛选。

1.4.1 一一对应原则

在获取初始的对应点集时，是基于点特征直方图的相似程度来确立的，存在源点云中的一点(目标点云中的一点)对应目标点云中多个点(源点云中多个点)的情况。此时需要优化点云以实现点云一对一的目标，即对于初始对应点集中的一组点对(p,q)需满足p的最邻近点是q,q的最邻近点是p, 优化后得到新的点集为A2。

1.4.2 近似等距离原则

等距离原则是源点云中的一点p1到目标点云的对应点q1的距离等于源点云中另一点p2到其目标点云中对应点q2的距离，同时源点云中p1与p2的距离等于目标点云中q1与q2的距离，两点集中点间距离模仿图如图4所示。

图4 两点集中点间距离模仿图

(p1,q1)、(p2,q2)为模拟点云中正确的对应点对，(p3,q3)是一组错误点对。

由于实际中存在误差，不能达到距离值完全相等的目标，所以设定一个容忍小范围误差的阈值ε1和ε2，以剔除点集A2中错误的对应点对。

其中，ε1的计算过程如下。首先，设L={l1,l2,l3,…,lF-1},表示两组对应点中源点云两点欧式距离与目标点云中两点欧式距离比值的集合，其中F表示A2对应点对的个数，L中集合参数可由下式计算

(6)

然后，根据式(6)可得ε1的表达式为

(7)

同理也可得到ε2，设M={m1,m2,m3,…,mF-1}表示两组对应点中源点云两点欧式距离与目标点云中两点欧式距离比值的集合。M中集合参数可根据下式计算

(8)

然后，根据式(8)计算

(9)

对于A2中的任意两组对应点对需满足

|lf-1| ≤ε1且 |mf-1| ≤ε2,

(10)

经过上述两次优化筛选得到最终的对应点集。

2 点云初始配准

为计算点云配准的旋转和平移矩阵，需要经过多次的迭代遍历过程，输出使配准误差值最小情况下的旋转矩阵和平移矩阵。具体步骤如下：

1)随机选取n(n≥3) 组对应点；

2)通过罗德里格斯公式求旋转矩阵和平移向量[14]；

3)根据式(11)计算此时的配准误差为

(11)

式中:Qb----目标点云旋转后的点集;

G----对应点对的个数;

4)重复上述3个步骤，直到满足迭代次数为止，输出最小误差对应的旋转矩阵和平移向量。

通过罗德里格斯公式求旋转矩阵和平移向量的过程。

(12)

式中：D=1-cosθ。

根据旋转关系，将目标点云中一点(X,Y,Z)转换到源点云坐标系下，得到点(X′,Y′,Z′),两个坐标点之间的关系为

(13)

式中：t----平移向量;

R----旋转矩阵。

通过反对称矩阵可以构建罗德里格矩阵表达为

R=(I+S)(I-S)-1,

(14)

式中：S----旋转向量对应的反对称矩阵;

I----单位矩阵。

设旋转向量为

a=[ax,ay,az]T。

根据已有的任意三个公共点，求解转换参数，(Xi,Yi,Zi)和(Xj,Yj,Zj)两组对应点对式 (11)相减得到：

(15)

对式(15)代入R,并整理转换可得到：

1)令

A和B中Δ表示求得的差值。

ΔXij=Xi-Xj,

随机选取3组及以上点云时,计算得到的矩阵A和矩阵B分别垂直合并,则AS=B，根据最小二乘原理可求得a=(ATA)-1ATB，对其进行单位化得到旋转轴O。

2)旋转角的取值为

式中：max(a)----a中列向量的最大值。

3)将求得的参数代入罗德里格斯旋转矩阵的展开式,可得到旋转矩阵，根据旋转前后两坐标点的关系，可求得平移向量。

3 实验结果及分析

3.1 实验环境与数据来源

实验采用计算机硬件环境为Intel(R) Core(TM) i5-5200U CPU @ 2.20 GHz 2.19 GHz，4 GB内存，软件环境为Windows 10 操作系统，使用Matlab编程软件。采用长春光机所自主研发的SVision751B型三维激光扫描仪对机器猫石膏像进行点云数据采集，原始点云数据及点云封装图分别如图5和图6所示。

图5 原始点云

图6 点云封装图

两图中左边均为目标点云，右边为源点云。

3.2 实验结果与分析

为减少因数据冗余造成计算效率低下的问题，对图5中的原始点云数据进行了精简，以精简后的点云作为输入的点云数据集，按照图1步骤完成点云配准实验。源点云与目标点云法向量的求取结果，法向量的方向统一指向点云外侧，源点云与目标点云的法向量如图7所示。

图7 源点云与目标点云的法向量

源点云与目标点云的特征点如图8所示。

图8 源点云与目标点云的特征点

特征点个数与输入点云个数对比见表1。

源点云中提取到的特征点有8 424个，目标点云中提取到的特征点有8 082个。

利用特征直方图，在目标点云与源点云中确立初始对应点集，共计8 424对，第1～1 000组对应点集如图9所示。

图9 优化前的对应点集(第1～1 000组)

从图中可以看出，此时的对应点集中存在较多的错误点对，运用文中提出的特征点对优化筛选算法进行处理后，得到新的对应点集，如图10所示。

图10 优化后的对应点集

相比图9，新点集中对应点对质量较高。

记录优化筛选前后点对个数的变化见表2。

通过数据对比发现，初始对应点集中错误点对的数量较多，若不对其优化筛选，将直接影响后续点云初始配准的精度，甚至导致配准失败。

表2 对应点对的数量

为进一步验证文中算法在精度及稳定性方面的性能，将所研究算法与基于RANSAC思想剔除错误点对的初始配准算法的实验结果对比分析见表3。

表3 不同迭代次数下配准误差对比

实验中不断调整迭代次数，并分别记录两种算法在不同迭代次数下的配准误差，对比发现，文中算法的配准误差相对较低。迭代次数为1 000时，两种算法的配准结果如图11所示。

(a) 文中算法

结合表3中该迭代次数下的配准误差对比分析(a)和(b)两图可发现，文中算法略优于基于RANSAC思想剔除错误点对的初始配准算法。

4 结 语

针对源点云和目标点云中特征点对容易产生错误匹配，从而导致初始配准精度较低的问题，文中对基于特征点对筛选优化的点云初始配准算法进行了研究，实验结果表明，该算法的误差较小，精确度较高,且稳定性较强。