CN115082716A - 一种面向道路精细重建的多源点云粗匹配算法 - Google Patents

Abstract

本发明公开了一种面向道路精细重建的多源点云粗匹配算法，用于路面的精细建模与分析。其利用点云粗略重叠区域的凹凸度相似性快速计算点云之间的近似比例，弥补了4PCS类算法无法匹配未知尺度点云的缺陷；针对4PCS类算法对较大数据量点云匹配效率低，所用均匀降采样无法保证简化点云的特征分布与重复率的问题，使用经典三维特征点提取算法对原始点云进行简化，并对特征点云进行采样率、计算效率与重复率的比较，找出了最佳的数据降采样方法；针对4PCS类算法无法有效匹配具有一定平面分布特征的点云，Nc‑5PCS设计了一种非共面5点基以及对应的特征描述方式，基于超球索引技术完成同名特征匹配，有效解决了4PCS类算法中共面4点基特征退化所导致的匹配失败。

Description

一种面向道路精细重建的多源点云粗匹配算法

技术领域

本发明涉及三维点云处理技术领域，特别涉及一种面向道路精细重建的多源点云粗匹配算法。

背景技术

建立精细的道路模型是高效、准确进行路面质量评价的基础，有助于提高相关特征提取与分类的精度，辅助制定合理的道路养护方案。传统的数字公路采集车，造价高，且对道路的通过性有一定要求，无法进行破损较为严重的路面点云采集与重建，特别是无法处理地震等严重灾害之后的道路损毁情况普查。近几年，随着无人机相关技术的不断发展，越来越多的学者开始重视无人机点云获取的高效性与灵活性，使用无人机搭载相应传感器在低空获取道路点云，完成道路建模与质量评价。

使用无人机可以灵活、高效地获取道路点云数据。其中，虽然无人机LiDAR点云精度高，但点云密度低，且分布不均匀；无人机摄影测量点云密度相对较高，但精度差，加之，无人机点云采集角度单一，且数据获取距离较远，因此，以上两种点云的道路模型均无法充分体现路面的细节特征。融合近景摄影测量点云与无人机点云可以丰富路面细节，获得更加精细的路面模型，目前融合多源点云进行道路精细建模的应用很少。此外，在无人机自主作业的过程中地面人员可同时采集局部兴趣区域影像，此种数据获取方式的组合，不会降低整体作业效率，既结合了数据获取的灵活与高效性，也可为后续精细建模提供了更优质的点云数据。

4PCS类方法是利用共面四点相交的仿射不变量，获取点云之间的共面四点特征完成点云匹配的方法。4PCS是在LCP(Largest Common Pointsets)与RANSAC匹配策略的基础上发展而来的。由于算法不需要计算点云的局部特征，对重叠度与噪声有较高的鲁棒性，是目前最优秀的点云粗匹配算法之一。

2008年Aiger和Mitra在二维4点仿射不变量的基础上，提出了4PCS算法，将点云匹配的复杂度进一步降低为O(n2)，由于不需要计算点云的局部特征，所以算法对噪声的鲁棒性强。2010年李敏使用曲面变分估计提取点云的多尺度特征改进4PCS的匹配效率与精度，但改进的本质是对点云的预处理，对4PCS的改进有限。2011年Yao等人提出了基于点蔟的NPCS(N-Points Approximate Congruent Sets)算法，将蔟内点的特征与点对距离作为高维特征进行匹配，由于点蔟的特征区分度高于4PCS中的平面四点基，所以算法匹配精度较高，且可以完成低重叠度的点云匹配，但算法计算复杂，对内存的占用率较高，无法有效处理大数据量点云。2012年Lam等人基于DoN特征点建立分割曲线对点云进行曲面分割与合并，此后使用4PCS确定分割区域的匹配关系完成点云匹配。算法中形状的分割与合并计算复杂，无法处理较大数据量点云。此后，Lam与Greenspan对之前的工作进行改进，使用非监督的方式合并分割区域，结合体素与形状哈希提高检索效率，使用聚类方法获取变换参数，此方法对遮挡导致的点云缺失敏感，且匹配精度与分割精度直接相关。2013年钱鹏鹏等人在4PCS提取等距点对时加入了曲率相似度限制，减少全等共面4点集合的数量，提高算法的匹配效率。2013年Theiler等人提出了K-4PCS(Keypoint-based 4PCS)算法，算法基于原始数据的3D DoG特征点云进行4PCS匹配，使得4PCS计算效率提高了7倍，且匹配精度优于原始算法。2014年Theiler等人对K-4PCS算法进行扩展，分别使用3D DoG与3D-Harris对大数据量点云进行降采样，Theiler指出3D DOG+4PCS的方法更适合平滑表面与具有高对比度的LiDAR点云配准，并分析了点云的纹理属性和几何对称性对匹配算法的影响。此后，Theiler等人使用MLESAC作为K-4PCS中的匹配策略，提高结果精度；使用最小距离先验值降低扫描位置带来的误差(Near-field Bias)；对匹配结果进行聚类，结合并行优化提高算法效率。2014年Krishnan等人针对复杂地形点云无法有效提取全等共面四点的问题，提出了全等金字塔基，增加的第五点仅作为备选4点集合的检验而不用于变换矩阵的求解，算法的匹配模型复杂、计算效率较低。2014年Mellado等人提出了Super 4PCS算法，在选取全等四点时加入对角线夹角限制以降低确认计算的开销，通过点云栅格化之后的智能索引技术大大提高了匹配效率，最终计算复杂度由O(n2+k)降低为O(n+m+k)。目前，Super-4PCS是4PCS改进算法中效率最高、对低重叠度与噪声的鲁棒性最好的算法。Mohamad提出了Super G4PCS(Super Generalized 4PCS)算法，使用Super-4PCS中的智能索引技术改进General-4PCS算法的计算效率，Super G4PCS虽然可以有效降低全等集合的数量，但是在搜索全等4点时计算开销较大，理论计算复杂度由Super-4PCS的O(n+m+c)增加为O(n+k1m+c)，k1是每个等比点圆形遍历非共面对应点的次数。2017年Huang等人在Super-4PCS的基础上提出了V4PCS(Volumetric 4PCS)算法，算法使用四面体基提高特征的区分度，并建立四面体边的链接索引表，在全等集合查找时去除了交点检索和角度检索的计算开销，算法效率比Super-4PCS提高了近20倍，由于算法在计算与存储点对时开销较大无法处理较大数据量点云的匹配。

