一种基于视觉矫正的三维点云地图融合方法与流程

本发明涉及计算机视觉领域，主要涉及三维点云地图融合，具体提供了一种基于视觉矫正的方法，可用于解决因视角差异过大导致点云地图融合失败的问题。

背景技术：

伴随着立体相机的快速发展，三维点云数据已经广泛的应用在计算机视觉领域中，例如机器人构建地图、导航、物体识别、跟踪。当机器人处于广阔的外部环境或者复杂的内部环境时，slam构建地图虽然能够解决机器人自主导航，但是由于环境的复杂性、广阔性，构建地图时会出现各种问题，比如构建地图过程中消耗时间过长以至于导航失败。因此，在这种情况下需要多个机器人共同构建地图，然后将各自的地图拼接成完整的地图，从而实现地图的共享，这样就可以提高自主导航的效率。

点云地图融合是指：将不同视角、不同位置采集的点云地图通过刚体变换将来自多个视角的点云融合在一起。besl，p.j.&mckay，n.d.(1992).amethodforregistrationof3-dshapes.ieeetransactionsonpatternanalysisandmachineintelligence，14(2)，239-256.公开了iterativeclosestpoint(icp)算法，通过寻找最近对应点，估算转换矩阵，计算目标函数的最小值，最后经过多次的迭代，计算出转换矩阵，然后将两个点云融合在一起。该算法只有提供精确的初始值时，才能得到比较良好的的融合结果。

为了计算精确的初始值，很多研究学者通过寻找特征匹配来计算初始值。klasing等人采用了基于优化平均的方法估计法向量，例如planepca，planesvd，quadsvd方法。然而点云的法向量包含的数据信息较少，点云地图视角的变化会导致点云法向量的计算，最终影响点云地图融合的效果。gelfand提供了提取点云体积的方法，然而该方法确不能适应点云地图视角变换。zou通过提取点云高斯曲率的方法来计算初始值，但是伴随着点云地图视角的变化，同一个物体的高斯曲率发生变化，这样会导致错误的特征匹配，从而点云地图融合失败。rusu等人提出了一种提取点云fpfh特征的方法，但是该方法存在着权重系数溢出、特征匹配错误的问题。ransac算法是融合算法的另外一种，最初由fischler等人提出，采用随机采样一致性来匹配一个数学模型，算法比较稳定但是该方法的复杂度在最坏情况下会达到o(n3)，当点云数量庞大时，算法时间消耗不可接受。

技术实现要素：

本发明的目的在于提供一种融合精度高、效率高的基于视觉矫正的三维点云地图融合方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种基于视觉矫正的三维点云地图融合方法，包括以下步骤：

1)对三维点云地图进行预处理，首先输入两个pcd格式的三维点云地图文件，从而获得有待融合的两个点云地图，这两个点云地图分别为目标点云地图pointtgt和待融合的点云地图pointsrc，然后对这两个点云地图进行预处理；

2)对于步骤1)中的两个点云地图，分别提取目标点云地图pointtgt和待融合的点云地图pointsrc的3d-sift关键点；

3)对步骤2)中的两个点云地图3d-sift关键点分别提取ipfh特征，得到待融合的点云地图pointsrc的特征向量p和目标点云地图pointtgt特征向量q，其中特征向量p有m个特征点，特征向量q有n个特征点；

4)计算特征向量p和特征向量q中特征点的欧氏距离，根据最小距离找到特征匹配点；

5)通过特征匹配点计算旋转矩阵，利用矩阵将点云地图pointtgt旋转，改变该点云地图的视角，缩小两个点云地图的视角差异；

6)采用icp精确融合算法将缩小视角差异后的两个三维点云地图融合在一起。

作为本发明进一步的方案：步骤1)中，所述预处理包括去噪和采样。

作为本发明再进一步的方案：步骤2)中，3d-sift关键点提取的具体步骤为：

i、点云尺度空间中检测极值点，对点云地图进行不同程度的下采样构建金字塔模型，对每一组点云地图构建高斯尺度空间，所用的函数如下所示

尺度空间：l(x，y，z，σ)＝g(x，y，z，kσ)*p(x，y，z)

高斯差分函数：

参数k指点金字塔中的组数，σ是尺度大小；

构建高斯差分空间：d(x，y，z，kⁱσ)＝l(x，y，z，kⁱ⁺¹σ)-l(x，y，z，kⁱσ)

构建dog空间后，通过对比的方法寻找极值点：对于某个点，其取值大于或者小于其邻域中所有的点的时候，即看做极值点；

ii、确定关键点方向，计算极值点所在邻域内每个点的方位角和仰角，计算公式如下：

公式中d表示极值点与邻域中心点之间的距离，θ表示方位角，φ表示仰角，x0，y0，z0指的是极值点所在邻域内的中心点；

统计邻域内每个点方位角θ和仰角φ的方向：采用两个直方图来统计每个点两个角度的方向，θ的方向取值范围为[0°，360°]，采用8柱直方图统计方向的情况，每个柱代表45度；而φ的取值为[-90°，90°]，采用4柱直方图来统计方向的情况，每个柱代表45度；

统计后，取两个直方图的峰值作为极值点的方位角和仰角，计算后，每个关键点就包含了x、y、z坐标信息、尺度信息和方向信息即(x，y，z，σ，θ，φ)；

iii、将某个关键点p所在的坐标系旋转到关键点的主方向上，由于前面已经计算出了主方向，因此采用如下旋转公式进行计算

其中pi为旋转之前的点，p’‘代表旋转后的点；

采用旋转后的点p’i重新计算前面提到的几个参数，包括d，θ，φ，接下来再计算旋转后的关键点p’与邻域点p’i之间的关系几何关系，该几何关系指的是向量p’p’i与旋转之前的关键点p处的法向量n之间的夹角δ，该夹角的计算采用如下公式，其中分子表示两个向量的点乘，分母表示两个向量的长度，对于一个关键点与邻域中所有的邻居点就会生成三元组数据信息(θ，φ，δ)

对若干个三元组的三个角度进行划分，生成特征描述子。

作为本发明再进一步的方案：步骤3)中，3d-sift关键点周围区域提取ipfh特征的具体步骤为：

i、在3d-sift关键点po(x，y，z，σ，θ，φ)，以该点为中心，确定k邻域，在邻域中点两两互连构建点对，计算每组点对的法向量；

ii、每组点对之间构建局部u-v-w坐标系，三个坐标轴采用如下方式定义：

根据局部坐标系和法向量计算四个特征值，计算方法如下公式所示，四个特征值称作spfh

iii、在两个点云地图中，重新选择k邻域，利用每个三维点临近的spfh合成ipfh，合成方法按照如下公式：

ω0指的是查询点p与每个邻域点距离的平均值，ωi代表了邻域空间中查询点p与一个邻近点pi之间的距离；

iv、统计每个点的f1、f3、f4取值情况，每个值取分为11个区间，ipfh采用33维度的数据信息进行描述。

作为本发明再进一步的方案：步骤4)中，ipfh特征匹配，确定对应关系，判断标准为特征向量之间的距离，采用如下的匹配策略：取点云地图pointsrc一点a的ipfh，在点云地图pointtgt中寻找与a向量距离最近的两个点b和c，通过下面公式计算，如果满足公式条件，则选择b为其匹配点

作为本发明再进一步的方案：步骤5)中，找到特征匹配点之后，采用奇异值分解的方法计算出转换矩阵，根据转换矩阵将点云地图pointsrc进行旋转，从而减小了两个点云地图之间的视角差，完成了视角矫正。

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

