CN110942476A - 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质 - Google Patents

基于二维图像引导的改进三维点云配准方法、系统及可读存储介质 Download PDF

Info

Publication number
CN110942476A
CN110942476A CN201910988369.0A CN201910988369A CN110942476A CN 110942476 A CN110942476 A CN 110942476A CN 201910988369 A CN201910988369 A CN 201910988369A CN 110942476 A CN110942476 A CN 110942476A
Authority
CN
China
Prior art keywords
dimensional
depth image
point
point cloud
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201910988369.0A
Other languages
English (en)
Inventor
余洪山
付强
王佳龙
郭林峰
王静文
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Research Institute Of Hunan University
Hunan University
Original Assignee
Shenzhen Research Institute Of Hunan University
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Research Institute Of Hunan University, Hunan University filed Critical Shenzhen Research Institute Of Hunan University
Priority to CN201910988369.0A priority Critical patent/CN110942476A/zh
Publication of CN110942476A publication Critical patent/CN110942476A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/213—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods
    • G06F18/2135—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods based on approximation criteria, e.g. principal component analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/50—Image enhancement or restoration using two or more images, e.g. averaging or subtraction
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Artificial Intelligence (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质，该方法采用点到面的距离作为目标函数的度量，利用FLANN最近邻算法搜寻方法搜寻最近邻点，先后采用基于彩色图像引导的深度图像滤波方法和基于PCA方法进行降采样，提高了深度点准确度的同时减少了后面迭代过程的计算量，可实现快速的三维点云配准，具有鲁棒性高，计算量小等优点，有效满足了机器人自定位、地图创建等应用领域中对三维点云配准的迫切需求。

Description

基于二维图像引导的改进三维点云配准方法、系统及可读存 储介质
技术领域
本发明属于机器人视觉控制领域，特别涉及一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质。
背景技术
深度图像配准即根据不同视角下场景间的点云重叠约束，计算不同视角对应的旋转平移矩阵，是三维场景构建和机器人三维位姿估计的关键步骤。随着三维激光雷达、立体摄像机、三维相机等深度图像信息获取技术的日益成熟，深度图像配准在三维场景建模、场景识别、机器人导航定位等应用显示出了巨大优势。深度图像配准的准确度和效率直接决定着三维场景构建、三维运动估计、机器人导航定位等任务的性能。
迭代最近点算法(Iterative Closest Point，ICP)作为点云配准中的一种最直接的方法，被广泛应用[1-2]。ICP深度图像匹配算法是基于点对点的配准，可在较小空间变换范围内或基于较好的初始估计下获得较为准确的三维运动估计。然而经典ICP算法主要缺点不足如下：
(1)由于算法没有包含局部形状信息，限制了配准精度，因而每次迭代都要搜索最近点，计算效率低。经典ICP算法通过逐一比较的方式求解最近邻关系，如对于具有M个点的场景数据集，需要在具有N个点的模型数据集中寻找最近邻，逐一比较的方式其时间复杂度为 O(MN)，该过程存在很大的计算量和搜索量[3]。
(2)在寻找匹配对应点时，经典ICP算法是假设欧氏距离最近的点为对应点，这样很可能会产生错误的对应关系，造成后续计算出来的变换矩阵存在很大误差，甚至可能使算法陷入局部最小值[4]。
(3)在连续的深度图像配准过程中，随着传感器获取深度图像数据的噪声数据增加、数据测量精度的下降、以及非重叠匹配区域比例的增长，算法容易陷入局部最优[5]。
发明内容
本发明提出了一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质，其目的在于，克服现有技术中ICP配准计算效率低、精度不高的问题。
一种基于RGB-D相机的三维点云ICP配准改进方法，包括以下步骤：
步骤1：获取相邻帧场景的二维彩色图像It、It+1和对应的深度图像Dt、Dt+1；
利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It和It+1以及与二维彩色图像对应的深度图像Dt和Dt+1；
步骤2：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure BDA0002237429600000021
和
Figure BDA0002237429600000022
以彩色图像对深度图像进行引导滤波，用于深度图像的噪点滤波和深度值平滑。相对于经典的中值滤波，模糊滤波，高斯滤波，双边滤波方法，本发明考虑了彩色图像像素值的连续性，剔除了噪点的同时保留了更多的边信息，获取了更平滑的深度图像。
步骤3：利用二维彩色图像和滤波后的深度图像生成三维点云；
将二维彩色图像It和滤波后的深度图像
Figure BDA0002237429600000023
生成三维点云Pt，将二维彩色图像It和滤波后的深度图像
Figure BDA0002237429600000024
生成三维点云Pt+1；
二维彩色图像I与三维点云D按二维彩色图像像素点一一对应，即二维彩色图像中第u 行、第v列的像素点It(u,v)与三维点云中对应的三维点Dt(u,v)(x,y,z)相对应，三维点Dt(u,v)(x,y,z)是指二维彩色图像中第u行、第v列的像素点对应的深度信息；所述三维点云Dt是指二维彩色图像所有像素点对应的三维点集合。
步骤4：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure BDA0002237429600000027
和
Figure BDA0002237429600000028
计算相邻帧场景的变换矩阵；
步骤5：依据降采样后的三维点云
Figure BDA0002237429600000029
和
Figure BDA00022374296000000210
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准。
进一步地，按照基于彩色图像引导的深度图像滤波方法对相邻时刻获得的深度图像Dt和 Dt+1进行滤波处理：
基于彩色图像引导的深度图像滤波方法的处理表达式为：
Figure BDA0002237429600000025
其中，G(Dt,It)表示以步骤1获取的深度图像Dt，二维彩色图像It作为输入，滤波后的深度图像
Figure BDA0002237429600000026
作为输出；
具体步骤为：
步骤2.1：设均值滤波函数为fmean(x)，计算均值滤波后的二维彩色图像It Mean＝fmean(It)，深度图像Dt Mean＝fmean(Dt)；计算二维彩色图像的关联系数
Figure BDA0002237429600000031
深度图像的关联系数
Figure BDA0002237429600000032
步骤2.2：计算二维彩色图像的方差
Figure BDA0002237429600000033
深度图像的协方差
Figure BDA0002237429600000034
步骤2.3：令
Figure BDA0002237429600000035
ε为正则化系数，取值范围为(0,1)；
步骤2.4：滤波后的深度图像
Figure BDA0002237429600000036
对深度图像Dt+1，二维彩色图像It+1重复步骤2.1-步骤2.4，得到滤波后的深度图像
Figure BDA0002237429600000037
所述二维彩色图像和深度图像均由灰度值矩阵构成，均值滤波方法fmean(x)是公开的、常见的滤波方法，可从公开的软件库OpenCV中获取。
进一步地，所述均值滤波方法的搜索窗口半径rmean＝8。
进一步地，所述步骤2.3中设置ε＝0.04。
考虑了深度图像对应的彩色图像像素值的连续性，构建了深度图像与彩色图像的相关性模型，提高了深度点的准确度的同时保留了图像更多的边信息。
进一步地，对三维点云进行降采样处理，利用降采样后的三维点云计算相邻帧场景的变换矩阵；
首先，根据深度图像中每个点的法向量n与x、y和z轴的夹角，将所有点依据与坐标轴的夹角按照设定的间隔角度划分为43个类，其中三个维度上所含类的个数均为4，取间隔角度为45°；
然后，随机地从每个类中该类中的1％个点，舍去其它点，得到降采样后的深度图像。
进一步地，所述三维点云中各点的法向量的提取过程如下：
令三维点云Pt中的一个点为p＝(xp,yp,zp)T，且以该点为中心，内核半径为r的邻域内的点的数量为N，则点p的邻域内点的集合为{pi(xi,yi,zi)}，i＝{1,2，…,N}；当函数S(n,d)取得最小值时得到的n为点p的法向量n，
Figure BDA0002237429600000038
其中，S表示点p的邻域内点拟合得到的平面，d表示拟合平面S与深度图像中的点p所在坐标系中的原点之间的距离。
在本发明中，实测某一原始点云个数为325200，降采样后点云个数为3252，压缩率为 1％。
进一步地，所述采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准的过程如下：
步骤5.1：从
Figure BDA0002237429600000041
中搜寻与深度图像
Figure BDA0002237429600000042
中每个点的最近邻点，构建{pi}和{qj}之间的映射关系βk，获得所有的匹配点对：
Figure BDA0002237429600000043
其中，{qj}表示
Figure BDA0002237429600000044
中的像素点集合，{pi}表示
Figure BDA0002237429600000045
中的像素点集合；k为迭代次数，k的最大值kmax＝20，βk为经k次迭代后计算出来的映射关系，Tk-1为上一次迭代计算后求得的变换矩阵，令k的初始值取值为1，T0＝(0,0,0,0,0,0)；
步骤5.2：基于表面法向量夹角法进行第一次匹配点对剔除；
遍历映射βk中所有匹配点对，若
Figure BDA0002237429600000046
剔除此匹配点对；
其中，
Figure BDA0002237429600000047
表示像素点pi在深度图像{pi}的法向量，
Figure BDA0002237429600000048
表示像素点qj在深度图像{qj}的法向量，
Figure BDA0002237429600000049
表示法向量
Figure BDA00022374296000000410
和法向量
Figure BDA00022374296000000411
的夹角，θthreshold表示角度阈值，取值为1°；
步骤5.3：基于匹配点对的中值距离进行第二次匹配点剔除；
若
Figure BDA00022374296000000412
剔除此匹配点对；
其中，
Figure BDA00022374296000000413
表示像素点pi和像素点qj之间欧式距离的绝对值，dmid表示遍历映射βk中所有匹配点对后，得到的所有匹配点对距离的中值距离，阈值距离dthreshold等于中值距离dmid；
步骤5.4：经过两次剔除操作后，以所有剩余匹配点对之间的距离和最小为目标函数，计算相邻帧场景的变换矩阵；
Figure BDA00022374296000000414
其中，(pi,q′i)是通过步骤5.2和5.3剔除错误关系匹配点对后，得到剩余匹配点对， i＝(1,...,m)；ωi为权值，
Figure BDA00022374296000000415
Figure BDA00022374296000000416
表示pi与q′i中深度值较大的点所对应的深度值；
步骤5.5：当Tk-Tk-1＞Tthreshold或k>＝kmax满足时，迭代终止，令Tlast＝Tk，得到最终的变换矩阵Tlast，若不满足，令k＝k+1，返回步骤5.1，对最新的深度图像重新构建映射关系，获得新的匹配点对。
进一步地，所述从
Figure BDA0002237429600000051
中搜寻与深度图像
Figure BDA0002237429600000052
中每个点的最近邻点时采用FLANN 搜寻方法。
与经典ICP算法通过逐一比较的方式求解最近邻关系的不同，本发明采用FLANN最近邻算法搜寻方法搜寻最近邻点，提高了搜寻匹配关系的速度，所述FLANN最近邻算法由公知的OpenCV库中获取。
一种基于RGB-D相机的三维点云ICP配准改进方法系统，包括：
图像获取单元：利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It、 It+1和对应的深度图像Dt、Dt+1；
引导滤波单元：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure BDA0002237429600000053
和
Figure BDA0002237429600000054
三维点云生成单元：利用二维彩色图像和滤波后的深度图像生成三维点云；
变换矩阵计算单元：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure BDA0002237429600000055
和
Figure BDA0002237429600000056
计算相邻帧场景的变换矩阵；
相邻场景配准单元：依据降采样后的三维点云
Figure BDA0002237429600000057
和
Figure BDA0002237429600000058
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准。
一种可读存储介质，包括计算机程序，所述计算机程序指令被处理终端执行时使所述处理终端执行上述一种基于RGB-D相机的三维点云ICP配准改进方法。
有益效果
本发明提出了一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质，可实现快速的三维点云配准，具有鲁棒性高，计算量小等优点，有效满足了机器人自定位、地图创建等应用领域中对三维点云配准的迫切需求。
与经典的ICP配准方法相比，本发明的先进性表现在：
1，与传统的深度图像中值滤波，模糊滤波，高斯滤波，双边滤波方法对比，本发明采用彩色图像引导的深度图像滤波方法，该方法充分考虑彩色图像像素值的连续性，构建了彩色图像与深度图像之间的相关性模型，提高了深度点的准确度的同时保留了图像更多的边信息，从而能构建出精度更高的场景三维点云。
2，与经典ICP算法采用点到点的距离作为目标函数度量方法不同，本发明采用点到面的距离作为目标函数的度量，收敛速度和精确度远高于经典ICP算法；
3，与经典ICP算法通过逐一比较的方式求解最近邻关系的不同，本发明采用FLANN最近邻算法搜寻方法搜寻最近邻点，提高了搜寻匹配关系的速度；
4，采用基于PCA方法进行降采样，减少了后面迭代过程的计算量；
5，采用可同时获取二维彩色图像信息和深度信息的市场化传感器，提高了信息获取的便利性，进一步体现了本发明提出的算法的实践性；
6，通过实验本发明提出的整体改进的三维点云ICP配准方案无论从配准精度还是运行速度上都明显优于经典的ICP算法。
附图说明
图1为本发明提出的基于RGB-D相机的三维点云ICP配准改进方法整体流程图；
图2为二维彩色图像和对应的深度图像生成三维点云的示意图，其中(a)为二维彩色图像，(b)深度图像，(c)三维点云图像；
图3为各种传统滤波方法与本发明实例提出的以彩色图像为引导的深度图像滤波方法对比，其中，(a)为原深度图像，(b)为中值滤波，(c)为模糊滤波，(d)为高斯滤波，(e)为双边滤波，(f)为本发明实例所述的方法；
图4为点云图像经过基于彩色图像引导的深度图像滤波处理前后的效果图，以及滤波后的点云降采样的示意图。其中(a)为滤波前，(b)为滤波后，(c)为降采样后的点云图；
图5为误匹配点对关系剔除方法示意图，以及基于点到面距离的目标函数示意图，其中， (a)指基于表面法向量夹角剔除法示意图，(b)指基于对应点间的中值距离剔除法示意图； (c)指基于点到面距离的目标函数示意图；
图6为机器人运行轨迹；
图7为本发明提出的改进的ICP点云配准前后对比效果图，其中(a)为改进前，(b)为改进后；
图8为本发明提出的改进的ICP点云配准方法迭代过程误差衰减曲线图；
图9为本发明改进的ICP配准结果图。
具体实施方式
下面结合附图和实施例对本发明做进一步的说明
本发明在经典ICP算法的基础上，提供一种基于RGB-D相机获取深度图像的ICP配准改进方法，算法提高了计算效率和准确性，可广泛应用于三维场景构建、机器人三维位姿估计等领域[1-2]。
如图1所示，一种基于RGB-D相机获取深度图像的ICP配准改进方法，包括以下步骤：
步骤1：获取相邻帧场景的二维彩色图像It、It+1和对应的深度图像Dt、Dt+1；
利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It和It+1以及与二维彩色图像对应的深度图像Dt和Dt+1；
步骤2：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure BDA0002237429600000071
和
Figure BDA0002237429600000072
如图3，所述以彩色图像为引导的深度图像滤波方法与各种经典滤波方法的对比，其中， (a)为原深度图像，(b)为中值滤波，(c)为模糊滤波，(d)为高斯滤波，(e)为双边滤波， (f)为本发明实例所述的方法；基于彩色图像引导的深度图像滤波方法描述如下：
基于彩色图像引导的深度图像滤波方法的处理表达式为：
Figure BDA0002237429600000073
其中，G(Dt,It)表示以步骤1获取的深度图像Dt，二维彩色图像It作为输入，滤波后的深度图像
Figure BDA0002237429600000074
作为输出；
具体步骤为：
步骤2.1：设均值滤波函数为fmean(x)，计算均值滤波后的二维彩色图像It Mean＝fmean(It)，深度图像Dt Mean＝fmean(Dt)；计算二维彩色图像的关联系数
Figure BDA0002237429600000075
深度图像的关联系数
Figure BDA0002237429600000076
步骤2.2：计算二维彩色图像的方差
Figure BDA0002237429600000077
深度图像的协方差
Figure BDA0002237429600000078
步骤2.3：令
Figure BDA0002237429600000079
ε为正则化系数，取值范围为(0,1)；
步骤2.4：滤波后的深度图像
Figure BDA0002237429600000081
对深度图像Dt+1，二维彩色图像It+1重复步骤2.1-步骤2.4，可得滤波后的深度图像
Figure BDA0002237429600000082
所述彩色图像和深度图像均由灰度值矩阵构成，均值滤波方法fmean(x)是公开的、常见的滤波方法，可从公开的软件库OpenCV中获取，在本发明中，设置均值滤波方法的搜索窗口半径rmean＝8；
所述步骤2.3中ε为正则化系数，在本发明中，设置ε＝0.04；
步骤3：利用彩色图像It和滤波后的深度图像
Figure BDA0002237429600000083
生成三维点云Pt，利用彩色图像It和滤波后的深度图像
Figure BDA0002237429600000084
生成三维点云Pt+1；
对于相机视角内的任意空间点P(X,Y,Z)，其对应的彩色图像中的像素坐标p(u,v)及深度图像中的深度数据d与之存在如下关系：
Figure BDA0002237429600000085
其中，fx和fy分别表示相机在x、y轴上的焦距，S表示深度图的缩放因子，一般取5000。上式可进一步转化为如下形式：
Figure BDA0002237429600000086
其中，fx、fy、cx、cy又被称为相机内参数，根据Kinect的内参数以及彩色和深度数据就可求得每个像素点对应的空间三维坐标，即形成三维点云。因此，根据公知的RGB-D相机投影模型以及该相机的内参矩阵，可以配准彩色图像和对应的深度图像生成三维点云，如图 2所示。
步骤4：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure BDA0002237429600000087
和
Figure BDA0002237429600000088
计算相邻帧场景的变换矩阵；
首先，根据深度图像中三维点云Pt和Pt+1每个点的法向量n与x、y和z轴的夹角，将所有点依据与坐标轴的夹角按照设定的间隔角度划分为43个类，其中三个维度上所含类的个数均为4，取间隔角度为45°；
然后，随机地从每个类中该类中的1％个点，舍去其它点，得到降采样后的三维点云
Figure BDA0002237429600000091
和
Figure BDA0002237429600000092
对点云进行降采样后，将会大量降低ICP算法中寻找对应点时的时间复杂度，实验证明将点云数量降采样至3000左右(原始点云的1％)时，计算效率与准确性的综合性能将达到一个最佳状态，采样前后的点云如图4所示；
实测某一原始点云个数为325200，降采样后点云个数为3252，压缩率为1％。
所述三维点云中各点的法向量的提取过程如下：
法向量作为点云的一种重要的几何信息，被广泛应用于点云配准中，本文改进的ICP算法中也会多次用到法向量信息。由于点云数据对于真实拍摄的物体而言表现为一组离散样本，因而可以直接从数据集中构造出表面法向量，即通过局部表面拟合的方法估计法向量，这实质是一个利用最小二乘法拟合平面的问题，求取的平面法向量即为点云数据的法向量本文利用主成分分析法(Principal Component Analysis，PCA)估计点云法向量，它是一种应用广泛的代数特征提取方法，是通过将具有一定相关性的变量通过线性变换选出较少的重要变量的一种统计学习方法。
步骤5：依据降采样后的三维点云
Figure BDA0002237429600000093
和
Figure BDA0002237429600000094
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，如图5所示，获得相邻帧场景的变换矩阵，进行相邻帧场景配准；
步骤5.1：从
Figure BDA0002237429600000095
中搜寻与深度图像
Figure BDA0002237429600000096
中每个点的最近邻点，构建{pi}和{qj}之间的映射关系βk，获得所有的匹配点对：
Figure BDA0002237429600000097
其中，k为迭代次数，k的最大值kmax＝20，βk为经k次迭代后计算出来的映射关系，Tk-1为上一次迭代计算后求得的变换矩阵，令k的初始值取值为1，T0＝(0,0,0,0,0,0)；
步骤5.2：基于表面法向量夹角法进行第一次匹配点对剔除；
遍历映射βk中所有匹配点对，若
Figure BDA0002237429600000098
剔除此匹配点对；
其中，
Figure BDA0002237429600000099
表示点pi在深度图像{pi}的法向量，
Figure BDA00022374296000000910
表示点qj在深度图像{qj}的法向量，
Figure BDA0002237429600000101
表示法向量
Figure BDA0002237429600000102
和法向量
Figure BDA0002237429600000103
的夹角，θthreshold表示角度阈值，取值为1°；
步骤5.3：基于匹配点对的中值距离进行第二次匹配点剔除；
若
Figure BDA0002237429600000104
剔除此匹配点对；
其中，
Figure BDA0002237429600000105
表示点pi和点qj之间欧式距离的绝对值，dmid表示遍历映射βk中所有匹配点对后，得到的所有匹配点对距离的中值距离，阈值距离dthreshold等于中值距离dmid；
步骤5.4：经过两次剔除操作后，以所有剩余匹配点对之间的距离和最小为目标函数，计算相邻帧场景的变换矩阵；
Figure BDA0002237429600000106
其中，
Figure BDA0002237429600000107
是通过步骤5.2和5.3剔除错误关系匹配点对后，得到剩余匹配点对，i＝(1,...,m)；ωi为权值，
Figure BDA0002237429600000108
Figure BDA0002237429600000109
表示pi与
Figure BDA00022374296000001010
中深度值较大的点所对应的深度值；
步骤5.5：当Tk-Tk-1＞Tthreshold或k>＝kmax满足时，迭代终止，令Tlast＝Tk，得到最终的变换矩阵Tlast，若不满足，令k＝k+1，返回步骤5.1，对最新的深度图像重新构建映射关系，获得新的匹配点对。
所述从
Figure BDA00022374296000001011
中搜寻与深度图像
Figure BDA00022374296000001012
中每个点的最近邻点时采用FLANN搜寻方法。与经典ICP算法通过逐一比较的方式求解最近邻关系的不同，本发明采用FLANN最近邻算法搜寻方法搜寻最近邻点，提高了搜寻匹配关系的速度。
一种基于RGB-D相机的三维点云ICP配准改进系统，包括：
图像获取单元：利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It、 It+1和对应的深度图像Dt、Dt+1；
引导滤波单元：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure BDA00022374296000001013
和
Figure BDA00022374296000001014
三维点云生成单元：利用二维彩色图像和滤波后的深度图像生成三维点云；
变换矩阵计算单元：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure BDA00022374296000001015
和
Figure BDA00022374296000001016
计算相邻帧场景的变换矩阵；
相邻场景配准单元：依据降采样后的三维点云
Figure BDA0002237429600000111
和
Figure BDA0002237429600000112
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准。
应当理解，本发明各个实施例中的功能单元模块可以集中在一个处理单元中，也可以是各个单元模块单独物理存在，也可以是两个或两个以上的单元模块集成在一个单元模块中，可以采用硬件或软件的形式来实现。
本发明的实例还提供一种可读存储介质，包括计算机程序，所述计算机程序指令被处理终端执行时使所述处理终端执行一种基于RGB-D相机的三维点云ICP配准改进方法，其有益效果参见方法部分的有益效果，在此不再赘述。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/ 或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/ 或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
机器人运行轨迹以及起始位置处的彩色点云如图6所示，实验共采集到70帧图像，采用深度数据进行试验。
图7显示了其中两帧点云配准前后状态的对比，由图可知，配准后两帧点云得到较为完整的融合(从椭圆处可明显观察到)；
图8显示了这两组点云在20次迭代过程中误差的衰减曲线，由图可知，误差在前几次迭代过程中衰减较快，经过后面若干次迭代后缓慢地趋于稳定，且最终误差值降低到0.011；
图9显示了本发明实例改进的ICP算法对18帧点云的配准结果，由图9可知，本发明的方法在不加初值的情况下仍然能够得到较为准确的结果。
分别采用本发明提出的一种基于RGB-D相机的三维点云ICP配准改进方法和经典ICP 算法对场景中70帧点云数据进行配准，计算两种算法所对应的绝对误差的均方根以及平均配准时间，其结果对比如表1所示，其中，配准误差指对应的绝对误差的均方根。由表1可知，本发明提出的改进ICP算法无论从配准精度还是运行速度上都明显优于经典的ICP算法。
表1配准精度和速度对比
Figure BDA0002237429600000121
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。
参考文献：
[1]A.Petitti,D.Di Paola,R.Colella,et al.,“A Distributed Map BuildingApproach for Mobile Robotic Networks,”IEEE 14th International Conference onAutomation Science and Engineering (CASE).IEEE,2018,pp:116-121.
[2]H.Men,B.Gebre,K.Pochiraju,“Color point cloud registration with 4DICP algorithm,”2011 IEEE International Conference on Robotics and Automation,2011,pp:1511-1516.
[3]S.Cheng,I.Marras,S.Zafeiriou,et al.,“Statistical non-rigid ICPalgorithm and its application to 3D face alignment,”Image and VisionComputing,2017,58,pp:3-12.
[4]黄高锋,陈义,符宏伟.基于特征点匹配及提纯的点云配准算法，测绘与空间地理信息，2019， 42(02)，pp:199-202.
[5]G.Yang,R.Zeng,A.Dong,et al.,“Research and Application of 3D FaceModeling Algorithm Based on ICP Accurate Alignment,”Journal of Physics:Conference Series.IOP Publishing,2018, 1069(1),pp:012149。

Claims (10)

1.一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质，其特征在于，包括以下步骤：
步骤1：获取相邻帧场景的二维彩色图像It、It+1和对应的深度图像Dt、Dt+1；
利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It和It+1以及与二维彩色图像对应的深度图像Dt和Dt+1；
步骤2：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure FDA0002237429590000011
和
Figure FDA0002237429590000012
步骤3：利用二维彩色图像和滤波后的深度图像生成三维点云；
将二维彩色图像It和滤波后的深度图像
Figure FDA0002237429590000013
生成三维点云Pt，将二维彩色图像It和滤波后的深度图像
Figure FDA0002237429590000014
生成三维点云Pt+1；
步骤4：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure FDA0002237429590000015
和
Figure FDA0002237429590000016
计算相邻帧场景的变换矩阵；
步骤5：依据降采样后的的三维点云
Figure FDA0002237429590000017
和
Figure FDA0002237429590000018
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准。
2.根据权利要求1所述的方法，其特征在于，按照基于彩色图像引导的深度图像滤波方法对相邻时刻获得的深度图像Dt和Dt+1进行滤波处理：
基于彩色图像引导的深度图像滤波方法的处理表达式为：
Figure FDA0002237429590000019
其中，G(Dt,It)表示以步骤1获取的深度图像Dt，二维彩色图像It作为输入，滤波后的深度图像
Figure FDA00022374295900000110
作为输出；
具体步骤为：
步骤2.1：设均值滤波函数为fmean(x)，计算均值滤波后的二维彩色图像It Mean＝fmean(It)，深度图像Dt Mean＝fmean(Dt)；计算二维彩色图像的关联系数
Figure FDA00022374295900000111
深度图像的关联系数
Figure FDA00022374295900000112
步骤2.2：计算二维彩色图像的方差
Figure FDA00022374295900000113
深度图像的协方差
Figure FDA0002237429590000021
步骤2.3：令
Figure FDA0002237429590000022
β＝Dt Mean-α*It Mean，ε为正则化系数，取值范围为(0,1)；
步骤2.4：滤波后的深度图像
Figure FDA0002237429590000023
对深度图像Dt+1，二维彩色图像It+1重复步骤2.1-步骤2.4，得到滤波后的深度图像
Figure FDA0002237429590000024
3.根据权利要求2所述的方法，其特征在于，所述均值滤波函数fmean(x)的搜索窗口半径rmean＝8。
4.根据权利要求2所述的方法，其特征在于，所述步骤2.3中设置ε＝0.04。
5.根据权利要求1所述的方法，其特征在于，对三维点云进行降采样处理，利用降采样后的三维点云计算相邻帧场景的变换矩阵；
首先，根据深度图像中每个点的法向量n与x、y和z轴的夹角，将所有点依据与坐标轴的夹角按照设定的间隔角度划分为43个类，其中三个维度上所含类的个数均为4，取间隔角度为45°；
然后，随机地从每个类中该类中的1％个点，舍去其它点，得到降采样后的深度图像。
6.根据权利要求1-5任一项所述的方法，其特征在于，所述三维点云中各点的法向量的提取过程如下：
令三维点云Pt中的一个点为p＝(xp,yp,zp)T，且以该点为中心，内核半径为r的邻域内的点的数量为N，则点p的邻域内点的集合为{pi(xi,yi,zi)}，i＝{1,2，...,N}；当函数S(n,d)取得最小值时得到的n为点p的法向量n，
Figure FDA0002237429590000025
其中，S表示点p的邻域内点拟合得到的平面，d表示拟合平面S与深度图像中的点p所在坐标系中的原点之间的距离。
7.根据权利要求1所述的方法，其特征在于，所述采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准的过程如下：
步骤5.1：从
Figure FDA0002237429590000026
中搜寻与深度图像
Figure FDA0002237429590000027
中每个点的最近邻点，构建{pi}和{qj}之间的映射关系βk，获得所有的匹配点对：
Figure FDA0002237429590000031
其中，{qj}表示
Figure FDA0002237429590000032
中的像素点集合，{pi}表示
Figure FDA0002237429590000033
中的像素点集合；k为迭代次数，k的最大值kmax＝20，βk为经k次迭代后计算出来的映射关系，Tk-1为上一次迭代计算后求得的变换矩阵，令k的初始值取值为1，T0＝(0,0,0,0,0,0)；
步骤5.2：基于表面法向量夹角法进行第一次匹配点对剔除；
遍历映射βk中所有匹配点对，若
Figure FDA0002237429590000034
剔除此匹配点对；
其中，
Figure FDA0002237429590000035
表示像素点pi在深度图像{pi}的法向量，
Figure FDA0002237429590000036
表示像素点qj在深度图像{qj}的法向量，
Figure FDA0002237429590000037
表示法向量
Figure FDA0002237429590000038
和法向量
Figure FDA0002237429590000039
的夹角，θthreshold表示角度阈值，取值为1°；
步骤5.3：基于匹配点对的中值距离进行第二次匹配点剔除；
若
Figure FDA00022374295900000310
剔除此匹配点对；
其中，
Figure FDA00022374295900000311
表示像素点pi和像素点qj之间欧式距离的绝对值，dmid表示遍历映射βk中所有匹配点对后，得到的所有匹配点对距离的中值距离，阈值距离dthreshold等于中值距离dmid；
步骤5.4：经过两次剔除操作后，以所有剩余匹配点对之间的距离和最小为目标函数，计算相邻帧场景的变换矩阵；
Figure FDA00022374295900000312
其中，(pi,qi′)是通过步骤5.2和5.3剔除错误关系匹配点对后，得到剩余匹配点对，i＝(1,...,m)；ωi为权值，
Figure FDA00022374295900000313
Figure FDA00022374295900000314
表示pi与qi′中深度值较大的点所对应的深度值；
步骤5.5：当Tk-Tk-1＞Tthreshold或k>＝kmax满足时，迭代终止，令Tlast＝Tk，得到最终的变换矩阵Tlast，若不满足，令k＝k+1，返回步骤5.1，对最新的深度图像重新构建映射关系，获得新的匹配点对。
8.根据权利要求7所述的方法，其特征在于，所述从
Figure FDA00022374295900000315
中搜寻与深度图像
Figure FDA00022374295900000316
中每个点的最近邻点时采用FLANN搜寻方法。
9.一种基于二维图像引导的改进三维点云配准方法、系统及可读存储介质，其特征在于，包括：
图像获取单元：利用RGB-D相机在t时刻和t+1时刻获取相邻帧场景的二维彩色图像It、It+1和对应的深度图像Dt、Dt+1；
引导滤波单元：对深度图像Dt和Dt+1进行以彩色图像为引导的深度图像滤波得到滤波后的深度图像
Figure FDA0002237429590000041
和
Figure FDA0002237429590000042
三维点云生成单元：利用二维彩色图像和滤波后的深度图像生成三维点云；
变换矩阵计算单元：采用基于PCA方法对三维点云Pt和Pt+1进行降采样，利用降采样后的三维点云
Figure FDA0002237429590000043
和
Figure FDA0002237429590000044
计算相邻帧场景的变换矩阵；
相邻场景配准单元：依据降采样后的三维点云
Figure FDA0002237429590000045
和
Figure FDA0002237429590000046
中对应点的映射关系，依次采用基于对应点间的中值距离的剔除法和基于表面法向量夹角的剔除法，剔除错误关系匹配点对，以剩余匹配点对之间的距离和最小为目标函数，获得相邻帧场景的变换矩阵，进行相邻帧场景配准。
10.一种可读存储介质，包括计算机程序，其特征在于，所述计算机程序指令被处理终端执行时使所述处理终端执行权利要求1至8任一项所述的方法。
CN201910988369.0A 2019-10-17 2019-10-17 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质 Pending CN110942476A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910988369.0A CN110942476A (zh) 2019-10-17 2019-10-17 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910988369.0A CN110942476A (zh) 2019-10-17 2019-10-17 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质

Publications (1)

Publication Number Publication Date
CN110942476A true CN110942476A (zh) 2020-03-31

Family

ID=69906067

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910988369.0A Pending CN110942476A (zh) 2019-10-17 2019-10-17 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质

Country Status (1)

Country Link
CN (1) CN110942476A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111524115A (zh) * 2020-04-17 2020-08-11 湖南视比特机器人有限公司 钢板切割件的定位方法和分拣系统
CN111612841A (zh) * 2020-06-22 2020-09-01 上海木木聚枞机器人科技有限公司 目标定位方法及装置、移动机器人及可读存储介质
CN112052847A (zh) * 2020-08-17 2020-12-08 腾讯科技（深圳）有限公司 图像处理方法、装置、电子设备和计算机可读存储介质
CN112132971A (zh) * 2020-09-08 2020-12-25 合肥的卢深视科技有限公司 三维人体建模方法、装置、电子设备和存储介质
CN113256611A (zh) * 2021-06-21 2021-08-13 浙江光珀智能科技有限公司 一种rgb-d配准精度测试方法及设备
CN113689471A (zh) * 2021-09-09 2021-11-23 中国联合网络通信集团有限公司 目标跟踪方法、装置、计算机设备及存储介质
CN113793370A (zh) * 2021-01-13 2021-12-14 北京京东叁佰陆拾度电子商务有限公司 三维点云配准方法、装置、电子设备及可读介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108805841A (zh) * 2018-06-12 2018-11-13 西安交通大学 一种基于彩色图引导的深度图恢复及视点合成优化方法
CN110223348A (zh) * 2019-02-25 2019-09-10 湖南大学 基于rgb-d相机的机器人场景自适应位姿估计方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108805841A (zh) * 2018-06-12 2018-11-13 西安交通大学 一种基于彩色图引导的深度图恢复及视点合成优化方法
CN110223348A (zh) * 2019-02-25 2019-09-10 湖南大学 基于rgb-d相机的机器人场景自适应位姿估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
KAIMING HE,ET AL.: "Guided Image Filtering" *
孟恬等: "基于快速行进算法的深度图像增强算法" *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111524115A (zh) * 2020-04-17 2020-08-11 湖南视比特机器人有限公司 钢板切割件的定位方法和分拣系统
CN111524115B (zh) * 2020-04-17 2023-10-13 湖南视比特机器人有限公司 钢板切割件的定位方法和分拣系统
CN111612841B (zh) * 2020-06-22 2023-07-14 上海木木聚枞机器人科技有限公司 目标定位方法及装置、移动机器人及可读存储介质
CN111612841A (zh) * 2020-06-22 2020-09-01 上海木木聚枞机器人科技有限公司 目标定位方法及装置、移动机器人及可读存储介质
CN112052847A (zh) * 2020-08-17 2020-12-08 腾讯科技（深圳）有限公司 图像处理方法、装置、电子设备和计算机可读存储介质
CN112052847B (zh) * 2020-08-17 2024-03-26 腾讯科技（深圳）有限公司 图像处理方法、装置、电子设备和计算机可读存储介质
CN112132971A (zh) * 2020-09-08 2020-12-25 合肥的卢深视科技有限公司 三维人体建模方法、装置、电子设备和存储介质
CN112132971B (zh) * 2020-09-08 2024-04-23 合肥的卢深视科技有限公司 三维人体建模方法、装置、电子设备和存储介质
CN113793370A (zh) * 2021-01-13 2021-12-14 北京京东叁佰陆拾度电子商务有限公司 三维点云配准方法、装置、电子设备及可读介质
CN113793370B (zh) * 2021-01-13 2024-04-19 北京京东叁佰陆拾度电子商务有限公司 三维点云配准方法、装置、电子设备及可读介质
CN113256611A (zh) * 2021-06-21 2021-08-13 浙江光珀智能科技有限公司 一种rgb-d配准精度测试方法及设备
CN113689471B (zh) * 2021-09-09 2023-08-18 中国联合网络通信集团有限公司 目标跟踪方法、装置、计算机设备及存储介质
CN113689471A (zh) * 2021-09-09 2021-11-23 中国联合网络通信集团有限公司 目标跟踪方法、装置、计算机设备及存储介质

Similar Documents

Publication Publication Date Title
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN110942476A (zh) 基于二维图像引导的改进三维点云配准方法、系统及可读存储介质
CN109544636B (zh) 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN108537848B (zh) 一种面向室内场景重建的两级位姿优化估计方法
CN108229416B (zh) 基于语义分割技术的机器人slam方法
Poursaeed et al. Deep fundamental matrix estimation without correspondences
CN111028292B (zh) 一种亚像素级图像匹配导航定位方法
CN112562081B (zh) 一种用于视觉分层定位的视觉地图构建方法
CN108648216B (zh) 一种基于光流与深度学习的视觉里程计实现方法和系统
CN113838191A (zh) 一种基于注意力机制和单目多视角的三维重建方法
CN112329662B (zh) 基于无监督学习的多视角显著性估计方法
CN111798453A (zh) 用于无人驾驶辅助定位的点云配准方法及其系统
CN108469729B (zh) 一种基于rgb-d信息的人体目标识别与跟随方法
Mittal et al. Generalized projection based m-estimator: Theory and applications
CN113592900A (zh) 一种基于注意力机制与全局推理的目标跟踪方法及系统
Yu et al. Recursive three-dimensional model reconstruction based on Kalman filtering
CN116429087A (zh) 一种适应于动态环境的视觉slam方法
CN114913289B (zh) 一种生产车间三维动态不确定性语义slam方法
CN115330874B (zh) 基于超像素处理遮挡的单目深度估计方法
Zhang et al. Dyna-depthformer: Multi-frame transformer for self-supervised depth estimation in dynamic scenes
Uke et al. Efficient method for detecting and tracking moving objects in video
CN115496859A (zh) 基于散乱点云交叉注意学习的三维场景运动趋势估计方法
Lin et al. 6D object pose estimation with pairwise compatible geometric features
Ji et al. View-Centric Multi-Object Tracking with Homographic Matching in Moving UAV

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20200331

WD01 Invention patent application deemed withdrawn after publication