4PCS中共面四点的比例特征理论上可以用于点云相似变换关系的求解，但实际计算时中间点的存储数量与计算开销成几何次方增加，导致算法匹配失败。2013年Corsini等人针对此问题，使用VSA(Variational Shape Approximation algorithm)对点云进行分割，以确保共面四点分布在不同的分割平面中，从而降低中间点数量与全等集合数量，完成了摄影测量点云与LiDAR点云的相似变换关系求解，但算法计算复杂，依然无法有效处理大数据量点云的配准，此外曲面分割效果直接影响了后续的匹配精度。2017年Zhang等人提出了G-super 4PCS(Generalized Super4-points Congruent Sets)算法，算法使用Lin等人所提出的基于多尺度Spin Image特征统计的比例求解方法，完成山体区域摄影测量点云与TLS点云之间的匹配。但算法的计算开销较大，所得比例误差较大，需要基于点对误差拟合抛物线对比例进行二次优化，算法计算效率低，无法有效处理大数量点云。

在众多点云匹配算法中，4PCS类算法不需要计算点云的局部描述子；对噪声与重叠度的鲁棒性强；算法仅使用了点云的几何信息，因此适用范围广，是非常优秀的点云粗匹配算法。但此类算法依然无法有效匹配异源点云，算法的瓶颈主要为：①虽然4PCS算法将RANSAC类匹配算法的计算复杂度降低为O(n2)，在后续的算法改进中，Super-4PCS进一步将计算复杂度降低为线性，但此类算法依然难以处理较大数据量的点云。②4PCS主要用于点云间刚体变换关系求解，算法无法处理具有未知尺度的相似变换关系。③4PCS类算法在点云存在一定的平面特征分布时，共面四点特征退化严重，导致匹配失败。

为了说明4PCS类算法在匹配异源点云时的问题，本发明通过使用具有未知尺度、且具有部分平面特征的LiDAR点云与近景摄影测量点云进行Super-4PCS匹配实验，实验结果如附图1所示，从附图1中可以看出Super-4PCS存在上述描述的问题。

发明内容

针对上述存在的问题，本发明以道路精细建模为应用背景，提出了一种面向道路精细重建的多源点云粗匹配算法，其提出了非共面五点全等集合的点云匹配算法(Non-coplanar 5Points Congruent Sets for Robust Pairwise Surface Registration，Nc-5PCS)，并结合无人机LiDAR点云与近景摄影测量点云的样例数据说明相关原理及中间的处理过程。

为了实现上述目的，本发明所采用的技术方案如下：

一种面向道路精细重建的多源点云粗匹配算法，其特征在于，包括以下步骤：

步骤1：获取目标道路的源点云P和目标Q；

步骤2：基于点云粗略重叠区域内凹凸程度的相似性获取源点云P和目标点云Q之间近似比例，将点云之间的相似变化转化为刚体变化；

步骤3：通过3D-Harris特征点提取算法对点云P和Q进行简化，得到特征源点云P和特征目标点云Q；

步骤4：基于步骤2得到的近似比例，在特征源点云P中构建非共面5点基B，并建立非共面5点基的特征描述；

步骤5：采用超球栅格化与哈希检索在特征目标点云Q中依据所述非共面5点基的特征描述查找近似的全等5点基集合{B’}；

步骤6：遍历B和{B’}进行匹配，得到全等非共面5点基和对应的变换矩阵H；

步骤7：基于LCP进行一致性度量，计算一致性度量下的最优解，以求得的最优解作为最优变换矩阵T；

步骤8：根据得到的最优变换矩阵T，完成道路多源点云匹配。

进一步地，步骤2的包括以下具体操作步骤：

步骤21：基于GPS位置信息确定粗略重叠区域，假设平面方程为Ax+By+Cz+D＝0(C≠0)，将平面方程转化为z＝ax+by+c，其中，a＝-A/C，b＝-B/C，c＝-D/C；从点云中随机选取3个点，判断三点是否共线，若不共线则计算这3个点所对应的平面方程，否则重新选取3点；

步骤22：计算点云中各个点(xi,yi,zi)到所得平面的距离di：

其中，a，b，c均为平面方程z的参数；

步骤23：计算所得距离{di}的标准偏差σ：

其中，

取阈值τ＝2σ；

步骤24：判断各个点的距离d_i＞τ时，则该点为高程异常值，存为外点outliers，其他点作为内点inliers用于平面拟合，形成外点集合{inliers}和外点集合{outliers}，并分别统计点数Noutliers与Ninliers；

