CN109934859B - 一种基于特征增强多维权重描述子的icp配准方法 - Google Patents
一种基于特征增强多维权重描述子的icp配准方法 Download PDFInfo
- Publication number
- CN109934859B CN109934859B CN201910201443.XA CN201910201443A CN109934859B CN 109934859 B CN109934859 B CN 109934859B CN 201910201443 A CN201910201443 A CN 201910201443A CN 109934859 B CN109934859 B CN 109934859B
- Authority
- CN
- China
- Prior art keywords
- point
- point cloud
- feature
- registration
- sampling
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000009616 inductively coupled plasma Methods 0.000 title abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 49
- 230000009466 transformation Effects 0.000 claims abstract description 37
- 230000008569 process Effects 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 238000012216 screening Methods 0.000 claims abstract description 8
- 238000005070 sampling Methods 0.000 claims description 101
- 239000013598 vector Substances 0.000 claims description 45
- 230000005484 gravity Effects 0.000 claims description 10
- 230000001131 transforming effect Effects 0.000 claims description 8
- 230000001154 acute effect Effects 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- -1 Q 1 Chemical compound 0.000 claims description 3
- 238000006386 neutralization reaction Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 description 9
- 230000008447 perception Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Landscapes
- Image Analysis (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本发明公开了一种基于特征增强多维权重描述子的ICP配准方法，所述方法包括以下步骤：S1、通过滤波器对3D相机获取两副点云进行滤波处理，然后提取点云中的特征增强多维权重描述子；S2、将提取的特征增强多维权重描述子分成两个特征点对集，并从中找出具有相同多维权重描述子的点对构成两个点对集，然后从两个点对集中取出两对具有相同多维权重描述子的对应点对进行点云粗配准，得到点云粗配准的变换矩阵；S3、计算经过变换矩阵变换后的变换点云与另一幅点云之间的最近邻点，并构成最近邻点对，通过建立约束条件筛除粗配准过程中的误配准对，最后采用ICP迭代法实现场景点云的精配准。本发明有效克服了噪声干扰，使得点云拼接的精度更高，且实施性更强。
Description
技术领域
本发明涉及到自主移动机器人基于三维视觉的环境感知及自动驾驶技术领域，尤其涉及一种基于特征增强多维权重描述子的ICP配准方法。
背景技术
随着激光扫描和自主移动机器人技术的飞速发展，集视觉传感器、控制技术、智能算法等于一体的智能移动机器人技术吸引了众多学者和工程技术人员的关注。移动机器人的同步定位与构图在其自主导航中发挥着重要作用。应用在移动机器人中的环境感知类传感器主要有激光传感器和视觉传感器，相比激光传感器，三维视觉传感器可直接提供环境的三维环境点云，但要获得大场景的三维点云，就要让激光扫描相机在不同时间、不同地方获取多幅扫描点云，因此点云配准过程成为移动机器人环境感知以及自动驾驶的重要部分。
现在大部分的三维激光扫描仪在静态情况下获取不同时刻、不同方位的环境点云，然而在自主移动机器人移动的过程中，获取的点云会有很大的噪声，噪声的存在使得环境点云的配准效果很差。现在的三维大场景重建过程中存在一些难以克服的问题，在配准过程中很难找到两幅场景点云的共同特征、重建的场景中包含形状特征不规则的区域或动态的部分以及机器人运动过程中产生的噪声，这些因素的存在都使得点云配准的鲁棒性较差且精确性不高，使得自主移动机器人在环境感知以及路径规划导航时差生偏差，造成其性能的损失。
如今对于点云配准大多是围绕ICP(Iterative Closest Point，即迭代最近点)配准方法进行的，但传统的ICP配准在面临点云密度变化、大噪声、遮挡存在时，无法得到良好的配准效果；且ICP算法在两点云相差较大的时候，极易陷入局部最优的情况，使得在大噪声以及点云密度变化大的场景点云配准中无法取得良好的配准效果。
鉴于此，如何克服场景点云配准过程中，由于噪声致使配准效果差，从而影响移动机器人的环境感知与定位导航效果的问题是本技术领域技术人员亟待解决的技术问题。
发明内容
本发明的目的是提供一种基于特征增强多维权重描述子的ICP配准方法，所述配准方法能够有效克服大场景点云拼接过程中噪声对特征点提取的干扰，使得点云配准的精度和效率更高。
为解决上述技术问题，本发明提供一种基于特征增强多维权重描述子的ICP配准方法，所述方法包括以下步骤：
S1、通过自主移动机器人上的3D轮廓扫描传感器获取两副不同视角下的点云X和点云Y，并利用滤波器分别对点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1，然后分别从所述采样点云X1和采样点云Y1中随机取出多个点对进行特征描述子的提取，得到对应每个点对的特征增强的多维权重描述子。
S2、将所述步骤S1中得到的对应每个点对的特征增强的多维权重描述子分成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0，然后分别从特征点对集x0和特征点对集y0中找出具有相同多维权重描述子的点对构成点对集x1和点对集y1，并分别从点对集x1和点对集y1中各随机取出一对具有相同多维权重描述子的对应点对进行点云粗配准，得到点云粗配准的变换矩阵。
S3、通过所述点云粗配准的变换矩阵对采样点云X1进行变换，得到变换点云Q，计算变换点云Q与待配准采样点云Y1之间的最近邻点构成最近邻点对，然后建立约束条件并筛选出最近邻点对中符合约束条件的点对，从而得到配准点对，最后采用ICP迭代法实现场景点云的精配准。
优选地，所述步骤S1具体实现方法包括：
S10、通过自主移动机器人上的3D轮廓扫描传感器获取所述机器人移动过程中两幅不同视角下的点云X和点云Y，并采用滤波器分别对所述点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1；
S11、从所述采样点云X1中的随机抽取两个采样点p1和p2，组成一个采样点对(p1,p2)，并计算采样点p1和p2之间的欧式距离f1(p1,p2)，可用公式表示：
f1(p1,p2)＝||p1-p2|| (1)
S12、利用K近邻方法搜索采样点p1周围最近的K个点组成协方差矩阵G1，可用公式表示：
式(2)中，xi(i＝1,2,3,…K)表示采样点p1周围K个点的坐标，
表示采样点p1周围K个点的重心坐标，其中
S13、对所述步骤S12中的协方差矩阵G1进行特征值分解，得到协方差矩阵G1的特征值λj及其对应的特征向量vj(j＝1,2,3)，从而求得采样点p1的切平面和法向量n1，可用公式表示：
G1*vj＝λj*vj (3)
由式(3)和式(4)可筛选出采样点p1的法向量n1，即vj或-vj中与采样点p1成锐角关系的一个特征向量；
S14、利用K近邻方法搜索采样点p2周围最近的K个点并构造协方差矩阵G′1，可用公式表示：
式(5)中，x′i(i＝1,2,3,…K)表示采样点p2周围K个点的坐标，
表示采样点p2周围K个点的重心坐标，其中
S15、对所述步骤S14中的协方差矩阵G′1进行特征值分解，得到协方差矩阵G′1的特征值λ′j以及对应的特征向量v′j(j＝1,2,3)，即可得到采样点p2的切平面和法向量n2，可用公式表示：
G′1*v′j＝λ′j*v′j (6)
由式(6)和式(7)可得到采样点p2的法向量n2，即v′j或-v′j中与采样点p2成锐角关系的一个特征向量；
S16、根据所述步骤S13得到的法向量n1以及所述步骤S15得到的法向量n2，计算采样点对p1和采样点p2之间的最大表面角f2(p1,n1,p2,n2)，可用公式表示：
f2(p1,n1,p2,n2)＝max(β1,β2) (8)
式(8)中，β1表示采样点p1的表面角，
β2表示采样点p2的表面角，
其中d表示采样点p1和采样点p2之间的特征向量差，d＝p2-p1；
S17、根据所述步骤S13筛选的法向量n1和所述步骤S15筛选的法向量n2，计算出采样点p1和采样点p2之间的法向量距离f3(n1,n2)，可用公式表示：
f3(n1,n2)＝cos-1(|n1·n2|) (9)；
S18、对所述步骤S11中的欧式距离f1(p1,p2)、所述步骤S16中的最大表面角f2(p1,n1,p2,n2)和所述步骤S17中的法向量距离f3(n1,n2)进行加权处理，得到特征增强的多维权重描述子H(f1,f2,f3)，可用公式表示：
H(f1,f2,f3)＝Q1·f1(p1,p2)+Q2·f2(p1,n1,p2,n2)+Q3·f3(n1,n2) (10)
式(10)中，Q1、Q2和Q3分别表示f1(p1,p2)、f2(p1,n1,p2,n2)和f3(n1,n2)的权重值，且满足条件Q1＞2Q2＞2Q3；
S19、重复所述步骤S11～S18，随机从所述采样点云X1和采样点云Y1抽取采样点对，直至得到与所述采样点云X1和采样点云Y1相对应的S个采样点对的特征增强的多维权重描述子H(f1,f2,f3)。
优选地，所述滤波器为VoxelGrid滤波器，滤波时采用的体素大小为0.01m。
优选地，所述点云X和点云Y的重叠度大于20％。
优选地，所述步骤S18中的Q1∈[0.5,0.7]，Q2∈[0.25,0.3]，Q3∈[0.15,0.25]。
优选地，所述步骤S19中的采样点对的数量S＝1000。
优选地，所述步骤S2具体实现方法包括：
S21、将所述步骤S1中得到的S个特征增强的多维权重描述子H(f1,f2,f3)组成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0；
S22、从所述步骤S21中的特征点对集x0和特征点对集y0中找出具有相同多维权重描述子的点对，并构成点对集x1，以及从特征点对集y0和特征点对集x0中找出具有相同多维权重描述子的点对构成点对集y1，其中具有相同多维权重描述子的点对应满足公式：
式(11)中，δ≈0.0001m，r表示多维权重描述子的序号，Hx0r和Hy0r分别表示特征点对集x0中与特征点集y0中序号为r的多维权重描述子；
S23、从所述步骤S22的点对集x1和点对集y1中各取出一对具有相同多维权重描述子的对应点对，并进行点云粗配准，获得四组点云粗配准的刚体变换矩阵；
S24、将所述步骤S23中的四组刚体变换矩阵应用于采样点云X1，获得四组对应的变换点云Q′，然后计算所述变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H，并选取使距离误差阈值H最小的刚体变换矩阵作为点云粗配准的变换矩阵(R1,T1)，其中变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H可用公式表示：
式(12)中，N表示具有相同多维权重描述子的对应点对的个数，L表示变换点云Q′中空间点M1(m1,g1,q1)与待配准采样点云Y1中对应的空间点M2(m2,g2,q2)之间的欧式距离，其中，
优选地，所述步骤S3具体实现方法包括：
S31、通过所述点云粗配准的变换矩阵(R1,T1)对采样点云X1进行变换，得到采样点云X1的变换点云Q，并利用kd tree方法计算出所述变换点云Q与待配准采样点云Y1之间的最近邻点，得到对应的最近邻点对(Qs,Y1s)(其中，s表示最近邻点对的序号，s＝1,2,3,…S，Qs和Y1s表示最近邻点对的三维空间坐标)；
S32、计算所述步骤S31中最近邻点对(Qs,Y1s)中点对的重心，可用公式表示：
S33、建立约束条件，并筛选出最近邻点对(Qs,Y1s)中符合约束条件的点对，从而得到配准点对(Q3t,Y2t)(其中，t表示配准点对的序号，t＝1,2,3,…M，M表示配准点对的数量，Q3t和Y2t表示配准点对的三维空间坐标)，其约束条件可用公式表示：
式(15)中，ξ取值为0.01mm～0.05mm；
S34、应用SVD方法计算出所述步骤S33中配准点对(Q3t,Y2t)之间的刚体变换矩阵(Rmk,Tmk)，则可求得变换的点云Q3t(k+1)＝Rmk·Q3tk+Tmk；
S35、计算所述步骤S34中的Q3t(k+1)与Y2t对应点对之间的距离平均值d′，可用公式表示：
S36、设置点云ICP迭代停止条件β，并比较△d′k与β之间的大小，若△d′k＞β，则重复所述步骤S34和所述步骤S35；若△d′k＜β，则停止ICP迭代，从而得到点云配准的最优刚体变换矩阵，完成点云的精配准，其中△d′k＝d′k+1-d′k，k为ICP迭代更新过程中的重复次数，且Q3t0＝Q3t。
与现有技术比较，本发明具有以下有益技术效果：
本发明通过提取基于特征增强的多维权重描述子，并以此用于点云的粗配准，该多维权重描述子能够很好的描述点云的表面特征，且对噪声具有良好的鲁棒性，能有效提高粗配准的精确性和实时性；然后利用配准对的重心作为约束条件，筛除粗配准过程中的误配准对，进一步提高了配准的精确性和实时性。本发明采用多维权重描述子结合特征点对的空间几何信息，能够有效克服大场景点云拼接过程中噪声对特征点提取的干扰，使得点云配准的精度更高；同时也减小了计算机的计算量，提升了配准的效率，使得场景点云拼接的实时性更强。
附图说明
图1为本发明中基于特征增强多维权重描述子的ICP配准方法的流程图；
图2为本发明中提取特征增强的多维权重描述子的方法流程图；；
图3为本发明中点云粗配准的方法流程图；
图4为本发明中粗配准中点对的对应点关系框图；
图5为本发明中点云精配准的方法流程图。
具体实施方式
为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。
如图1所示，一种基于特征增强多维权重描述子的ICP配准方法，所述方法包括以下步骤：
S1、通过自主移动机器人上的3D轮廓扫描传感器获取两副不同视角下的点云X和点云Y，并利用滤波器分别对点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1，然后分别从所述采样点云X1和采样点云Y1中随机取出多个点对进行特征描述子的提取，得到对应每个点对的特征增强的多维权重描述子；
S2、将所述步骤S1中得到的每个点对的特征增强的多维权重描述子分成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0，然后分别从特征点对集x0和特征点对集y0中找出具有相同多维权重描述子的点对构成点对集x1和点对集y1，并分别从点对集x1和点对集y1中各随机取出一对具有相同多维权重描述子的对应点对进行点云粗配准，得到点云粗配准的变换矩阵；
S3、通过所述点云粗配准的变换矩阵对采样点云X1进行变换，得到变换点云Q，计算变换点云Q与待配准采样点云Y1之间的最近邻点构成最近邻点对，然后建立约束条件并筛选出最近邻点对中符合所述约束条件的点对，从而得到配准点对，最后采用ICP迭代法实现场景点云的精配准。
本实施例中，通过提取基于特征增强的多维权重描述子，并以此用于点云的粗配准，有效提高了粗配准的精确性和实时性；然后利用初始配准对重心作为约束条件，筛除粗配准过程中的误配准对，进一步提高了配准的精确性和实时性。本实施例中通过采用多维权重描述子结合特征点对的空间几何信息，有效克服了大场景点云拼接过程中噪声对特征点提取的干扰，使得点云配准的精度更高；同时也减小了计算机的计算量，提升了配准的效率，使得场景点云拼接的实时性更强。需要说明的是，本实施例中从所述采样点云X1和采样点云Y1中个取出多个点进行特征描述子提取，所取出多个点对的数量应能够满足后续配准操作，本实施例中从所述采样点云X1和采样点云Y1中个取出点的数量为1000个。
如图2所示，所述步骤S1具体实现方法包括：
S10、通过自主移动机器人上的3D轮廓扫描传感器获取所述机器人移动过程中两幅不同视角下的点云X和点云Y，并采用滤波器分别对所述点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1；
S11、从所述采样点云X1中的随机抽取两个采样点p1和p2，组成一个采样点对(p1,p2)，并计算采样点p1和采样点p2之间的欧式距离f1(p1,p2)，可用公式表示：
f1(p1,p2)＝||p1-p2|| (1)
S12、利用K近邻方法搜索采样点p1周围最近的K个点组成协方差矩阵G1，可用公式表示：
式(2)中，xi(i＝1,2,3,…K)表示采样点p1周围K个点的坐标，
表示采样点p1周围K个点的重心坐标，其中
S13、对所述步骤S12中的协方差矩阵G1进行特征值分解，得到协方差矩阵G1的特征值λj及其对应的特征向量vj(j＝1,2,3)，从而求得采样点p1的切平面和法向量n1，可用公式表示：
G1*vj＝λj*vj (3)
由式(3)和式(4)可筛选出采样点p1的法向量n1，即即vj或-vj中与采样点p1成锐角关系的一个特征向量，这里采样点p1的坐标为三维坐标，则其协方差矩阵G1为3*3矩阵，对应的协方差矩阵G1就存在3个特征值和特征向量，故j＝1,2,3；
S14、利用K近邻方法搜索采样点p2周围最近的K个点并构造协方差矩阵G′1，可用公式表示：
式(5)中，x′i(i＝1,2,3,…K)表示采样点p2周围K个点的坐标，
表示采样点p2周围K个点的重心坐标，其中
S15、对所述步骤S14中的协方差矩阵G′1进行特征值分解，得到协方差矩阵G′1的特征值λ′j以及对应的特征向量v′j(j＝1,2,3)，从而求得采样点p2的切平面和法向量n2，可用公式表示：
G′1*v′j＝λ′j*v′j (6)
由式(6)和式(7)可筛选出采样点p2的法向量n2，即v′j或-v′j中与采样点p2成锐角关系的特征向量，这里采样点p2的坐标为三维坐标，则其协方差矩阵G′1为3*3矩阵，对应的协方差矩阵G′1就存在3个特征值和特征向量，故j＝1,2,3；；
S16、根据所述步骤S13得到的法向量n1以及所述步骤S15得到的法向量n2，计算出采样点p1和采样点p2之间的最大表面角f2(p1,n1,p2,n2)，可用公式表示：
f2(p1,n1,p2,n2)＝max(β1,β2) (8)
式(8)中，β1表示采样点p1的表面角，
β2表示采样点p2的表面角，
其中d表示采样点p1和采样点p2之间的特征向量差，d＝p2-p1；
S17、根据所述步骤S13筛选的法向量n1和所述步骤S15筛选的法向量n2，计算出采样点p1和采样点p2之间的法向量距离f3(n1,n2)，可用公式表示：
f3(n1,n2)＝cos-1(|n1·n2|) (9)；
S18、对所述步骤S11中的欧式距离f1(p1,p2)、所述步骤S16中的最大表面角f2(p1,n1,p2,n2)和所述步骤S17中的法向量距离f3(n1,n2)进行加权处理，得到特征增强的多维权重描述子H(f1,f2,f3)，可用公式表示：
H(f1,f2,f3)＝Q1·f1(p1,p2)+Q2·f2(p1,n1,p2,n2)+Q3·f3(n1,n2) (10)
式(10)中，Q1、Q2和Q3分别表示f1(p1,p2)、f2(p1,n1,p2,n2)和f3(n1,n2)的权重值，且满足条件Q1＞2Q2＞2Q3；
S19、重复所述步骤S11～S18，随机从所述采样点云X1和采样点云Y1抽取采样点对，直至得到与所述采样点云X1和采样点云Y1相对应的S个采样点对的特征增强的多维权重描述子H(f1,f2,f3)。
本实施例中，通过计算出采样点对(p1,p2)的欧式距离f1(p1,p2)、最大表面角f2(p1,n1,p2,n2)以及法向量距离f3(n1,n2)的值并进行加权处理，以得到特征增强的多维权重描述子H(f1,f2,f3)，使其能够更好地描述大场景噪声点云的情形，提高了粗配准的精确性和实时性的值提高粗配准的精确性，其中欧氏距离f1(p1,p2)的值在描述点云物体的尺寸变化具有良好的效果，最大点对表面角f2(p1,n1,p2,n2)的值在点云的描述、辨别过程中可以起到很大的作用，但在面对有噪声或遮挡的点云时，其描述效果会有所缩减，而法向量距离f3(n1,n2)描述点及其领域的表面指向的相似性。
如图2所示，所述滤波器为VoxelGrid滤波器(体素滤波器)，滤波时采用的体素大小为0.01m。
如图2所示，所述点云X和点云Y的重叠度大于20％。本实施例中，为了保证配准精度，所述3D轮廓扫描传感器获取的两副不同视角下的点云X和点云Y的重叠度大于20％，所述3D轮廓扫描传感器为3D相机。
如图2所示，所述步骤S18中的Q1∈[0.5,0.7]，Q2∈[0.25,0.3]，Q3∈[0.15,0.25]。
如图2所示，所述步骤S19中的采样点对的数量S＝1000。
如图3所示，所述步骤S2具体实现方法包括：
S21、将所述步骤S1中得到的S个特征增强的多维权重描述子H(f1,f2,f3)组成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0；
S22、从所述步骤S21中的特征对点集x0和特征点对集y0中找出具有相同多维权重描述子的点对，并构成点对集x1，以及从特征点对集y0和特征点对集x0中找出具有相同多维权重描述子的点对构成点对集y1，其中特征对点集x0和特征点对集y0中具有相同多维权重描述子的点对满足：
式(11)中，δ≈0.0001m，r表示多维权重描述子的序号，
和
分别表示特征点对集x0中与特征点集y0中序号为r的多维权重描述子；
S23、从所述步骤S22的点对集x1和点对集y1中各取出一对具有相同多维权重描述子的对应点对，并进行点云粗配准，获得四组点云粗配准的刚体变换矩阵；
S24、将所述步骤S23中的四组刚体变换矩阵应用于采样点云X1，获得四组对应的变换点云Q′，然后计算所述变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H，并选取使距离误差阈值H最小的刚体变换矩阵作为点云粗配准的变换矩阵(R1,T1)，其中变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H可用公式表示：
式(12)中，N表示具有相同多维权重描述子的对应点对个数，L表示变换点云Q′中空间点M1(m1,g1,q1)与待配准采样点云Y1中对应的空间点M2(m2,g2,q2)之间的欧式距离，其中，
本实施例中，如图4所示，从点对集x1和点对集y1中各取出一对具有相同多维权重描述子的对应点对，记做x11,x12,x13,x14和y11,y12,y13,y14，因点对集x1中的点对x11,x12和点对集y1中的点对y11,y12是具有相同多维权重描述子的点对，它们在配准时存在两种配准转换关系，即x11与x12、y11与y12为对应配准对，或x11与x12、y12与y11为对应配准对；同理点对集x1中的点对x13,x14和点对集y1中的点对y13,y14也存在两种配准转换关系，并以此为粗配准的对应点，可获得四组点云粗配准的刚体变换矩阵。
本实施例中，通过从所述特征增强的多维权重描述子H(f1,f2,f3)组成的特征点对集中找出相同特征的多维权重描述子进行粗配准，并通过计算变换点云与待配准采样点云之间的距离误差阈值找到点云粗配准的变换矩阵(R1,T1)，进一步提高了配准的精确性和实时性。
如图5所示，所述步骤S3具体实现方法包括：
S31、通过所述点云粗配准的变换矩阵(R1,T1)对采样点云X1进行变换，得到采样点云X1的变换点云Q，并利用kd tree(k-dimensional tree，即K维树)方法计算出所述变换点云Q与待配准采样点云Y1之间的最近邻点，得到对应的最近邻点对(Qs,Y1s)(其中，s表示最近邻点对的序号，s＝1,2,3,…S，Qs和Y1s表示最近邻点对的三维空间坐标)；
S32、计算所述步骤S31中最近邻点对(Qs,Y1s)中点对的重心，可用公式表示：
S33、建立约束条件，并筛选出最近邻点对(Qs,Y1s)中符合约束条件的点对，从而得到配准点对(Q3t,Y2t)(其中，t表示配准点对的序号，t＝1,2,3,…M，M表示配准点对的数量，Q3t和Y2t表示配准点对的三维空间坐标)，其约束条件可用公式表示：
式(15)中，ξ取值为0.01mm～0.05mm；
S34、应用SVD方法计算出所述步骤S33中配准点对(Q3t,Y2t)之间的刚体变换矩阵(Rmk,Tmk)，则可求得变换的点云Q3t(k+1)＝Rmk·Q3tk+Tmk；
S35、计算所述步骤S34中的Q3t(k+1)与Y2t对应点对之间的距离平均值d′，可用公式表示：
S36、设置点云ICP迭代停止条件β，并比较△d′k与β之间的大小，若△d′k＞β，则重复所述步骤S34和所述步骤S35；若△d′k＜β，则停止ICP迭代，从而得到点云配准的最优刚体变换矩阵，完成点云的精配准，其中△d′k＝d′k+1-d′k，k为ICP迭代更新过程中的重复次数，且Q3t0＝Q3t。
本实施例中，迭代停止条件β＝0.001mm，通过变换矩阵(R1,T1)将采样点云X1进行变换，得到变换点云Q，并利用kd tree方法计算出变换点云Q与待配准采样点云Y1之间的最近邻点，然后应用最近邻点对重心作为约束条件筛除粗配准过程中的误配准对，最后采用ICP迭代法完成点云的精配准，有效提升了配准的效率和精度，使得场景点云拼接的实时性更强。
以上对本发明所提供的一种基于特征增强多维权重描述子的ICP配准方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
Claims (8)
1.一种基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述方法包括以下步骤：
S1、通过自主移动机器人上的3D轮廓扫描传感器获取两副不同视角下的点云X和点云Y，并利用滤波器分别对点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1，然后分别从所述采样点云X1和采样点云Y1中随机取出多个点对进行特征描述子的提取，得到对应每个点对的特征增强的多维权重描述子；
S2、将所述步骤S1得到的每个点对的特征增强的多维权重描述子分成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0，然后从特征点对集x0和特征点对集y0中找出具有相同多维权重描述子的点对构成点对集x1和点对集y1，并分别从点对集x1和点对集y1中各随机取出一对具有相同多维权重描述子的对应点对进行点云粗配准，得到点云粗配准的变换矩阵；
S3、通过所述点云粗配准的变换矩阵对采样点云X1进行变换，得到变换点云Q，计算变换点云Q与待配准采样点云Y1之间的最近邻点构成最近邻点对，然后建立约束条件，并筛选出最近邻点对中符合约束条件的点对，从而得到配准点对，最后采用ICP迭代法实现场景点云的精配准。
2.如权利要求1所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S1具体实现方法包括：
S10、通过自主移动机器人上的3D轮廓扫描传感器获取所述机器人移动过程中两幅不同视角下的点云X和点云Y，并采用滤波器分别对所述点云X和点云Y进行滤波处理，得到对应的采样点云X1和采样点云Y1；
S11、从所述采样点云X1中的随机抽取两个采样点p1和p2，组成一个采样点对(p1,p2)，并计算采样点p1和采样点p2之间的欧式距离f1(p1,p2)，可用公式表示：
f1(p1,p2)＝||p1-p2|| (1)
S12、利用K近邻方法搜索采样点p1周围最近的K个点组成协方差矩阵G1，可用公式表示：
式(2)中，xi(i＝1,2,3,…K)表示采样点p1周围K个点的坐标，
表示采样点p1周围K个点的重心坐标，其中
S13、对所述步骤S12中的协方差矩阵G1进行特征值分解，得到协方差矩阵G1的特征值λj及其对应的特征向量vj(j＝1,2,3)，从而求得采样点p1的切平面和法向量n1，可用公式表示：
G1*vj＝λj*vj (3)
由式(3)和式(4)可筛选出采样点p1的法向量n1，即vj或-vj中与采样点p1成锐角关系的一个特征向量；
S14、利用K近邻方法搜索采样点p2周围最近的K个点并构造协方差矩阵G′1，可用公式表示：
式(5)中，x′i(i＝1,2,3,…K)表示采样点p2周围K个点的坐标，
表示采样点p2周围K个点的重心坐标，其中
S15、对所述步骤S14中的协方差矩阵G′1进行特征值分解，得到协方差矩阵G′1的特征值λ′j以及对应的特征向量v′j(j＝1,2,3)，从而求得采样点p2的切平面和法向量n2，可用公式表示：
G′1*v′j＝λ′j*v′j (6)
由式(6)和式(7)可筛选出采样点p2的法向量n2，即v′j或-v′j中与采样点p2成锐角关系的一个特征向量；
S16、根据所述步骤S13得到的法向量n1以及所述步骤S15得到的法向量n2，计算出采样点p1和采样点p2之间的最大表面角f2(p1,n1,p2,n2)，可用公式表示：
f2(p1,n1,p2,n2)＝max(β1,β2) (8)
式(8)中，β1表示采样点p1的表面角，
β2表示采样点p2的表面角，
其中d表示采样点p1和采样点p2之间的特征向量差，d＝p2-p1；
S17、根据所述步骤S13筛选的法向量n1和所述步骤S15筛选的法向量n2，计算出采样点p1和采样点p2之间的法向量距离f3(n1,n2)，可用公式表示：
f3(n1,n2)＝cos-1(|n1·n2|) (9)；
S18、对所述步骤S11中的欧式距离f1(p1,p2)、所述步骤S16中的最大表面角f2(p1,n1,p2,n2)和所述步骤S17中的法向量距离f3(n1,n2)进行加权处理，得到特征增强的多维权重描述子H(f1,f2,f3)，可用公式表示：
H(f1,f2,f3)＝Q1·f1(p1,p2)+Q2·f2(p1,n1,p2,n2)+Q3·f3(n1,n2) (10)
式(10)中，Q1、Q2和Q3分别表示f1(p1,p2)、f2(p1,n1,p2,n2)和f3(n1,n2)的权重值，且满足条件Q1＞2Q2＞2Q3；
S19、重复所述步骤S11～S18，随机从所述采样点云X1和采样点云Y1抽取采样点对，直至得到与所述采样点云X1和采样点云Y1相对应的S个采样点对的特征增强的多维权重描述子H(f1,f2,f3)。
3.如权利要求2所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S10中所述滤波器为VoxelGrid滤波器，滤波时采用的体素大小为0.01m。
4.如权利要求3所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述点云X和点云Y的重叠度大于20％。
5.如权利要求4所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S18中的Q1∈[0.5,0.7]，Q2∈[0.25,0.3]，Q3∈[0.15,0.25]。
6.如权利要求5所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S19中的采样点对的数量S＝1000。
7.如权利要求6所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S2具体实现方法包括：
S21、将所述步骤S1中得到的S个特征增强的多维权重描述子H(f1,f2,f3)组成与采样点云X1和采样点云Y1相对应的特征点对集x0和特征点对集y0；
S22、从所述步骤S21中的特征点对集x0和特征点对集y0中找出具有相同多维权重描述子的点对构成点对集x1，以及从特征点对集y0和特征点对集x0中找出具有相同多维权重描述子的点对构成点对集y1，其中具有相同多维权重描述子的点对应满足公式：
式(11)中，δ≈0.0001m，r表示多维权重描述子的序号，
和
分别表示特征点对集x0中与特征点集y0中序号为r的多维权重描述子；
S23、从所述步骤S22的点对集x1和点对集y1中各取出一对具有相同多维权重描述子的对应点对，并进行点云粗配准，获得四组点云粗配准的刚体变换矩阵；
S24、将所述步骤S23中的四组刚体变换矩阵应用于采样点云X1，获得四组对应的变换点云Q′，然后计算所述变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H，并选取使距离误差阈值H最小的刚体变换矩阵作为点云粗配准的变换矩阵(R1,T1)，其中变换点云Q′中的点与待配准采样点云Y1中的点之间的距离误差阈值H可用公式表示：
式(12)中，N表示具有相同多维权重描述子的对应点对个数，L表示变换点云Q′中空间点M1(m1,g1,q1)与待配准采样点云Y1中对应的空间点M2(m2,g2,q2)之间的欧式距离，其中，
8.如权利要求7所述的基于特征增强多维权重描述子的ICP配准方法，其特征在于，所述步骤S3具体实现方法包括：
S31、通过所述点云粗配准的变换矩阵(R1,T1)对采样点云X1进行变换，得到采样点云X1的变换点云Q，并利用kd tree方法计算出所述变换点云Q与待配准采样点云Y1之间的最近邻点，得到对应的最近邻点对(Qs,Y1s)，其中，s表示最近邻点对的序号，s＝1,2,3,…S，Qs和Y1s表示最近邻点对的三维空间坐标；
S32、计算所述步骤S31中最近邻点对(Qs,Y1s)中点对的重心，可用公式表示：
S33、建立约束条件，并筛选出最近邻点对(Qs,Y1s)中符合约束条件的点对，从而得到配准点对(Q3t,Y2t)，其中，t表示配准点对的序号，t＝1,2,3,…M，M表示配准点对的数量，Q3t和Y2t表示配准点对的三维空间坐标，其约束条件可用公式表示：
式(15)中，ξ取值为0.01mm～0.05mm；
S34、应用SVD方法计算出所述步骤S33中配准点对(Q3t,Y2t)之间的刚体变换矩阵(Rmk,Tmk)，则可求得变换的点云Q3t(k+1)＝Rmk·Q3tk+Tmk；
S35、计算所述步骤S34中的Q3t(k+1)与Y2t对应点对之间的距离平均值d′，可用公式表示：
S36、设置点云ICP迭代停止条件β，并比较Δd′k与β之间的大小，若Δd′k＞β，则重复所述步骤S34和所述步骤S35；若Δd′k＜β，则停止ICP迭代，从而得到点云配准的最优刚体变换矩阵，完成点云的精配准，其中Δd′k＝d′k+1-d′k，k为ICP迭代更新过程中的重复次数，且Q3t0＝Q3t。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910201443.XA CN109934859B (zh) | 2019-03-18 | 2019-03-18 | 一种基于特征增强多维权重描述子的icp配准方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910201443.XA CN109934859B (zh) | 2019-03-18 | 2019-03-18 | 一种基于特征增强多维权重描述子的icp配准方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109934859A CN109934859A (zh) | 2019-06-25 |
| CN109934859B true CN109934859B (zh) | 2023-03-24 |
Family
ID=66987426
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910201443.XA Active CN109934859B (zh) | 2019-03-18 | 2019-03-18 | 一种基于特征增强多维权重描述子的icp配准方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109934859B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112950706B (zh) * | 2019-12-11 | 2023-11-03 | 京东科技信息技术有限公司 | 可移动设备定位数据处理方法、装置、设备及存储介质 |
| CN113483664B (zh) * | 2021-07-20 | 2022-10-21 | 科派股份有限公司 | 一种基于线结构光视觉的屏风板自动上料系统及方法 |
| CN114549601B (zh) * | 2022-02-11 | 2023-03-28 | 中国科学院精密测量科学与技术创新研究院 | 一种顾及点对可靠性的滑坡多时相tls点云精配准方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015039375A1 (zh) * | 2013-09-17 | 2015-03-26 | 中国科学院深圳先进技术研究院 | 点云数据质量自动优化方法及系统 |
| CN107886528A (zh) * | 2017-11-30 | 2018-04-06 | 南京理工大学 | 基于点云的配电线路作业场景三维重建方法 |
| CN108133458A (zh) * | 2018-01-17 | 2018-06-08 | 视缘(上海)智能科技有限公司 | 一种基于目标物体空间点云特征的自动拼接方法 |
-
2019
- 2019-03-18 CN CN201910201443.XA patent/CN109934859B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015039375A1 (zh) * | 2013-09-17 | 2015-03-26 | 中国科学院深圳先进技术研究院 | 点云数据质量自动优化方法及系统 |
| CN107886528A (zh) * | 2017-11-30 | 2018-04-06 | 南京理工大学 | 基于点云的配电线路作业场景三维重建方法 |
| CN108133458A (zh) * | 2018-01-17 | 2018-06-08 | 视缘(上海)智能科技有限公司 | 一种基于目标物体空间点云特征的自动拼接方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109934859A (zh) | 2019-06-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112102458B (zh) | 基于激光雷达点云数据辅助的单镜头三维图像重构方法 | |
| CN108038906B (zh) | 一种基于图像的三维四边形网格模型重建方法 | |
| CN108665491B (zh) | 一种基于局部参考点的快速点云配准方法 | |
| CN109544456A (zh) | 基于二维图像和三维点云数据融合的全景环境感知方法 | |
| CN109934859B (zh) | 一种基于特征增强多维权重描述子的icp配准方法 | |
| CN108921895B (zh) | 一种传感器相对位姿估计方法 | |
| CN107818598B (zh) | 一种基于视觉矫正的三维点云地图融合方法 | |
| CN112750198B (zh) | 一种基于非刚性点云的稠密对应预测方法 | |
| CN102411779B (zh) | 基于图像的物体模型匹配姿态测量方法 | |
| CN112929626B (zh) | 一种基于智能手机影像的三维信息提取方法 | |
| CN104463953A (zh) | 基于惯性测量单元与rgb-d传感器的三维重建方法 | |
| CN111860651A (zh) | 一种基于单目视觉的移动机器人半稠密地图构建方法 | |
| CN109766903A (zh) | 一种基于曲面特征的点云模型曲面匹配方法 | |
| CN117132630A (zh) | 一种基于二阶空间兼容性度量的点云配准方法 | |
| CN110415281B (zh) | 一种基于Loam曲率加权的点集刚体配准方法 | |
| CN110176041B (zh) | 一种基于双目视觉算法的新型列车辅助装配方法 | |
| CN117541614B (zh) | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 | |
| CN108062766B (zh) | 一种融合颜色矩信息的三维点云配准方法 | |
| CN112001954B (zh) | 一种基于极曲线约束的水下pca-sift图像匹配方法 | |
| Shen et al. | A 3D modeling method of indoor objects using Kinect sensor | |
| CN115423854B (zh) | 一种基于多尺度特征提取的多视角点云配准及点云融合方法 | |
| CN116681844A (zh) | 一种基于亚米级立体像对卫星影像的建筑物白膜构建方法 | |
| Zhang et al. | A 3D point cloud reconstruction method | |
| Liu et al. | Spherical transformer: Adapting spherical signal to CNNs | |
| CN112509018A (zh) | 一种四元数空间优化的三维图像配准方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |