基于SHOT 特征提取的三维点云配准算法*

梁古南 潘 丰 王昌龙

（1.江南大学轻工过程先进控制教育部重点实验室 无锡 214122）（2.无锡信捷电气股份有限公司 无锡 214072）

1 引言

随着三维信息获取设备成本的下降，利用三维信息来处理常见的工程应用如物体的姿态识别和抓取定位等，已是机器视觉领域的研究热门方向之一。在三维特征描述方面，Salti S 等［1］研究了一种基于三维几何信息描述的SHOT 局部特征描述子，其特征维度高达352 维。Rusu 等［2］研究了一种33维的FPFH 局部特征描述子。两种特征描述子都具有较好地实时性，但前者的描述性更加精准。在配准方面，经典的配准算法是Paul J.Besl 等［3］提出的迭代最近点，后续的许多配准算法［4～5］都是在其基础上的改进。ICP 算法要求配准的两块点云有一个较好的初始位姿，否则容易陷入局部最优而得不到精确解。文献［6］研究了一种基于高斯混合模型的连续点移配准方法，将配准的两块点云分类为两个模型，迭代优化靠近，直到方差达到要求；该方法方向识别性差，耗时大。基于对偶四元素的方法要求配准的点对正确，否则难以求解出合理的位姿参数［7～8］。针对存在的问题，本文设计了一种基于SHOT特征提取的三维点云配准算法可以实现目标点云和模板点云之间位姿参数的精确求解。

2 算法原理

预 处 理 后［9］的 两 幅 点 云 为P∈R3×n和Q∈R3×m，P为目标点云，Q为模板点云。配准的目的是求得两者之间的位姿参数即旋转矩阵和平移向量，使两者重合在一起。点云配准算法流程如图1所示。

图1 点云配准流算法程图

2.1 初始配准

2.1.1 关键点提取

考虑到配准效率，仅需对一些具有显著特征的点即关键点进行计算。本文用ISS算法提取目标点云的关键点［10］，该算法有两个表述特征值之间比例系数的阈值threshold1和threshold2，前者取0.1～0.4，后者取0.6～1。ISS 算法提取关键点的步骤如下：

Step1：对目标点云P构建KD树［11］，置k=1；

Step2：取P第k个点Pk，搜索其一定半径内的邻域点；

Step3：用主元分析法获得该邻域的三个特征向量和特征值［12］，对特征值升序排序得到λ1,λ2,λ3；

Step4：计算λ1与λ2的比值并记为δ1，计算λ2与λ3的比值并记为δ2；对δ1和δ2进行判断，若δ1≥threshold1和δ2≤threshold2都成立，则认为该点是关键点，保存该点。否则，不保存该点，转Step5；

Step5：判断k是否等于n。是，停止遍历，结束循环；否则，k=k+1转Step2。

提取的关键点集合记为KeypointP∈R3×l，l为关键点的数目。半径的设定为在数据中任选一点，用KD 树搜索最近的点，得到两者的距离d，半径取1d～1.5d。图2 瓶子关键点阈值上下限取0.3 和0.98，半径取1mm。兔子关键点阈值上下限取0.12和0.9，半径取6mm，可得到和图2一样的效果。

图2 半径r=1mm瓶子关键点显示

2.1.2 SHOT特征提取

本文用SHOT 特征描述子对点进行特征提取，具体的实现步骤如下。

1）对当前点构建稳健的局部参考坐标系

用主元分析法获得当前点半径为r的邻域的三个特征向量和特征值，对特征值进行升序排序，对应的特征向量作x轴、y轴和z轴，构建局部坐标系。用邻域点对坐标系的符号进行修正［13］。

2）对局部球型邻域划分成32个子块

对球型邻域沿径向、高度和方位角方向划分成2 份、2 份和8 份，对子块按顺序进行序号编码，图3是划分成2份，2份和4份的球型邻域示意图。

图3 球型邻域示意图

3）特征编码和直方图统计

点的法向量［14］与z轴正向夹角余弦值范围在-1～1，将其映射到0～1，把区间[0 ,1] 分成11等份后扩展到长度为11 的区间内，每个子块可得到一个长度为11 维的向量描述，32 个子块依次连接形成352 维，统计区间内横坐标出现的频数并归一化。

4）线性插值消除边界效应

SHOT 描述子是局部特征描述子，相邻块之间存在边界效应。为此需对每个点在当前块、径向相邻块、高度相邻块和方位相邻块上进行线性插值。图4 是方位插值与直方图统计图。q点所在块为1，相邻块为2；qy和qx是点q的纵坐标和横坐标，wq是点q与x轴正向的夹角，wj和wj+1是对应块的平分线与x轴正向的夹角；cosθi是点q与z轴正向夹角的余弦值，φ为45°。权值分配系数分别为dj和dj+1；其它方向的插值类似，提取的关键点特 征 记 为SHOTP∈Rl×352，模 板 特 征 记 为SHOTQ∈Rm×352。图5 是任选两个关键点，绘制出SHOT特征示意图。

图4 方位插值与直方图统计

图5 任意两个关键点SHOT特征示意图

2.1.3 配准点对提取

引入二次约束来提高配准点对的质量［15］。初步提取配准点对的过程为：在KeypointP中随机采样f≥3 个点，对每个点在模板特征中搜索与其特征距离最近的点作模板配准点。初步提取的配准点对记为(Di,DTi)，i=1,2,…,f，Di为目标配准点，DTi为模板配准点，模板Q和目标P的质心为Qcenter∈R3×1和Pcenter∈R3×1，距离比值δdis由式（1）计算：