步骤25：重复步骤21-24迭代N次，选取内点数最多的点集进行平面拟合，通过对所有内点inliers组成的协方差矩阵进行SVD分解，最终获得平面方程z＝a₀x+b₀y+c₀，且：

其中，a₀、b₀与c₀为平面方程的参数，p为设定的正确概率，一般p＝99％，，w为内点概率；

步骤26：在距离求解时仅遍历外点outliers，最终获得重叠区域凹凸度的极值dis＝Max({dis_i})为：

步骤27：计算点云之间的近似比例，且计算公式为：

其中，d_Q-average与d_P-average分别为点云Q与点云P粗略重叠区域的凹凸度均值。

进一步地，步骤4中所述非共面5点基的构建步骤包括：

步骤41：基于平面拟合过程中平面内点与外点的hash储存，将外点中距离平面较远的子集{outliers，d_outliers≥d_Localmax×60％}，作为非共面点的备选集{p5}，用以随机选取1个非共面点；以所述内点集合{inliers}为共面4点的备选集；

步骤42：设特征源点云P和特征目标点云Q的近似重叠度为ω，如果没有预估重叠度，按照ω的值从大到小遍历{1、0.75、0.5、0.25、…}，根据重叠度ω估计底面四点的边长约束d_ω：

d_ω＝dia_p×ω (6)

其中，dia_p为点云p的直径；

步骤43：依据所述边长约束，设置边长为d₀＝d_ω×A，其中A为(0.7，0.95)范围内的随机值，在内点集合中，随机选取两点p₁与p₃，使得：

d₀-σ≤||p₁-p₃||≤d₀+σ (7)

其中，σ为距离的误差阈值；

步骤44：在内点集合中随机寻找第三点p₂，设置边长为d₁＝d_ω×A，使得p₂满足：

d₁-σ≤||p₂-p₃||≤d₁+σ (8)

且满足：

即在满足边长限制的同时，所得边尽量垂直；

步骤45：随机寻找第四点p₄，使得||p₄-p₂||≥d₁×0.5，且满足：

π/2≤∠P₂P₄P₁≤2π/3 (9)；

步骤46：在所述备选集{p5}中随机选取一点作为非共面点，与p₁、p₂、p₃、p₄组成非共面5点基B。

进一步地，步骤4中所述非共面5点基的特征描述为：

{d₁，d₂，d₃，r₁，r₂，θ₁，θ₂}，其中{d₁，d₂，r₁，r₂，θ₁}为沿用Super-4PCS算法中共面特征，d₃为非共面点f与底面对角线交点e的距离且d₃＝||f-e||₂，θ₂为e点拟合向量

与

的夹角且

为了简化底面{a，b，c，d}的法向量计算开销，将底面四点{a，b，c，d}的法向量均值

记为：

其中，

分别表示底面a，b，c，d的法向量。

进一步地，步骤5所述的全等非共面5点基的查找步骤包括：

步骤51：输入目标点云Q和非共面5点基B所对应的特征不变量{d₁，d₂，d₃，r₁，r₂，θ₁，θ₂}，其中点云模型Q在步骤24中已建立平面内点集合{inliers}与外点集合{outliers}；

步骤52：采用Super-4PCS算法中的超球栅格化进行三维球与点的相交计算，在一定距离误差ε下，在点云Q的内点集合{inliers}中提取d₁与d₂的对应等距点对集合{q_i，q_j}；

步骤53：基于r₁与r₂，计算点对的交点集{e₁}与{e₂}，并基于交点的位置与对应的线段方向建立hash索引，使用三维格网对中间点进行存储；

步骤54：使用位置hash检索{q_i，q_j}中点对的中间点{e_1i，e_2j}正好处于同一个hash方格中，将中间点对作为交点备选集；

步骤55：使用hash表在高斯球面格网上进行角度搜索，将方向夹角等于θ₁的中间点对作为交点，建立5面体基B的近似全等底面集合Bottom＝{Bt₁、Bt₂、Bt₃、…}以及对应的对角线交点集合Intersection＝{It₁、It₂、It₃、…}；

步骤56：对集合Intersection内每一个交点建立近似向量

与对应的高斯球面hash值；

步骤57：基于每一个交点位置{It_i}与距离d₃，使用步骤52中所述超球栅格化在外点集合{outliers}中搜索非共面点f的备选集合{f_i}；

步骤58：基于{f_i}的法向量

建立高斯球面索引，提取与

夹角为θ₂的非共面点子集{f’_i}，最终获得近似全等非共面5点集合{B’}。

进一步地，步骤6中计算变化矩阵的步骤为：

步骤61：假设B＝{p₁、p₂、p₃、p₄、p₅}，Q中近似全等基集合{B_i’}中任意一个基为B’＝{q₁、q₂、q₃、q₄、q₅}，所得变换关系为H，包括旋转与缩放L与平移t，即相似变换关系表示为：

H(B′)＝LB′+t (11)；

步骤62：根据步骤27的缩放尺度，将点云间的相似变化转化为刚体变换关系L：

L(q_i)+t＝p_i (12)；

步骤63：用方程(7)减去方程(6)，可得：

L(q_i-q_j)+t＝(p_i-p_j) (13)；

其中，{(q_i-q_j)}＝[q₁-q₂、q₁-q₃、q₁-q₄、q₁-q₅]，即B中的两两线性无关的边；{(p_i-p_j)}＝[p₁-p₂、p₁-p₃、p₁-p₄、p₁-p₅]，即B’中的对应的线性无关的边；

步骤64：根据公式(8)可得矩阵L为：

L＝{(p_i-p_j)}×{(q_i-q_j)}^-1 (14)；

其中，L为正定方阵；

步骤65：使用SVD分解SVD(L)＝U∑V^T，U与V代表旋转，∑代表尺度，可得旋转矩阵R＝UV^T＝L，将经过L变换后{q_i}的中心点与{p_i}的中心点相减即可获得平移向量t。

进一步地，步骤7中得到最优变换矩阵T的步骤为：

步骤71：假设源点云P中的一点p_i，法向量为

在目标点云Q中的对应点为q_i，法向量为

近似全等基B_i’所对应的变换矩阵为H_i，用公式(10)进行一致性约束，获得LCP：

步骤72：依据得到的LCP对点云匹配的变换矩阵H_i进行一致性度量，计算一致性度量下的最优解，求得的最优解即为最优变换矩阵T。

本发明的有益效果是：

本发明提出了一种面向道路精细重建的多源点云粗匹配算法，所使用的数据为无人机Lidar获得的道路点云以及路面近景摄影测量点云，本发明的算法属于Super-4PCS的改进算法，基于RANSAC+LCP的匹配策略，使用Super-4PCS中类似的智能索引技术完成全等5点集合的快速检索与匹配，本发明通过设计了一种非共面5点基以及对应的特征描述，并结合基于路面凹凸度相似性完成了具有尺度差异的异源点云的匹配。其与现有技术相比具有以下优点：

第一，本发明基于点云粗略重叠区域内凹凸程度的相似性获取点云之间近似比例，将点云之间的相似变化转化为刚体变化，避免了在相似变换中全等点对与全等基的数量呈二次方级别增长导致的匹配失败。所求尺度误差较小，可在之后的ICP精确匹配中不断优化，最终接近点云的真实尺度，弥补了4PCS类算法无法匹配未知尺度点云的缺陷。

第二，针对较大数据量的点云匹配时，Super-4PCS使用均匀采样无法保证采样点的重复率与特征分布，影响后续匹配精度的问题，本发明使用3D-Harris进行特征点云的提取，进行完成点云采样，为后续的匹配精度打好基础。

第三，Super-4PCS在具有一定平面分布特征的点云匹配时，平面特征退化明显会导致匹配失败，本发明提出了一种高区分度的非共面五点基以及对应的特征描述方法，结合智能索引(超球栅格化与hash检索)以完成相关点云的匹配，有效提高了匹配成功率。

第四，为了进一步提高算法的匹配效果，本发明在确定最优变换矩阵时加入了公共点对的法向量夹角限制，增强了公共点统计的一致性评价，提高了匹配算法的精准度。

附图说明

图1为Super-4PCS算法的匹配实验结果图；

图2为Nc-5PCS的算法流程图；

图3(a)-(b)为3D-Harris特征点云的提取效果图，其中(a)为无人机Lidar点云，(b)为近景摄影测量点云；

图4为源点云中非共面5点基的选取过程图；

图5a为非共面5点基的特征描述图；图5b为近景摄影测量点云的法向量坡度分量(Dip)的分布图；

图6为非共面5点基的查找流程图；

图7(a)-(c)为LiDAR点云与近景摄影测量点云的匹配结果图，其中(a)为无人机LiDAR点云，(b)为近景摄影测量点云，(c)为两个点云的匹配效果；

图8为等距点对的搜索；

图9为近似全等四点集合中的错误对应基；

图10(a)-(b)为全等四点的查找。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

本发明针对4点全等集合点云匹配算法(4Points Congruent Sets for RobustPairwise Surface Registration，4PCS)对较大数据量点云处理效率低、无法有效匹配具有一定平面分布特征的点云以及具有未知尺度的点云等难点问题，提出了非共面5点全等集合点云匹配算法(Non-coplanar5Points Congruent Sets for Robust PairwiseSurface Registration，Nc-5PCS)。

附图2为Nc-5PCS算法流程与关键部分的计算复杂度，其对应的伪代码(关键部分)如表1所示。

表1 Nc-5PCS算法伪代码

本发明以道路的无人机Lidar点云与近景摄影测量点云为道路多源点云的示例数据，应用本发明所提的匹配算法进行道路多源点云匹配，且本算法适用于所有类型的道路点云数据。

1、4PCS类算法存在的问题

