实现激光点云高效配准的 ICP 优化及性能验证

　　摘 要：激光点云常规匹配算法是迭代最近点(Iterative Closest Point, ICP)算法，但其收敛速度慢、鲁棒性差，因此提出一种融合多种优化算法的激光点云高效 ICP 配准方法。首先对点云体素滤波降采样，通过 ISS 算子提取关键点，采用快速点特征直方图(Fast Point Feature Histograms, FPFH)提取关键点特征，嵌入 OpenMP (多核多线程并行处理模式)提高特征提取速度;然后基于提取的 FPFH 特征，使用采样一致性初始配准算法(Sample Consensus Initial Alignment, SAC-IA)进行相似特征点粗配准，获取点云集间的初始旋转平移变换矩阵;最后采用 ICP 算法进行精配准，同时采用最优节点优先(Best Bin First, BBF)优化 K-D tree 近邻搜索法来加速对应关系点对的搜索，并设定动态阈值消除错误对应点对，提高配准快速性和准确性。对两个实例的配准点云进行了实验验证，结果表明，提出的优化配准算法具有明显速度优势和精度优势。

　　本文源自红外与激光工程 发表时间：2021-03-10《红外与激光工程》系中国宇航学会光电技术专业委员会会刊，由中国航天科工集团公司主管，创刊于1972年，是国家科委和国家新闻出版署批准的国内外公开发行的部级学术刊物。 本刊是中国航天界光电子技术领域内学术性与工程应用性集于一体的综合性刊物，主要刊登国内红外与激光技术方面的学术论文和工程研究报告，集中反映了中国光电技术在宇航、卫星及导弹武器系统中的工程应用水平。

　　关键词 激光点云;快速点特征直方图;采样一致性算法;迭代最近点算法;点云配准

　　1 引言

　　激光扫描三维重建作为一种新兴空间数据获取技术已广泛应用于多种领域[1,2]。采用三维激光扫描仪对特定目标扫描采集点云时，受到目标尺寸、外界环境等因素干扰，致使点云无法被一次性获取，需多站点多角度多次扫描才能实现，多站点云需进行配准，转换到统一坐标系中形成完整点云。

　　点云配准主要采用 Besl[3]提出的传统迭代最近点(ICP)算法，具有良好配准精度，但收敛速度慢，且当两组点云存在较大差异时，算法会陷入局部最优解，鲁棒性差[4]。为了提高配准效率，在进行 ICP 配准之前可以先进行粗配准[5]。粗配准是根据局部特征的旋转平移不变性，采用特征匹配实现[6]。如李宇翔[7]提出基于内部形态描述子结合 K-D tree 的改进迭代最近点配准算法来提高配准速度;Rasu[8]使用点特征直方图对点的几何特征描述得到多维特征向量;孙文潇[9]采用局部表面拟合进行法线估计并计算其快速点特征直方图进行粗配准。

　　对于 ICP 算法优化中，卢纯青等[10]基于深度信息动态尺度估计方法提升点云配准的时间稳定性和配准精度;Bae[11]计算点与其邻域内点集组成的协方差矩阵，使用点的曲率变化率代替表面曲率，减少计算量;唐志荣[12]使用典型相关分析法对两组转动矩阵进行求解，实现点云配准;黄源[13]根据点云在不同半径内的法向量变化度提取特征点，利用距离约束条件获取准确匹配点;王帅等[14]使用谱聚类按几何结构特征对各视角点云区域分割，采用改进 ICP 对点云精确配准。

　　现有文献对粗配准、精配准 ICP 算法及其各种变种算法研究集中在优化算法本身上，而对加速搜索算法和错误对应点对配准算法精度和速度的影响研究较少。为进一步提高点云配准算法的速度和精度，本项目融合多种优化方法，重点提高搜索速度和特征值提取速度，包括优化点特征直方图提取方法与效率、采样一致性初始配准(SAC-IA)和采用最优节点优先 K-D tree 近邻搜索法等，来实现高效、高精度的 ICP 点云优化配准算法。

