0 概述

基于点云数据对现实生活场景进行感知是计算机视觉领域研究的热点。点云特征包括全局特征与局部特征, 前者多应用于三维物体识别, 而后者在位姿配准中应用较多。文献[1-3]提出点特征直方图(Point Feature Histograms, PFH)特征提取算法, 该算法具有较好的特征提取能力, 但运算速度较慢。为提升PFH运算速度, 文献[4]提出快速点特征直方图(Fast Point Feature Histograms, FPFH)特征提取算法, 该算法可提升特征提取的运算速度, 节省运算空间, 减少数据处理的迭代步数, 因此广泛应用于三维物体的识别及配准。文献[5]提出基于ICP的配准算法, 该算法使用FPFH点云特征对钢质零件的位姿信息进行求解。文献[6]基于FPFH特征设计了点云配准系统, 以对由多个深度相机同时拍摄所得的点云或由单个相机从不同角度拍摄所得的多幅点云进行配准。文献[7]使用FPFH特征进行粗配准, 在求出初始位姿的基础上进行ICP配准。文献[8]使用FPFH特征对人体各部位点云进行提取并完成识别匹配。

上述基于点FPFH特征的提取算法在计算点云FPFH特征时未考虑邻域半径的选取标准, 未能根据点云的特点自动鉴别出合适的邻域半径。另外, 通过手动多次调试以取得邻域半径的方法效率较低。为此, 本文提出一种基于点云弧长密度的FPFH特征提取算法, 根据点云弧长密度自动鉴别出合适的邻域半径, 以用于FPFH的特征计算。

1 基于点云弧长密度的FPFH特征提取算法 1.1 特征提取算法

基于点云弧长密度的自动邻域半径鉴别FPFH特征提取算法包括2个部分, 分别为求解半径函数与计算FPFH特征, 如图 1所示。

在求解半径函数过程中, 首先计算多对点云的弧长密度, 根据弧长密度以手动选择的方法找出多个邻域半径, 计算点云FPFH特征并完成采样一致性初始配准(Sample Consensus Initial Aligment, SAC-IA), 提取配准效果最好的邻域半径和弧长密度, 使用最小二乘法拟合邻域半径与弧长密度的函数关系, 称为半径函数。将半径函数与FPFH特征提取算法相结合, 求出点云弧长密度并输入给半径函数, 进而自动鉴别得到合适的邻域半径以计算FPFH特征。本文算法在计算PFFH特征时不再使用人工手动选择邻域半径的方法, 根据点云本身的弧长密度特点自动鉴别出合适的邻域半径, 从而达到全自动的目的。

1.2 点云弧长密度 1.2.1 点云密度估算

目前对点云密度的估算方法有2种:基于距离的方法与基于分块的方法^[9]。基于距离的点云密度求解方法是以点云内各点间的欧氏距离为度量标准衡量点云中点的分布情况。而基于分块的方法是将点云均匀分成若干块, 计算出分布在各块点的点云数目并进行取舍组合, 进而得到点云的密度估算值。但是以上2种方法均是对点云分布粗略的估算, 不能较好地体现点云中点的分布特性, 因此本文提出点云弧长密度提取方法, 以点云分布表面的曲面特性为基础估算点云分布情况, 能较好地体现点云分布的本质特征。

1.2.2 点云法线估算

计算点云的弧长密度要先计算点云各点的法线。目前, 估算点云法线的方法有曲面重建和直接估算法。曲面重建通过重建点云模型, 得到待求点所在物体表面的拟合曲面, 然后计算得到法线。直接估算法则直接根据点的邻域估计点的法线。本文使用的方法属于直接估算法, 通过在点云中某点的k-邻域内使用PCA算法进行求解^[10]。对于点云P中的一点p_i=(x_i, y_i, z_i), i=1, 2, …, n, 在其k-邻域内构建协方差矩阵有:

$ {\mathit{\boldsymbol{A}}_{3 \times 3}} = \frac{1}{n}\sum\limits_{i = 1}^n {\left( {{\mathit{\boldsymbol{p}}_i} - \mathit{\boldsymbol{\bar p}}} \right)} {\left( {{\mathit{\boldsymbol{p}}_i} - \mathit{\boldsymbol{\bar p}}} \right)^{\rm{T}}} $

(1)

其中, p= $\frac{{1}}{{n}}\sum\limits^n_{i=1}$(p_xi, p_yi, p_zi)为k-邻域内所有点的质心, n为点云中点的个数。式(1)中协方差矩阵能够较好地描述点云局部几何信息。通过对A进行奇异值分解(Singular Value Decomposition, SVD)^[11]求解出特征值λ_i(i=0, 1, 2), 并按照由大到小的顺序进行排列, 即λ₂≥λ₁≥λ₀≥0, 各个特征值对应的特征向量为v₂、v₁、v₀。PCA算法的作用是将数据集映射到低维空间, 数据集的特征值(在SVD中用奇异值表征)按照重要性排列, 降维的过程就是舍弃不重要的特征向量的过程, 而剩下的特征向量组成的空间即为降维后的空间。因此, 前2个特征值所对应的特征向量可以描述一个点云局部表面的变化特性。对于给定的三维点云中的点, 可以描述一个平面, 如式(2)所示。

$ ax + by + cz + d = 0 $

(2)

其中, (a, b, c)为该平面的法向量, d为平面到坐标原点的距离, 从而可知与最小的特征值λ₀对应的特征向量v₀为对应于点p_i的平面的法向量n_i。

为确定法向量的方向, 取点云采集设备的位置为视点v_p=(x_vp, y_vp, z_vp), 当视点满足式(3)时, n_i的取向正确。

$ {\mathit{\boldsymbol{n}}_i} \cdot \left( {{\mathit{\boldsymbol{v}}_p} - {\mathit{\boldsymbol{p}}_i}} \right) > 0 $

(3)

当视点满足式(4)时, n_i取向为原法向量的反向n_i=－n_i。

$ {\mathit{\boldsymbol{n}}_i} \cdot \left( {{\mathit{\boldsymbol{v}}_p} - {\mathit{\boldsymbol{p}}_i}} \right) < 0 $

(4)

图 2所示为点云表面法线示意图。可以看出, 由现实生活中的物体采样生成的点云并不光滑, 点与点之间具有一定的弧度特性, 即使所采样的物体对象表面为平面, 所生成的点云并不严格平滑, 具有丰富的凹凸几何性质。现实生活的物体一般表现为空间曲面特性, 如图 3所示。因此, 与基于距离的方法与基于分块的方法相比, 使用点云中点与点之间的弧长来描述点云中点的分布疏密情况更能保持物体的几何性质。

1.2.3 弧长密度求解

点云的弧长密度可以近似地体现出物体表面的空间关系特征、形状特征及疏密特性, 计算点云的弧长密度, 需要先计算每组点对的弧长。将点云中的2点p_i和p_j近似看成空间球体表面上的2点, 以图 3(a)中公鸡的点云为例, 局部放大公鸡的点云, 将球体表面上2点p_i、p_j与球心相连, 得到的空间几何位置关系如图 4所示。

对于点云中一点p_i, 求解所得单位法向量为n_i, 在点云邻域半径为r的范围内距离点p_i最近的点为p_j, 对应的单位法向量为n_j, 2点的欧氏距离为d。

由图 4可得:

$ {\mathit{\boldsymbol{n}}_i} \cdot {\mathit{\boldsymbol{n}}_j} = \cos \varphi $

(5)

则有:

$ \varphi = \arccos \left( {{\mathit{\boldsymbol{n}}_i} \cdot {\mathit{\boldsymbol{n}}_j}} \right) $

(6)

同时, 由三角形的边角关系可得:

