[发明专利]一种基于三维曲线匹配的点云相对位姿估计方法

说明书

技术领域

本发明涉及三维环境重构领域，尤其涉及一种基于三维曲线匹配的点云相对位姿估计方法。

背景技术

已有的点云相对位姿估计方法主要分为两类。一类基于点云特征匹配，缺点是受点云密度变化影响很大，密度差异会直接导致错误概率提高。另一类基于点云的直接匹配，缺点是依赖初始位姿，且运算量大。

对于三维曲线的匹配，已有方法也主要基于卷积，难以适用于多曲线对多曲线的匹配。

发明内容

本发明的目的是克服现有技术的不足，提供一种基于三维曲线匹配的点云相对位姿估计方法。

基于三维曲线匹配的点云相对位姿估计方法是：

1）分别提取两帧输入点云的边缘轮廓，得到代表三维边缘轮廓曲线的三维点集                                                、；

2）分别针对三维点集、提取得到有序点集的集合、,每个有序点集或均表示了一条三维曲线；

3）将每个有序点集或进行分解，分解后的每个子集表示一个圆弧段，建立每段圆弧的特征描述向量；

4）匹配两帧点云边缘曲线圆弧段的特征描述向量，建立匹配关系，计算两帧点云的相对位姿变换矩阵,为旋转矩阵，为平移矩阵。

所述的步骤1）为：a）对于无序点云，搜索每个点周围半径小于范围内的数量不超过的所有最近邻点，得到点集，为点集拟合平面，以点集在平面内的投影点位置为自变量，点集到平面的距离为函数值，拟合二元二次函数，得到Hessian矩阵，计算Hessian矩阵的特征值和，假定，若且，其中、分别为相应阈值，则认为该点为边缘点；b）对于有序点云,即深度图，利用Canny算法提取边缘点集。

所述的为点集拟合平面的方法为：计算点集的均值，得到平面的中心；计算的特征向量，其最小特征值对应的特征向量即为平面的法向量；平面的中心和法向量即表示了一个经过中心、法向量为的平面。

所述的拟合二元二次函数并得到Hessian矩阵的方法为：对于点集中每一点，假定最大的两个特征值对应的特征向量分别为和，计算一个以、为自变量，为值的键值对， 

最终形成一组向的键值映射,并利用最小二乘法求取Hessian矩阵

，

则可表示为

。

所述的步骤2）为：使用区域增长算法，以每次从三维点集或中随机选取的区域增长种子点为起始，以点集所表示的曲线一阶连续为条件，通过不断吸纳符合增长条件的新点，有序地拓展曲线点集，若不能再拓展，则另取种子点并拓展另一个曲线点集，直至所有点被拓展完毕。

所述的曲线一阶连续条件为：

其中为点云中两近邻点、之间的距离，、分别为两点离坐标原点的距离，为阈值。

所述的步骤3）为：对于每一个表示三维曲线的有序点集或，依次计算每个点处曲线的曲率和扭率，

其中

为有序点集中的下一个点，随后将表示三维曲线的有序点集拆分成若干子集，每个子集内的点具有相同的曲线曲率，且扭率为零，即该子集表示了一个圆弧，记该圆弧曲率为，弧长为，中心坐标为，依据右手坐标系规则，以圆弧法方向、圆弧起始点切法向以及与这两个方向垂直的方向为三个轴，建立坐标系，记该坐标系和世界坐标系所成角度向量为,定义该圆弧的特征描述向量为

。

所述的步骤4）为： 针对两帧点云边缘曲线圆弧段的特征描述向量，建立k-d树，在k-d树中，建立两帧点云边缘曲线圆弧段之间的匹配关系，运用随机抽样一致性算法，去除部分错误匹配，建立代价函数

其中为两帧点云边缘曲线圆弧段之间的匹配关系中一组匹配关系，为所有存在的匹配关系，、分别为两帧点云中的一段圆弧段的特征描述向量，

是评估特征描述向量、之间距离的函数，与两帧点云的相对位姿转换矩阵有关，的定义为

其中、为指定权重的参数，

最后，优化求解

得到两帧点云的相对位姿变换矩阵。

本发明与现有技术相比，具有的有益效果：

1.不依赖初始位姿，仅通过环境的轮廓进行匹配，估计相对位姿；

2.仅处理边缘点集，降低了运算量；

3.适应多曲线对多曲线的匹配，可抵抗点云中干扰物造成的影响。