半径为r的邻域内各自包含的点数目为NUMQ和NUMP，邻域比值δNUM描述由式（2）计算：

其中，Di∈R3×1和DTi∈R3×1分别为Q和P中的点，f为初步提取的点对数目，m为Q的点数目，n为P的点数目，半径r同2.1.1关键点邻域半径一样。

理想的δdis和δNUM皆为1，由于干扰的存在，实际的取值时应留有裕量，下上限可分别取0.9 和1.1，即0.9 ≤δdis≤1.1 和0.9 ≤δNUM≤1.1，当约束条件均满足时，则认定其为一组合适的点对。由于旋转矩阵的自由度为3，因此至少需要3组点对，最终提取出的配准点对记为(Di,DTi)，i=1,2,…,s，s≥3。图6是瓶子配准点对提取显示。

图6 瓶子配准点对提取显示

2.1.4 对偶四元数求解旋转矩阵和平移向量

其中，K()是式（5）生成的斜对称矩阵，I是单位阵，

旋转矩阵和平移向量的参数估计步骤如下：

1）根据2.1.3 节提取出配准点对(Di,DTi)，i=1,2,…s，设定初始配准误差均方根阈值Th1；

2）由式（6）生成矩阵H，进行特征值分解求最大特征值对应的特征向量vˉ，由式（7）解得四元数：

其中，c1和c2是两个重要的四元数系数矩阵c1=是点DTi和Di对应的四元数，是将和代入式（3）求得的矩阵，是将代入式（4）求得的矩阵的转置。

3）由式（8）求出Rcoarse∈R3×3和Tcoarse∈R3×1：

其中，是将代入式（4）求得的矩阵，是将代入式（3）求得的矩阵的转置。

4）由式（9）更新得到Pnew，由式（10）计算误差均方根Error1RMSE，若Error1RMSE≤Th1，停止迭代输出Pnew、Rcoarse和Tcoarse，否则，返回步骤1）。

2.2 精确配准

对模板点云Q和2.1.4得到的更新后的目标点云Pnew∈R3×n，ICP算法实现的步骤如下：

1）置迭代次数k=1，设定最大迭代次数kmax，精确配准的误差均方根阈值Th2，Th2取0.01 mm，初始化Rfinal和Tfinal为2.1.4解得的Rcoarse和Tcoarse；

2）对Pnew中的每个点Pnew(i)在Q中搜索与其距离最近的点Qnew(i)，最近点的集合记为Qnew∈R3×n；

3）由式（11）计算Qnew和Pnew的质心uQ和uP，由式（12）对进行去质心操作，得到和；

5）对ConvQP进行奇异值分解［16］得到左奇异向量U∈R3×3和右奇异向量V∈R3×3，求得本次迭代的旋转矩阵和平移矩阵Ticp=uQ-

7）由式（13）计算均方根误差Error2RMSE；

若Error2RMSE＞Th2或k≤kmax，更新Pnew的每个点赋值给Pnew，k=k+1 转步骤2）继续迭代；否则停止迭代，输出Rfinal和Tfinal。

3 仿真与分析

本算法试验的软件平台为Matlab R2014b，试验数据采用盛相科技公司的3D 结构光相机Sizector R扫描获取的瓶子三维点云数据和斯坦福3D库里的兔子模型，分别对算法进行验证和对比分析。

3.1 验证环节

1）瓶子的长、宽和高为6cm、2.5cm 和5cm，点数为1500 点左右，关键点提取半径取1mm，阈值上下限分别取0.98和0.3，特征提取半径为4mm，邻域约束半径取4mm。以模板和目标之间的方位差为180°为例给出仿真结果，其它角度经试验均可得到验证。配准效果如图7 所示，可以看出两者较好地贴合在一起，配准成功，验证了算法的可行性。

图7 瓶子配准效果图

2）兔子的长、宽和高为18cm、10cm 和15cm，点数为1300 点左右，关键点提取半径取4mm，阈值上下限取0.9 和0.12，特征提取半径取10mm，邻域约束半径取10mm。这给出模板与目标之间的方位差为45°的配准效果图，如图8所示。

图8 兔子配准效果图

3.2 对比环节

表1记录了点云配准试验数据，模型一1200点左右，数据缺失大，模型二1500 点左右，数据较完整。对比模型一和二的试验数据发现当目标数据较完整时由于SHOT 特征描述性更加准确，点对提取耗时相对更短。但当目标数据点缺失大时，特征的描述性会下降，相应的也会增加点对提取耗时，从而加大整个初始配准的时间开销，从本文方法在模型一和模型二下的数据纵向对比中得到。总的来说，基于本文的方法比基于FPFH+ICP 方法耗时更短。基于传统ICP 的配准方对数据的方位要求苛刻，适应性差。从表1 可以看出当模板与目标之间的方位超出范围后，对于模型一和二都不能实现配准。对比三种方法的实验结果可以得出本文的方法能适应数据的不同位姿。

表1 点云配准试验结果

本文试验了其它常见的模型如Bunny 兔等，在参数设置合理的情况下可在较短时间内得到较好的配准效果。

4 结语

本文设计一种基于SHOT 特征提取的三维点云配准算法，分为初始配准和精确配准。首先对目标点云进行关键点提取，在关键点中随机采样并计算其特征，在模板特征中结合二次约束搜索出合适的配准点对，用对偶四元数求解位姿参数完成初始配准。用迭代最近点算法实现最终位姿参数的精确求解完成精确配准。试验表明本文方法能缩小配准耗时，适应目标与模板之间的不同位姿，在数据缺失的情况下也能够得到较好的配准结果。