$ r = \frac{d}{{\sqrt {2(1 - \cos \varphi )} }} $

(7)

综合上述各式, 可得到弧长l_i:

$ {l_i} = \varphi r $

(8)

从而可得到平均弧长l:

$ \bar l = \frac{1}{n}\sum\limits_{i = 1}^n {{l_i}} $

(9)

弧长密度是将弧长l作为点云分布疏密的衡量标准。对于相对光滑的平面, 某些点之间所计算的单位法向量可能相互平行, 角度φ的值为0, 不能使用弧长密度来表示点云的密度, 此时可使用2点的欧氏距离d代替平均弧长l。然后将φ的值为0和不为0这2种情况进行综合求解得到点云整体弧长密度, 描述点云分布的同时, 可保持点云表面的曲面性质。

1.3 点云FPFH特征与SAC-IA

点云FPFH是对点特征直方图PFH的加速。PFH通过统计点云内各点的k-邻域内点对的法线相对角度关系来形成直方图, 计算原理如图 5所示。其中, p_q是查询点, 该查询点位于半径为r的球心, p_q的k-邻域点均位于球内, 邻域内所有点两两相连构成点对。

要计算点云中任一个点p的PFH特征, 首先要建立局部坐标系和求解点云的法线, 步骤如下:

1) 搜索该点位于半径为r的球内的k-邻域点;

2) 对于k-邻域内的任意点对p_i与p_j(i与j不相等), 对应的法线分别为n_i与n_j, 在p_i上定义局部坐标系uvw, 如图 6所示, 坐标系的方向与法向量和点之间的关系如式(10)所示。

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{u}} = {\mathit{\boldsymbol{n}}_i}}\\ {\mathit{\boldsymbol{v}} = \frac{{{\mathit{\boldsymbol{p}}_j} - {\mathit{\boldsymbol{p}}_i}}}{{\left\| {{\mathit{\boldsymbol{p}}_j} - {\mathit{\boldsymbol{p}}_i}} \right\|}} \times \mathit{\boldsymbol{u}}}\\ {\mathit{\boldsymbol{w}} = \mathit{\boldsymbol{u}} \times \mathit{\boldsymbol{v}}} \end{array}} \right. $

(10)

3) 基于uvw坐标系, 根据式(10)计算p_i与p_j法线n_i与n_j之间的3个变量值:

$ \left\{ {\begin{array}{*{20}{l}} {\alpha = \mathit{\boldsymbol{v}} \cdot {\mathit{\boldsymbol{n}}_j}}\\ {\phi = \mathit{\boldsymbol{u}} \cdot \frac{{{\mathit{\boldsymbol{p}}_j} - {\mathit{\boldsymbol{p}}_i}}}{{\left\| {{\mathit{\boldsymbol{p}}_j} - {\mathit{\boldsymbol{p}}_i}} \right\|}}}\\ {\theta = \arctan \left( {\mathit{\boldsymbol{w}} \times {\mathit{\boldsymbol{n}}_j},\mathit{\boldsymbol{u}} \times {\mathit{\boldsymbol{n}}_j}} \right)} \end{array}} \right. $

(11)

将3个量值联合构成(α, ϕ, θ)的形式, 每个值使用5个区间来统计, 因此会生成125维的PFH特征。

由图 5可以看出, PFH的计算复杂度为O(nk²)(n为点云的总点数), 运算速度较慢。为提升特征提取的运算速度, 文献[4]提出FPFH特征提取算法, 该算法将计算复杂度降低为O(nk)。FPFH特征的计算原理如图 7所示。

FPFH特征计算步骤为:

1) 计算查询点p_q与p_q的k-邻域内的点的PFH, 称为SPFH。

2) 对于p_q邻域内的每一点, 计算相应k-邻域内的点的SPFH。

3) 使用式(12)计算p_q的FPFH:

$ \mathit{FPFH}\left( {{\mathit{\boldsymbol{p}}_q}} \right) = SPFH\left( {{\mathit{\boldsymbol{p}}_q}} \right) + \frac{1}{k}\sum\limits_{i = 1}^k {\frac{1}{{{w_i}}}} SPFH\left( {{\mathit{\boldsymbol{p}}_i}} \right) $

(12)

其中, w_i是点p_q与其邻域点p_i的欧氏距离, 并将其作为计算的权值。点云中的一点生成FPFH特征, 如图 8所示。

由上述分析可知:在FPFH特征计算过程中, 邻域半径的选择比较重要, 较大的半径会导致邻域内的点过多, 计算速度会降低; 较小的半径虽能加速运算过程, 但计算所得特征性能较差。因此, 在计算FPFH时, 根据点云特性自动鉴别出合适的邻域半径, 使得特征的运算速度得到大幅提升, 并改善特征性能。

根据以上计算所得的弧长密度l, 以手动选取的方式选择多个邻域半径, 计算每对点云的FPFH并完成SAC-IA^[12], 具体步骤为:

1) 对源点云P进行抽样, 为保证所采样的点具有不同的FPFH特征, 采样点两两之间的距离应大于预先给定的最小距离阈值d_min。

2) 搜索目标点云Q中与步骤1的N个采样点FPFH特征一样或接近的一个点或多个点, 选其中一个作为P对于Q的对应点。

3) 计算对应点的刚体变换矩阵, 通过求解对应点变换后的距离误差和函数来判断配准的性能。误差越小, 配准效果越好。

配准误差得分用Huber惩罚函数^[13]L_h(e_i)表示, 有:

$ {L_h}\left( {{e_i}} \right) = \left\{ {\begin{array}{*{20}{l}} {\frac{1}{2}e_i^2,\left\| {{e_i}} \right\| \le {t_e}}\\ {\frac{1}{2}{t_e}\left( {2\left\| {{e_i}} \right\| - {t_e}} \right),\left\| {{e_i}} \right\| > t_e} \end{array}} \right. $

(13)

其中, t_e为预设的阈值, e_i为第i组对应点的距离误差。迭代以上3个步骤, 手动选择多个邻域半径对点云集中的每对点云完成SAC-IA, 求解误差得分最小时的变换矩阵, 并确定误差得分最小时所选择的邻域半径, 其中, 最优邻域半径采用穷举的方法得到。将多对点云集统计出误差得分小且配准效果好的邻域半径与配准的点云弧长密度一一对应, 所得的多组数值代表配准性能最优时的邻域半径与弧长密度, 并用于半径函数的拟合。

1.4 拟合函数曲线

基于1.3节所统计出的邻域半径与弧长密度数值组, 使用最小二乘法^[14-15]完成曲线的拟合, 找出邻域半径与弧长密度的函数关系。最小二乘法是广泛使用的数据分析方法, 通过一组实验数据, 根据残差平方和最小准则, 确定解析函数以描述变量间的关系, 进而求取数据之间的客观规律。

对于给定的实验数据(x_i, y_i)(i=0, 1, …, m), 求出自变量x与因变量y的函数关系y=s(x; a₀, a₁, …, a_n)(n < m), 实质是在函数类s(x)∈span{φ₀, φ₁, …, φ_n}上确定唯一的函数s(x)=a₀φ₀(x)+a₁φ₁(x)+…+a_nφ_n(x), 使在给定点x_i上的误差δ_i=s(x_i)－y_i(i=0, 1, …, m)的平方和最小。当φ_k(x)=x^k(k=0, 1, 2, …, n)时, 函数s(x)=a₀+a₁x+…+a_nxⁿ。

通过选择合适的n值, 使用最小二乘法拟合出邻域半径r与弧长密度l的函数曲线, 称为半径函数, 记作r(l), 其图形曲线如图 9所示(图中的弧长密度与邻域半径分别放大1 000倍且弧长密度为平均弧长)。

半径函数的计算公式如式(14)所示。

$ r(\bar l) = {b_0} + {b_1}{(\bar l)^2} + \cdots + {b_n}{(\bar l)^n} $

(14)

其中, b₀, b₁, …, b_n为系数。

半径函数r(l)客观上反映了邻域半径与弧长密度之间的规律。对于一对待配准的点云数据, 先计算该点云的弧长密度l, 将计算出的点云弧长密度l代入半径函数r(l), 所求得的邻域半径即为自动鉴别出的邻域半径r, 用于计算点云的FPFH特征及SAC-IA。

1.5 自动邻域半径鉴别FPFH特征提取算法

将半径函数r(l)与原始FPFH特征提取算法相结合构建自动邻域半径鉴别FPFH特征提取算法, 计算FPFH特征前需估算点云弧长密度l, 输入到函数r(l)中得到邻域半径r, 用于计算FPFH特征, 如图 1(b)所示, 得到这些特征之间的对应关系, 从而完成初始配准, 使2个点云集获得一个较好的初始位置^[16], 以减少迭代次数和配准时间。该算法根据点云中点的分布疏密鉴别出与点云自身分布特性相匹配的邻域半径, 自动完成特征提取, 不需要人工参与设置邻域半径, 保证提取出的特征具有较高匹配质量的同时具有较快的计算速度。

2 实验结果与分析

为验证算法的有效性, 本文在CPU为Intel(R) Core(TM) i5-3230@2.60 GHz、内存4 GB的PC上, 使用VS 2015基于PCL V1.72点云库开发实验程序, 测试数据为斯坦福大学的Armadillo点云数据。该点云使用激光扫描设备每间隔一定角度扫描模型制作, 部分点云数据展示如图 10所示。为加速运算过程, 对点云进行下采样之后分别使用手动人工选择邻域半径与本文算法计算FPFH特征并完成SAC-IA。

表 1所示为人工选择邻域半径实验部分配准误差得分, 其中, 邻域半径每间隔0.005 m人工选取。表 2所示为人工选择半径与本文算法的最小邻域半径、最大邻域半径、配准误差得分最小的2个半径所计算得到的FPFH特征对比结果, 其中, “—”表示自动鉴别出的半径。图 11所示为该组dragon点云配准视觉效果。

从表 1与表 2可以看出, 较小的邻域半径虽然能减少FPFH特征的提取时间, 但是特征的性能较差; 较大的邻域半径在一定程度上能增强特征的性能, 但运算速度较慢。通过人工多次选择邻域半径的方法计算FPFH特征, 虽然能找出较为合适的半径值, 但是过程较复杂且效率较低。本文算法根据点云本身的弧长密度选择合适的半径计算FPFH特征, 不仅增强了特征的性能(配准误差得分达到2.102 15e-05), 同时减少了特征计算的时间, 仅为0.463 s。从图 11可以看出, 使用邻域半径计算的FPFH特征的配准效果较差, 而在人工选择的邻域半径中, r=0.030 m与r=0.070 m配准误差得分与本文算法的得分相当接近。此外, 本文算法在耳朵与前爪部分配准效果较好。

为验证本文算法针对不同点云同样有效, 选择另外一组dragon点云来进行实验, 结果如表 3和表 4所示, 其中, “—”表示自动鉴别出的半径。图 12所示为此组dragon点云配准视觉效果。

从表 3、表 4和图 12可以看出, 本文提出的算法针对不同的点云同样有效, 在人工选择的邻域半径中r=0.075 m的配准误差与本文算法较为接近, 且在嘴部效果更为突出, 因此本文算法具有较好的实用性和有效性。

3 结束语

本文提出一种基于弧长密度的FPFH提取算法, 根据点云的弧长密度, 自动鉴别合适的邻域半径, 以计算FPFH特征。实验结果表明, 该算法的计算过程简单且效率较高。但是, 本文算法在提取特征时速度较慢, 解决该问题是下一步的研究方向。