　　2 优化原理

　　如图 1 所示为融合了多种优化算法实现点云高效配准优化方案。其中，在点云预处理中对点云进行体素网格滤波;在关键点选择中，采用内部形态描述子(Intrinsic Shape Signatures, ISS)算法提取关键点;在特征提取优化中，嵌入多核多线程 OpenMP 并行处理模式，加速关键点快速特征直方图提取;在粗配准中，采用采样一致性初始配准优化;精配准中，采用最优节点优先法优化 K-D Tree 近邻搜索，加速对应关系点对搜索，同时设置动态阈值剔除错误对应关系点对，提高精匹配精度。

　　2.1 点云预处理

　　由于激光雷达扫描点云中空间点数目非常大，会影响点云的配准效率;另外点密度分布也不均匀，且扫描点存在粗大误差和测量误差，因此有必要进行体素栅格滤波。根据原始点云分布空间创建八叉树三维体素栅格，细分深度 8。在每个小网格体素中，用所有点的重心近似代替体素中所有点，可得降采样点云的同时还保持点云基本形状特征。

　　2.2 关键点选择

　　采用内部形态描述子(Intrinsic Shape Signatures, ISS)算法进行关键点提取：设点云 W 中含 m 个点， p x y z i m i i i i = = ( , , , 1,2, , ) ，对每个点 i p 建立一个局部坐标系，并设定一搜索半径 iss r ，确定 i p 为球心、 iss r 为半径球体内的包含点，计算权值ij ： 1 , ij i j iss i j p p r p p  = −  − (1)计算每个点 i p 的协方差矩阵： ( ) ( )( ) cov i j iss i j iss T ij i j i j p r i ij p p r p p p p p  −  −  − − =  (2)获得协方差矩阵的特征值  1 2 3 , ,    i i i ，从大到小排序;设置阈值1 和2 ，满足 2 1 1 i i 和 3 2 2 i i 的点即为关键点。

　　2.3 特征提取

　　如图 2 所示，基于中心点与 k 邻域之间的法向矢量关系计算点特征描述向量。

　　图 2(a)以关键点 pq 为中心划半径为 r 圆球，包含 k 个近邻点。如图 2(b)所示，将 pq 与 k 个邻域点拟合一小平面，其法向量即 q n ，对 q n 定义一个笛卡尔坐标系，三坐标轴为: 2 q k q k q x n p p y x p p z x y  =  −  =  −  =   (3)描述 q n 与某近邻点 pk 法线 k n 之间的相对空间偏差关系，用三个角度特征值： cos( )  =  arc y nk (4) 2 arccos( ) k q k q p p x p p  − =  − (5) =   arctan , (z n x n k k ) (6)

　　计算出 pq 与半径 r 邻域内所有点组成的点对之间的三个特征值后，用直方图统计三个特征值的区间分布数目，记为简化点特征直方图 (Simplified Point Feature Histogram，SPFH)。再依次查询每个邻域点 ki p 的 SPFH。综合关键点 pq SPFH 特征和 k 个近邻点 SPFH 特征，计算关键点 pq 的 FPFH 直方图特征序列为： ( ) ( ) ( ) 1 1 1 + k q q i i i FPFH p SPFH p SPFH p k d = =   (7)其中， di 为对应点对的欧氏距离。

　　FPFH 将三个特征 ( , , )   的参数范围均划分为 11 个统计子区间，统计每个子区间上点数目，合并得到一个 33 维特征向量。图 3(a)所示，取建筑物激光点云中三点 A、B、C，分别为平面点、曲面点和角点，三点的 FPFH 序列如图 3(b)、(c)、(d)。可见，点的几何位置不同，FPFH 值也各有特点。

　　2.4 基于采样一致性的粗配准