4PCS类算法依据仿射不变性在原理上可以处理相似变换的点云匹配。但当去除点对距离的限制进行相似变换关系求解时，需要计算与存储中间点的数量由O(n)变为O(n²)，n为点云的数量；基于range tree插入与检索中间点的时间复杂度由O(k₁logk₁+k₂)变为O(n²logn+k'₂)，k1为查找的等距点对的数量(通常k₁远小于n)，k₂与k'₂为交点的数量(等于全等备选四点集合数量)；全等备选四点集合数量k₂远小于k'₂，导致过滤计算开销较大；Super-4PCS虽然使匹配的复杂度降低为线性，但其所使用的智能索引主要优化等距点对提取部分的计算效率，无法针对相似变换进行求解。因此，4PCS类算法无法有效解决具有未知尺度的点云匹配。

目前，4PCS类算法主要用于刚体变换关系求解，但无论是在点云不同分割区域选取点对降低中间点数量的优化方法，还是基于全体点云的Spin Image统计计算近似尺度的方法，点云的分割以及局部特征计算开销都很大，严重影响了整个算法的效率，无法有效处理较大数量点云。

2、Nc-5PCS算法中多源点云间的尺度求解

(2)尺度(即近似比例)计算方法

本发明针对路面点云具有一定面积平面分布特征，提出了一种简单、高效的近似比例计算方法，将后续相似变换关系转化为刚体变换关系求解，所得近似比例可通过ICP算法迭代求精，最终获得正确缩放比例系数。

所述近似比例计算方法通过平面拟合之后的局部凹凸度相似性求解获取点云之间的近似比例。如附图3所示，由于点云P与Q存在一定面积的平面分布，可以通过拟合平面求取平均凹凸度(凹凸度表现为路面坑洞深度)。其通过多源点云之间路面重叠区域经平面拟合之后的局部凹凸度相似性求解点云之间的近似比例。由于无人机LiDAR点云数据具有GPS位置信息，近景摄影测量所获取的相片具有GPS位置信息，虽然以上位置信息精度较差无法参与点云匹配，但可以确定点云在区域中的大致位置，进而确定粗略重叠区域。故在GPS确定粗略重叠区域后先使用RANSAC方法找到区域内高差值(坐标Z值)小的点作为内点，使用最小二乘方法进行路面平面方程Ax+By+Cz+D＝0(C≠0)的求解，具体的近似比例求解方法为：

S1：基于GPS位置信息确定粗略重叠区域，假设平面方程为Ax+By+Cz+D＝0(C≠0)，将平面方程转化为z＝ax+by+c，其中，a＝-A/C，b＝-B/C，c＝-D/C；从点云中随机选取3个点，判断三点是否共线，若不共线则计算3点对应的平面方程，否则重新选取3点；

其中的A、B、C、D和a、b、c均为平面方程的参数；

S2：计算点云中各个点(x_i，y_i，z_i)到所得平面的距离d_i：

S3：计算所得距离{d_i}的标准偏差σ：

其中，

取阈值τ＝2σ；

S4：判断各个点的距离d_i＞τ时，则该点为高程异常值，存为外点outliers，其他点作为内点inliers用于平面拟合，建立平面内点集合{inliers-Q}和外点集合{outliers-Q}，并分别统计点数N_outliers与N_in1iers，在后续循环遍历时，作为遍历次数的约束条件；

S5：重复S1-S4迭代N次，选取内点数最多的点集进行平面拟合，通过对所有内点inliers组成的协方差矩阵进行SVD分解完成此最小二乘问题的求解，最终获得平面方程z＝a₀x+b₀y+c₀，且迭代次数N计算公式为：

其中，p为预设的正确概率，一般p＝99％，w为内点概率；

S6：为提高算法效率，在距离求解时仅遍历外点outliers，最终获得粗略重叠区域凹凸度的均值dis＝Average({dis_i})：

S6：计算点云之间的尺度，计算公式为：

其中，d_Q-average和d_P-average分别为点云Q和点云P粗略重叠区域的凹凸度均值；

3、Nc-5PCS算法中多源点云间的简化

3D Harris算法简单、计算效率高，由于仅使用了点云的几何属性，适合多源点云的特征点提取。因此本发明以3D-Harris作为多源点云的特征点提取算法，对点云进行简化(由于3D-Harris为三维点云特征提取的经典算法，不再赘述)。附图3所示为无人机Lidar点云和近景摄影测量点云的3D-Harris特征点云的提取效果。

4、Nc-5PCS算法中非共面5点基的构建

点云匹配所用的基，是点云之间的对应特征，是点云匹配的基础。一般来说，在源/场景点云P中进行设计与构建，在模型/目标点云Q中进行全等基的查找，并依据对应点位置计算点云的变换矩阵。

本发明通过引入非共面的第5点构建三维分布的5面体基。如图4所示，非共面的第5点可以显著增强基的几何特征，使得模型点云中的全等基数量大大降低，从而降低了全等集合的确认计算开销。特别是点云具有一定面积的平面分布特征时，非共面基的优势更加明显。图4中的最左边的图为源点云在平面拟合后的距离分布，中间的四幅小图中由上至下分别为拟合平面的外点，拟合平面的内点，在外点中深度值较大的子集中选取非共面点以及在内点结合中选取底面四点，最右边的图为最终选取的非共面5点。

基于平面拟合过程中平面内点与外点的hash储存，将外点中距离平面较远的子集{outliers，d_outliers≥d_Localmax×60％}，作为非共面点的备选集{p5}，随机选取非共面点，其中d_outliers为点云平面拟合之后的外点到拟合平面的距离，d_Localmax是外点中到拟合平面的最远的距离；从内点集合{inliers}中选取共面4点，最终组成非共面5点基，如图4所示，构建上述非共面5点基的具体步骤为：

S1：假设特征源点云P与特征目标点云Q的近似重叠度为ω，由于无人机LiDAR点云数据具有GPS位置信息，根据定位信息可以估计近似重叠度，但对于没有辅助信息进行重叠度估计时，则需要按照假设的近似重叠度ω的值从大到小进行遍历{1、0.75、0.5、0.25、…}，根据重叠度ω估计底面四点的边长约束d_ω：

d_ω＝dia_p×ω (6)

其中，dia_p为点云p的直径(即点云外包体的边长)；

S2：依据计算的边长约束，设置边长为d₀＝d_ω×A，A为(0.7，0.95)范围内的随机值，以避免较宽边长无法找到满足角度要求的底面四点；在内点集合中，随机选取两点p₁与p₃，使得：

d₀-σ≤||p₁-p₃||≤d₀+σ (7)

其中，σ为距离的误差阈值；

S3：随机寻找内点集合中的第三点p₂，设置边长为d₁＝d_ω×A，使得p₂满足：

d₁-σ≤||p₂-p₃||≤d₁+σ (8)

并且满足

即在满足边长限制的同时，所得边尽量垂直；

S4：随机寻找内点集合中的第四点p₄，使得||p₄-p₂||≥d₁×0.5，且满足：

π/2≤∠P₂P₄P₁≤2π/3 (9)

S5：在外点集合的子集O中，随机选取一点作为非共面点，与p₁、p₂、p₃、p₄组成非共面5点基B。

与4PCS类搜索共面四点相比，非公共面5点基在构建底面4点时有明显的优势：①由于非共面5点中底面4点都是在平面拟合的内点中选取，所以省去了四点共面判断的计算开销。②由于限制了底面4点搜索时的边长与夹角，所以也省去了对底面凹凸四边形的判断计算，并避免了狭长四边形的出现。

5、Nc-5PCS算法中非共面5点基的特征描述

建立完非共面5点基B后，就要进行非共面5点基B的特征描述与匹配，需要构建B的不变特征，即建立查找对应全等基所依据的不变量。所构建的特征既要简单，便于存储与计算；也要充分描述B的几何属性，以便降低近似全等基的数量，从而降低最佳匹配基的确认计算开销。

通过借鉴Super-4PCS算法，将平面的特征简化为对角线长、夹角与仿射比例，并基于智能索引对相关特征进行高效的存储与检索，本发明依然沿用Super-4PCS中的智能索引方法，并通过添加少量的特征元素，将平面特征扩展为5面体特征。如图5a所示，在原Super-4PCS的共面特征{d₁,d₂,r₁,r₂,θ₁}的基础上，增加了非共面点f与底面对角线交点e的距离d₃，d₃＝||f-e||₂，以及e点拟合向量

与

的夹角θ₂，

即图5a所示的非共面5点基的特征为{d₁,d₂,d₃,r₁,r₂,θ₁,θ₂}。由于底面4点都是在平面的内点中选取，如图5a所示内点的法向量大部分都垂直于拟合平面，为了简化底面{a,b,c,d}的法向量计算开销，将底面四点a,b,c,d的法向矢量之和作为平面的法向量均值，即：

相比于其他方式的特征描述，此种特征描述所记录的特征元素少，能有效降低近似全等基的搜索计算开销；此种特征描述中法向量的近似求解方式，计算简单且效率高。

6、Nc-5PCS算法中非共面5点基的查找(即匹配)

在建立源点云P中非共面5点基B的特征描述后，依据此特征在模型点云Q中查找对应的全等5点基集合。使用类似Super-4PCS中的智能索引方法，来完成全等集合的遍历。附图6为查找流程的示意图，其包括以下步骤：

S1：输入目标点云Q和非共面5点基B所对应的特征不变量{d₁,d₂,d₃,r₁,r₂,θ₁,θ₂}，其中点云模型Q已在近似比例计算中建立了平面内点集合{inliers-Q}与外点{outliers-Q}集合；

S2：采用Super-4PCS算法中的超球栅格化完成三维球与点的相交计算，在查找全等非共面基的时，将点对所对应的向量映射为高斯球面的点，将角度匹配转化为球面圆形与球面栅格化的求交，以便快速完成角度检索，在检索中，基于点对的中间点坐标与单位化方向矢量建立哈希索引快速完成检索，在一定距离误差ε下，在点云Q的内点集合{inliers-Q}中提取d₁与d₂的对应等距点对集合{q_i,q_j}，超球栅格化过程如图8-10所示；

其中，图8的共面四点基{p₁,p₂,p₃,p₄}的对角线长度为d₁与d₂，依据此距离不变量在Q中查找等距点对；图9点云P中的四点基{p₁,p₂,p₃,p₄}与点云Q中{q₁,q₂,q₃,q₄}是对应的全等基，以交点e为中心，所有(q₃,q₄)的旋转线{(q’₃，q’₄)}都可以与(q₁,q₂)组成满足仿射不变性的冗余4点基；图10(a)为四点基的对角线夹角α，图10(b)为依据中间等比点e₁与e₂位置Hash查找近似全等四点，并依据角度不变量α与高斯球面索引查找全等共面四点，d为对角线矢量在高斯球面的映射点；

S3：基于r₁与r₂，计算点对(例如图5a中的{a,c},{d,b})的中间点/交点集{e₁}与{e₂}，并基于交点的位置与对应的线段方向建立hash索引，使用三维格网对中间点进行存储；

S4：使用位置hash获取在同一格网位置的中间点{e_1i,e_2j}，将其作为交点备选集，之后，使用hash表在高斯球面格网上进行角度检索，将方向夹角等于θ₁的中间点对作为交点，至此，建立了5面体基B的近似全等底面集合Bottom＝{Bt₁、Bt₂、Bt₃、…}以及对应的对角线交点集合Intersection＝{It₁、It₂、It₃、…}；

S5：对集合内每一个交点建立近似向量

与对应的高斯球面hash值；

S6：基于每一个交点位置{It_i}与距离d₃，使用步骤S2中超球栅格化在外点集合{outliers-Q}中搜索非共面点f的备选集合{f_i}；

S7：基于{f_i}的法向量

建立高斯球面索引，使用与步骤S4类似的方式提取与

夹角为θ₂的非共面点子集{f’_i}，最终获得近似全等非共面5点集合{B’}。

7、Nc-5PCS算法中多源点云间变换矩阵的求解

当找到基B在模型点云Q中的近似全等基{B_i’}后，可依据顶点的对应关系，计算变换矩阵H_i，并依据一致性度量选取最优解，完成模型匹配。目前，现有技术中已公开求解变换矩阵的方法，因此本发明仅介绍求解步骤，不对公式推导进行展开说明。

(1)求解变换矩阵

假设B＝{p₁、p₂、p₃、p₄、p₅}，Q中近似全等基集合{B_i’}中任意一个基为B’＝{q₁、q₂、q₃、q₄、q₅}，所得变换关系为H，包括旋转与缩放L与平移t，即变换关系表示为：

H(B′)＝LB′+t (11)

由于前面已经完成了点云之间的缩放尺度的求解，因此点云间为刚体变换关系：

L(q_i)+t＝p_i (12)

通过方程(7)减(6)，可得：

L(q_i-q_j)+t＝(p_i-p_j) (13)

其中，{(q_i-q_j)}＝[q₁-q₂、q₁-q₃、q₁-q₄、q₁-q₅]，即B中的两两线性无关的边；{(p_i-p_j)}＝[p₁-p₂、p₁-p₃、p₁-p₄、p₁-p₅]，即B’中的对应的线性无关的边；

通过式(8)可得刚体变化矩阵L为：

L＝{(p_i-p_j)}×{(q_i-q_j)}^-1 (14)

其中，矩阵L是正定方阵；

使用SVD分解SVD(L)＝UEV^T，U与V代表旋转，∑代表尺度(刚体变换下是单位矩阵)，因此旋转矩阵R＝UV^T＝L，将经过L变换后{q_i}的中心点与{p_i}的中心点相减即可获得平移向量t。

非共面5点基的匹配(查找)算法在每次筛选近似全等基{B_i’}以及RANSAC迭代中都需要比较所得变换矩阵，因此需要选择一致性度量下的最优解作为最终的匹配结果。目前4PCS类算法普遍采用满足一定距离误差阈值δ下的一致点个数来进行度量，即最大公共点数(LCP)。

为了提高算法的匹配精度，本发明使用距离误差约束δ与法向量夹角约束

假设源点云P中的一点p_i，法向量为

在模型点云Q中的对应点为q_i，法向量为

近似全等基B_i’所对应的变换矩阵为H_i，用如下公式进行一致性约束，获得LCP：

最终获得满足以上收敛条件的变换矩阵参数T，完成道路多源点云匹配，匹配结果如附图7所示。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (7)

1.一种面向道路精细重建的多源点云粗匹配算法，其特征在于，包括以下步骤：

步骤1：获取目标道路的源点云P和目标Q；

步骤2：基于点云粗略重叠区域内凹凸程度的相似性获取源点云P和目标点云Q之间近似比例，将点云之间的相似变化转化为刚体变化；

步骤3：通过3D-Harris特征点提取算法对点云P和Q进行简化，得到特征源点云P和特征目标点云Q；

步骤4：基于步骤2得到的近似比例，在特征源点云P中构建非共面5点基B，并建立非共面5点基的特征描述；

步骤5：采用超球栅格化与哈希检索在特征目标点云Q中依据所述非共面5点基的特征描述查找近似的全等5点基集合{B’}；

步骤6：遍历B和{B’}进行匹配，得到全等非共面5点基和对应的变换矩阵H；

步骤7：基于LCP进行一致性度量，计算一致性度量下的最优解，以求得的最优解作为最优变换矩阵T；

步骤8：根据得到的最优变换矩阵T，完成道路多源点云匹配。

2.根据权利要求1所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤2的包括以下具体操作步骤：

步骤21：基于GPS位置信息确定粗略重叠区域，假设平面方程为Ax+By+Cz+D＝0(C≠0)，将平面方程转化为z＝ax+by+c，其中，a＝-A/C，b＝-B/C，c＝-D/C；从点云中随机选取3个点，判断三点是否共线，若不共线则计算这3个点所对应的平面方程，否则重新选取3点；

步骤22：计算点云中各个点(x_i，y_i，z_i)到所得平面的距离d_i：

其中，a，b，c均为平面方程z的参数；

步骤23：计算所得距离{d_i}的标准偏差σ：

其中，

取阈值τ＝2σ；

步骤24：判断各个点的距离d_i＞τ时，则该点为高程异常值，存为外点outliers，其他点作为内点inliers用于平面拟合，形成外点集合{inliers}和外点集合{outliers}，并分别统计点数N_outliers与N_inliers；

步骤25：重复步骤21-24迭代N次，选取内点数最多的点集进行平面拟合，通过对所有内点inliers组成的协方差矩阵进行SVD分解，最终获得平面方程z＝a₀x+b₀y+c₀，且：

其中，a₀、b₀与c₀为平面方程的参数，p为设定的正确概率，一般p＝99％，，w为内点概率；

步骤26：在距离求解时仅遍历外点outliers，最终获得重叠区域凹凸度的极值dis＝Max({dis_i})为：

步骤27：计算点云之间的近似比例，且计算公式为：

其中，d_Q-average与d_p-average分别为点云Q与点云P粗略重叠区域的凹凸度均值。

3.根据权利要求2所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤4中所述非共面5点基的构建步骤包括：

步骤41：基于平面拟合过程中平面内点与外点的hash储存，将外点中距离平面较远的子集{outliers，d_outliers≥d_Localmax×60％}，作为非共面点的备选集{p5}，用以随机选取1个非共面点；以所述内点集合{inliers}为共面4点的备选集；

步骤42：设特征源点云P和特征目标点云Q的近似重叠度为ω，如果没有预估重叠度，按照ω的值从大到小遍历{1、0.75、0.5、0.25、…}，根据重叠度ω估计底面四点的边长约束d_ω：

d_ω＝dia_p×ω (6)

其中，dia_p为点云p的直径；

步骤43：依据所述边长约束，设置边长为d₀＝d_ω×A，其中A为(0.7，0.95)范围内的随机值，在内点集合中，随机选取两点p₁与p₃，使得：

d₀-σ≤||p₁-p₃||≤d₀+σ (7)

其中，σ为距离的误差阈值；

步骤44：在内点集合中随机寻找第三点p₂，设置边长为d₁＝d_ω×A，使得p₂满足：

d₁-σ≤||p₂-p₃||≤d₁+σ (8)

且满足：

即在满足边长限制的同时，所得边尽量垂直；

步骤45：随机寻找第四点p₄，使得||p₄-p₂||≥d₁×0.5，且满足：

π/2≤∠P₂P₄P₁≤2π/3 (9)；

步骤46：在所述备选集{p5}中随机选取一点作为非共面点，与p₁、p₂、p₃、p₄组成非共面5点基B。

4.根据权利要求3所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤4中所述非共面5点基的特征描述为：{d₁，d₂，d₃，r₁，r₂，θ₁，θ₂}，其中{d₁，d₂，r₁，r₂，θ₁}为沿用Super-4PCS算法中共面特征，d₃为非共面点f与底面对角线交点e的距离且d₃＝||f-e||₂，θ₂为e点拟合向量

与

的夹角且

为了简化底面{a，b，c，d}的法向量计算开销，将底面四点{a，b，c，d}的法向量均值

记为：

其中，

分别表示底面a，b，c，d的法向量。

5.根据权利要求4所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤5所述的全等非共面5点基的查找步骤包括：

步骤51：输入目标点云Q和非共面5点基B所对应的特征不变量{d₁，d₂，d₃，r₁，r₂，θ₁，θ₂}，其中点云模型Q在步骤24中已建立平面内点集合{inliers}与外点集合{outliers}；

步骤52：采用Super-4PCS算法中的超球栅格化进行三维球与点的相交计算，在一定距离误差ε下，在点云Q的内点集合{inliers}中提取d₁与d₂的对应等距点对集合{q_i,q_j}；

步骤53：基于r₁与r₂，计算点对的交点集{e₁}与{e₂}，并基于交点的位置与对应的线段方向建立hash索引，使用三维格网对中间点进行存储；

步骤54：使用位置hash检索{q_i,q_j}中点对的中间点{e_1i,e_2j}正好处于同一个hash方格中，将中间点对作为交点备选集；

步骤55：使用hash表在高斯球面格网上进行角度搜索，将方向夹角等于θ₁的中间点对作为交点，建立5面体基B的近似全等底面集合Bottom＝{Bt₁、Bt₂、Bt₃、…}以及对应的对角线交点集合Intersection＝{It₁、It₂、It₃、…}；

步骤56：对集合Intersection内每一个交点建立近似向量

与对应的高斯球面hash值；

步骤57：基于每一个交点位置{It_i}与距离d₃，使用步骤52中所述超球栅格化在外点集合{outliers}中搜索非共面点f的备选集合{f_i}；

步骤58：基于{f_i}的法向量

建立高斯球面索引，提取与

夹角为θ₂的非共面点子集{f’_i}，最终获得近似全等非共面5点集合{B’}。

6.根据权利要求5所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤6中计算变化矩阵的步骤为：

步骤61：假设B＝{p₁、p₂、p₃、p₄、p₅}，Q中近似全等基集合{B_i’}中任意一个基为B’＝{q₁、q₂、q₃、q₄、q₅}，所得变换关系为H，包括旋转与缩放L与平移t，即相似变换关系表示为：

H(B′)＝LB′+t (11)；

步骤62：根据步骤27的缩放尺度，将点云间的相似变化转化为刚体变换关系L：

L(q_i)+t＝p_i (12)；

步骤63：用方程(7)减去方程(6)，可得：

L(q_i-q_j)+t＝(p_i-p_j) (13)；

其中，{(q_i-q_j)}＝[q₁-q₂、q₁-q₃、q₁-q₄、q₁-q₅]，即B中的两两线性无关的边；{(p_i-p_j)}＝[p₁-p₂、p₁-p₃、p₁-p₄、p₁-p₅]，即B’中的对应的线性无关的边；

步骤64：根据公式(8)可得矩阵L为：

L＝{(p_i-p_j)}×{(q_i-q_j)}^-1 (14)；

其中，L为正定方阵；

步骤65：使用SVD分解SVD(L)＝U∑V^T，U与V代表旋转，∑代表尺度，可得旋转矩阵R＝UV^T＝L，将经过L变换后{q_i}的中心点与{p_i}的中心点相减即可获得平移向量t。

7.根据权利要求6所述的一种面向道路精细重建的多源点云粗匹配算法，其特征在于，步骤7中得到最优变换矩阵T的步骤为：

步骤71：假设源点云P中的一点p_i，法向量为

在目标点云Q中的对应点为q_i，法向量为

近似全等基B_i’所对应的变换矩阵为H_i，用公式(10)进行一致性约束，获得LCP：

步骤72：依据得到的LCP对点云匹配的变换矩阵H_i进行一致性度量，计算一致性度量下的最优解，求得的最优解即为最优变换矩阵T。

Images