1、本发明使用3d-sift提取关键点，并在关键点上计算fpfh特征，最后算出初始粗估计，为传统的点云配准方法提供了一个初始转化矩阵，可以减少配准算法的的迭代次数，提高算法的配准成功率和精度，降低计算成本。

2、本发明将sift特征拓展到三维点云中，提取3d-sift关键点，从而保证特征对视角旋转、变换的鲁棒性；通过提取ipfh特征，克服了原始fpfh特征的权重系数错误的问题，同时该特征综合邻域点的几何特性，来表征一个三维点的特征，大大提高了算法的稳定性。通过这样处理，本发明能够将两个视角差异很大的三维点云地图融合在一起。

附图说明

图1为基于视觉矫正的三维点云地图融合方法的流程示意图。

图2为基于视觉矫正的三维点云地图融合方法中使用3d-sift提取关键点的示意图。

图3为基于视觉矫正的三维点云地图融合方法中使用fpfh进行特征匹配的示意图。

图4为基于视觉矫正的三维点云地图融合方法进行点云融合的效果图。

具体实施方式

下面结合具体实施方式对本发明的技术方案作进一步详细地说明。

请参阅图1，一种基于视觉矫正的三维点云地图融合方法，包括以下步骤：

1)对三维点云地图进行预处理，首先输入两个pcd格式的三维点云地图文件，从而获得有待融合的两个点云地图，这两个点云地图分别为目标点云地图pointtgt和待融合的点云地图pointsrc，然后对这两个点云地图进行预处理，预处理包括去噪和采样；

2)对于步骤1)中的两个点云地图，分别提取目标点云地图pointtgt和待融合的点云地图pointsrc的3d-sift关键点；

3d-sift关键点提取的具体步骤为：

i、点云尺度空间中检测极值点，对点云地图进行不同程度的下采样构建金字塔模型，对每一组点云地图构建高斯尺度空间，所用的函数如下所示，

尺度空间：l(x，y，z，σ)＝g(x，y，z，kσ)*p(x，y，z)

高斯差分函数：

参数k指点金字塔中的组数，σ是尺度大小；

构建高斯差分空间：d(x，y，z，kⁱσ)＝l(x，y，z，kⁱ⁺¹σ)一l(x，y，z，kⁱσ)

构建dog空间后，通过对比的方法寻找极值点：对于某个点，其取值大于或者小于其邻域中所有的点的时候，即看做极值点；

ii、确定关键点方向，计算极值点所在邻域内每个点的方位角和仰角，计算公式如下：

公式中d表示极值点与邻域中心点之间的距离，θ表示方位角，φ表示仰角，x0，y0，z0指的是极值点所在邻域内的中心点；

统计邻域内每个点方位角θ和仰角φ的方向：采用两个直方图来统计每个点两个角度的方向，θ的方向取值范围为[0°，360°]，采用8柱直方图统计方向的情况，每个柱代表45度；而φ的取值为[-90°，90°]，采用4柱直方图来统计方向的情况，每个柱代表45度；

统计后，取两个直方图的峰值作为极值点的方位角和仰角，计算后，每个关键点就包含了x、y、z坐标信息、尺度信息和方向信息即(x，y，z，σ，θ，φ)；

iii、将某个关键点p所在的坐标系旋转到关键点的主方向上，由于前面已经计算出了主方向，因此采用如下旋转公式进行计算

其中pi为旋转之前的点，p’i代表旋转后的点；

采用旋转后的点p’i重新计算前面提到的几个参数，包括d，θ，φ，接下来再计算旋转后的关键点p’与邻域点p’i之间的关系几何关系，该几何关系指的是向量p’p’i与旋转之前的关键点p处的法向量n之间的夹角δ，该夹角的计算采用如下公式，其中分子表示两个向量的点乘，分母表示两个向量的长度，对于一个关键点与邻域中所有的邻居点就会生成三元组数据信息(θ，φ，δ)

对若干个三元组的三个角度进行划分，生成特征描述子；