　　FPFH 特征向量提取后，通过采样一致性算法对目标点云和模板点云粗配准：(1)模板点云中，获取 m 个待配准特征点;(2)目标点云中，搜索与模板点云中特征点 FPFH 值相似的所有点，随机选一个相似点，作为从模板点云到目标点云的对应关系点;(3)计算对应关系点对的旋转平移矩阵，并计算对应关系点旋转平移转换后的距离误差总和函数，即 Huber 损失函数： ( ) ( ) 1 2 , 2 1 2 , 2 i i h i i i e e r L e r e r e r   =  −   (8)其中， r 为距离阈值， i e 为对应点对间的欧氏距离。Huber 损失函数最小值对应的旋转平移变换矩阵就是粗配准所求的最优变换矩阵。

　　2.5 优化 ICP 精配准

　　2.5.1 进行搜索加速优化的 ICP 精配准

　　采用最优节点优先 (Best Bin First, BBF)优化 K-D Tree 近邻搜索，加速对应点对搜索：

　　(1)分割结点确定：统计所有特征点在(x, y, z)各维度方差，方差最大的维度为主维度，将特征点按主维度排序，正中间特征点为分割结点。

　　(2)搜索对应关系点对：待查询点值与分割结点值比较，若小于等于，则进左子树，否则进右子树，沿 K-D Tree 搜索，直至叶子结点，在此子空间内查找与待查询点距离最近点，同时建立优先级，点的优先级 Tpri 定义为： , ( , , ) T s r i x y z pri i i = −  (9) i s 为待查询点第 i 维值， i r 为特征点第 i 维值， i 为分割维度。 Tpri 越小，优先级越大。

　　(3)“回环”搜索：按优先级大小查找是否存在与该点距离更近对应点，若在其他分割子空间中存在，则进入此子空间继续搜索。

　　2.5.2 动态阈值设置

　　将每次迭代完的配准误差(即欧式适合度评分)设置为下次迭代的距离阈值，进行自适应动态设置，在第次配准误差为[15]： ( ( )) 2 , , 1 1 1 n MS a i b i i R p R p t n   = = −  + −  (10) ( p p a i b i , , −  )  (11) R和 t分别为第次迭代的旋转和平移矩阵，为迭代的距离阈值。点云偏差一般服从正态分布，则取 =3RMS ，剔除不满足式(11)的对应点对，防止该点对参与下次迭代。

　　3 实验过程与结果分析

　　3.1 实验过程

　　采用两组点云进行算法验证实验。第一组采用 PCL 官网下载建筑物点云;第二组为采用激光雷达扫描建筑物获得。实验采用计算机硬件环境：处理器 Intel(R) Core(TM) i5-8300H CPU @ 2.30GHz，内存 16.0GB;操作系统 ubuntu 16.04，64-bit，编程语言 VC++。

　　3.2 实验结果分析

　　3.2.1 实验 1

　　图 4(a)建筑物点云，目标点云(绿色)共 112586 个点，模板点云(红色)共 112624 个点，两原始点云经体素滤波后如图 4(b)所示，滤波后分别含 17677、17640 个点。

　　从目标点云和模板点云中分别随机选取两个关键点进行特征提取并显示点特征直方图，如图 5 所示，A、C 是模板点云关键点，B、D 是目标点云关键点。其中， A 点和 D 点的 FPFH 特征基本一致，因此作为一组对应关系点对。

　　当 ISS 算子加入后，对点云的配准效果进行了实验验证比较。如表 1 所示，其中 SAC-IA 是进行传统 FPFH 特征提取进行的配准实验结果，SAC-IA+ISS 是先提取 ISS 关键点、然后再进行 FPFH 特征提取进行的配准实验结果。表 1 的结果表明，采用 ISS 算子提取关键点后，配准欧拉评分近似相同，而配准时间却减少近一半，由 12.061s 降为 6.797s，因此 ISS 算子的引入有效提高了点云的配准效率。

　　在特征提取过程中，嵌入了 OpenMP 多核多线程并行计算模式，加快特征提取速度，采用单线程 pcl::FPFH Estimation 特征提取耗时 13.522s，而采用 OpenMP 模式耗时 3.058s，仅为单线程的 22.6%。基于提取的 FPFH 特征值进行粗配准，共耗时 6.091s，粗配准的点云效果如图 6(a)所示，同时获得了两组点云的粗配准变换矩阵。两组点云基本重合，但仍有较明显错位，需进一步精配准。

　　完成粗配准后，再进行优化 ICP 配准，耗时 0.895s，共迭代 5 次，欧式适合度评分为 0.0040。其中，欧式适合度评分是目标点云到模板点云对应关系点对的距离平方和。ICP 精配准效果如图 6(b)所示，同时获得了两组点云的精配准变换矩阵。

　　为比较，对两组点云进行常规 ICP 配准，共耗时 8.297s，迭代 45 次，欧式适合度评 0.0041。

　　3.2.2 实验 2

　　将激光雷达固定在三角支架上，获得不同站点实验室走廊的扫描点云。取两站点扫描的激光点云进行配准，第一组为红色点云，含 290545 点;第二组为绿色点，含 290608 点，如图 7(a)所示。对两个原始点云进行体素网格滤波后分别含 5938、5942 个点，如图 7(b)所示。

　　对走廊点云筛选关键点，提取 FPFH 特征，并进行 SAC-IA 粗配准，求取两组点云的变换矩阵，粗配准效果如图 7(c)所示，耗时 1.080s，同时获得了两组点云的粗配准变换矩阵。

　　对粗配准获得的两组点云，再进行优化 ICP 配准，耗时 0.416s，迭代 7 次，欧式适合度评分 0.0057，如图 7(d)所示，同时获得了两组点云的精配准变换矩阵。

　　为比较，对走廊点云进行常规 ICP 配准，共耗时 2.042s，迭代 33 次，欧式适合度评分 0.0058。

　　3.2.3 实验结果分析

　　点云配准结果的主要评估标准是迭代次数、欧式适合度评分和配准耗时。上述两个配准验证实验中，常规 ICP 算法和优化算法的评估参数如表 2 所示。另外，为了进一步说明所提出的优化组合方式的优越性，与文献[8]所采用的点云配准方法进行了实验比较，此方法记作 SAC-IA+ICP，即采用采样一致性的粗配准和精配准的组合，实验结果也列入表 2 中。由表 2 可见，在欧拉评分近似相同的配准精度效果下，组合优化方法 Optimized SAC-IA+ICP，在迭代次数和配准时间上均有显著改善。

　　表 2 可见，优化 SAC-IA+ICP 在两组实验中的迭代次数均远少于常规 ICP，仅为其 1/9~1/4。另外，优化 SAC-IA+ICP 在两组实验中计算速度均快于常规 ICP，在第一组实验中，优化 SAC-IA+ICP 为 6.986s，约占常规 ICP 的 84.19%;在第二组实验中，优化 SAC-IA+ICP 为 1.496s，约占常规 ICP 的 73.26%。优化 SAC-IA+ICP 在两组实验中得欧式适合度评分均略优于常规 ICP。

　　4 结论

　　提出了一种融合多种优化算法的激光点云高效 ICP 配准方法，采用多核多线程 OpenMP 模式加快 FPFH 特征提取，根据提取的点云 FPFH 特征采用 SAC-IA 粗配准计算初始旋转平移变换矩阵，采用最优节点优先法优化 K-D tree 近邻搜索法加速搜索，及设置动态阈值消除错误点对等优化措施。通过对建筑物点云和走廊点云进行算法验证，比较了优化 SAC-IA+ICP 算法和常规 ICP 配准算法的性能，结果可知，优化 SAC-IA+ICP 算法的迭代次数减小为 1/9~1/4，耗时减小为 73.26%~84.19%，欧式适合度评分有所降低，因此具有高效和高精度的优势。