3)对步骤2)中的两个点云地图3d-sift关键点分别提取ipfh特征，得到待融合的点云地图pointsrc的特征向量p和目标点云地图pointtgt特征向量q，其中特征向量p有m个特征点，特征向量q有n个特征点；

3d-sift关键点周围区域提取ipfh特征的具体步骤为：

i、在3d-sift关键点po(x，y，z，σ，θ，φ)，以该点为中心，确定k邻域，在邻域中点两两互连构建点对，计算每组点对的法向量；

ii、每组点对之间构建局部u-v-w坐标系，三个坐标轴采用如下方式定义：

根据局部坐标系和法向量计算四个特征值，计算方法如下公式所示，四个特征值称作spfh

iii、在两个点云地图中，重新选择k邻域，利用每个三维点临近的spfh合成ipfh，合成方法按照如下公式：

ω0指的是查询点p与每个邻域点距离的平均值，ωi代表了邻域空间中查询点p与一个邻近点pi之间的距离；

iv、统计每个点的f1、f3、f4取值情况，每个值取分为11个区间，ipfh采用33维度的数据信息进行描述；

4)计算特征向量p和特征向量q中特征点的欧氏距离，根据最小距离找到特征匹配点；

ipfh特征匹配，确定对应关系，判断标准为特征向量之间的距离，采用如下的匹配策略：取点云地图pointsrc一点a的ipfh，在点云地图pointtgt中寻找与a向量距离最近的两个点b和c，通过下面公式计算，如果满足公式条件，则选择b为其匹配点

5)通过特征匹配点计算旋转矩阵，利用矩阵将点云地图pointtgt旋转，改变该点云地图的视角，缩小两个点云地图的视角差异；

找到特征匹配点之后，采用奇异值分解的方法计算出转换矩阵，根据转换矩阵将点云地图pointsrc进行旋转，从而减小了两个点云地图之间的视角差，完成了视角矫正；

6)采用icp精确融合算法将缩小视角差异后的两个三维点云地图融合在一起。

在图2中，展示了基于视觉矫正的三维点云地图融合方法中，使用3d-sift提取关键点的效果图。sift特征是图像局部特征，其对旋转、尺度缩放、亮度变化保持不变性、对视角变化、仿射变换、噪声也保持一定程度的稳定性。

在图3中，展示了基于视觉矫正的三维点云地图融合方法中，在3d-sift提取的关键点上进一步提取fpfh特征，并进行特征匹配的示意图。fpfh是对pfh特征的改进，是一种以统计直方图的形式描述一个样本点周围的局部几何特征信息的点特征表示方法，计算出源和目标点云中的特征点的欧式距离，根据最小距离找到特征匹配点，通过特征匹配点计算旋转矩阵。

在图4中，展示了基于视觉矫正的三维点云地图融合方法中的测试与实验结果，本实验采用了三组数据集，其中前两组数据来自于使用xtionpro立体传感器采集的室内点云数据；另一组来自于pcl提供的由3d激光扫描仪采集的室外部分城市点云。图片的第一列和第二列分别代表源和目标点云，第三列和第四列是融合之后的点云，在第四列中用不同的颜色表示了两张点云融合之后的结果。

本发明使用3d-sift提取关键点，并在关键点上计算fpfh特征，最后算出初始粗估计，为传统的点云配准方法提供了一个初始转化矩阵，可以减少配准算法的的迭代次数，提高算法的配准成功率和精度，降低计算成本。

本发明将sift特征拓展到三维点云中，提取3d-sift关键点，从而保证特征对视角旋转、变换的鲁棒性；通过提取ipfh特征，克服了原始fpfh特征的权重系数错误的问题，同时该特征综合邻域点的几何特性，来表征一个三维点的特征，大大提高了算法的稳定性。通过这样处理，本发明能够将两个视角差异很大的三维点云地图融合在一起。

上面对本发明的较佳实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域的普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。