CN114511673A - 一种基于改进icp的海底局部环境初步构建方法 - Google Patents

一种基于改进icp的海底局部环境初步构建方法 Download PDF

Info

Publication number
CN114511673A
CN114511673A CN202210091474.6A CN202210091474A CN114511673A CN 114511673 A CN114511673 A CN 114511673A CN 202210091474 A CN202210091474 A CN 202210091474A CN 114511673 A CN114511673 A CN 114511673A
Authority
CN
China
Prior art keywords
point
points
distance
cloud data
point cloud
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.)
Granted
Application number
CN202210091474.6A
Other languages
English (en)
Other versions
CN114511673B (zh
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202210091474.6A priority Critical patent/CN114511673B/zh
Publication of CN114511673A publication Critical patent/CN114511673A/zh
Application granted granted Critical
Publication of CN114511673B publication Critical patent/CN114511673B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2210/00—Indexing scheme for image generation or computer graphics
    • G06T2210/61—Scene description

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Graphics (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供一种基于改进ICP的海底局部环境初步构建方法，包括：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。本发明对ISS算法所进行的改进，可使该算法在一定程度上忽略点云数据尺度信息带来的影响。

Description

一种基于改进ICP的海底局部环境初步构建方法
技术领域
本发明属于同时定位与构图技术领域，具体涉及一种基于改进ICP的海底局部环境初步构建方法。
背景技术
随着三维实时成像声呐的出现，使得水下实时稠密点云数据的直接获取成为可能，这在一方面可加快水下SLAM技术的发展。在经典的水下环境建图方法中，一般都是结合运载体的导航系统来确定所采集数据的空间信息的，而如果运载体的导航系统无法正常工作时，那么水下环境构建基本是无法完成的。为了能在运载体的导航系统无法正常工作时仍然能进行局部的环境构建，故设计了一种仅基于点云数据的局部环境初步构建方法，该方法对经典的ISS(Intrinsic Shape Signatures)、FPFH(Fast Point FeatureHistograms)和RANSAC(Random Sample Consensus)算法都进行了优化。
发明内容
本发明的目的是为了提供一种短时间内不需要依赖惯导导航系统的基于改进ICP的局部环境初步构建方法。
本发明的目的是这样实现的：包括如下步骤：
步骤1：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；
步骤2：特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；
步骤3：特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；
步骤4：点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。
本发明还包括：
1.步骤1中特征点提取具体为：
设k时刻采集到的一帧三维点云数据为
Figure BDA0003489384300000011
其中pki＝{(xki,yki,zki)|1≤i≤nk}，表示Pk中的第i个点在空间中的坐标位置；
在经典的ISS算法中，对于pki，在计算协方差矩阵Cov(pki)前，设定给定参数r；
计算出pki的邻域
Figure BDA0003489384300000021
其中Pki为以pki为半径，以r为半径空间内点的集合，r为给定值；
对邻域(||pki-pj||2＜r)内的每个点pki,l，令Δpki,l＝pki,l-pki，由此得到新集和
Figure BDA0003489384300000022
在ΔPki的所有元素的各个维度上选择绝对值最大的一个值，令该值为rmax；
令
Figure BDA0003489384300000023
利用Pki,nor进行ISS算法后续的计算，判断pki是否为特征点；
设从Pk中提取得到的特征点为
Figure BDA0003489384300000024
nfk表示特征点的数量。
2.步骤2中特征点描述具体为：
对于Fk中的第i个特征点fi，计算SPFH(fi)时，所使用的参考单位向量为
Figure BDA0003489384300000025
而不直接使用fi处的法向量
Figure BDA0003489384300000026
设以fi为圆心，以rFPFH为半径的空间内点的集合(不包含fi)为
Figure BDA0003489384300000027
Figure BDA0003489384300000028
为
Figure BDA0003489384300000029
中元素数量；
fi距离
Figure BDA00034893843000000210
中所有点距离的最小距离为
Figure BDA00034893843000000211
则有
Figure BDA00034893843000000212
其中KFPFH为给定值；
令描述子χi＝FPFH′(fi)，则有描述子点集
Figure BDA00034893843000000213
3.参考单位向量
Figure BDA00034893843000000214
的具体计算方法为：
Figure BDA00034893843000000215
中每个点对应法向量所组成的集合为
Figure BDA00034893843000000216
则有
Figure BDA00034893843000000217
其中KSPFH是给定参数；
对
Figure BDA00034893843000000218
单位化，即可得到
Figure BDA00034893843000000219
4.步骤3中特征点匹配具体为：
k和k+1时刻所得到的点云数据为Pk和Pk+1，描述子点集为Xk、Xk+1；
对Xk和Xk+1中的描述子进行配对操作，去除那些空间中相差较远的特征点配对结果，得到的配对结果记为
Figure BDA0003489384300000031
其中，对于Ck,k+1中的第i个元素有ci＝{fk,i,fk+1,i}，fk,i∈Xk，fk+1,i∈Xk+1；
计算Pk中每个点到其最近邻居点(除该点自身外)的距离，则有距离集合：
Figure BDA0003489384300000032
计算Dk,d中距离的平均值dk,mean；
给定距离尺度系数Kscale的值，并确定dinlier为：dinlier＝Kscale*dmean；
利用RANSAC来估计Pk和Pk+1间的位姿关系，且使用dinlier来判断Pk和Pk+1中配对成功的点数，即可得到旋转矩阵Rk→k+1和平移向量tk→k+1。
5.步骤4中点云数据匹配及局部环境构建为：
利用初始估计的位姿Rk→k+1和tk→k+1对Pk进行变换得到Pk,k→k+1，即；Pk,k→k+1＝Rk→k+1Pk+tk→k+1；
从Pk,k→k+1中选出位于曲率较低处的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
Figure BDA0003489384300000033
其中np,k表示
Figure BDA0003489384300000034
中点的数量；
在Pk+1中搜索距离
Figure BDA0003489384300000035
中的每个点最近的λ个点；
从这λ个点中再筛选出距离
Figure BDA0003489384300000036
中相应点小于给定距离D1的点，得到点集
Figure BDA0003489384300000037
其中，l表示上述筛选操作后
Figure BDA0003489384300000038
中包含点集的数量；
从Pk+1中选出距离点
Figure BDA0003489384300000039
最近的λ个点，记为
Figure BDA00034893843000000310
再从
Figure BDA00034893843000000311
中选出距离点
Figure BDA00034893843000000312
小于给定距离D1的点集
Figure BDA00034893843000000313
其中，γi表示
Figure BDA0003489384300000041
中点的数量；
Figure BDA0003489384300000042
中距离
Figure BDA0003489384300000043
最近的一个点记为pnst-nbor，比较pnst-nbor与
Figure BDA0003489384300000044
两点处的法向量，若两法向量的夹角在区间[θt,180°-θt]，则将
Figure BDA0003489384300000045
从
Figure BDA0003489384300000046
中舍去，其中θt为给定值；
计算
Figure BDA0003489384300000047
中所有点与pnst-nbor的距离，若存在距离大于给定距离R1的情况，则将
Figure BDA0003489384300000048
从
Figure BDA0003489384300000049
中舍去；
计算
Figure BDA00034893843000000410
中的每个点集中的点是否位于曲率较低处；
若没有位于曲率较低处，则将对应的点集剔除出
Figure BDA00034893843000000411
得到
Figure BDA00034893843000000412
其中，l′表示
Figure BDA00034893843000000413
中点集的数量；
对于
Figure BDA00034893843000000414
中的每个点集，计算这些点集中的点在Pk,k→k+1中的最近邻，若最近邻没有位于平面上，或最近邻距离
Figure BDA00034893843000000415
中相应点的距离超过给定距离R2，则将相应点集从
Figure BDA00034893843000000416
剔除，且R2＞R1；
得到
Figure BDA00034893843000000417
相应的最近邻点集记为
Figure BDA00034893843000000418
Figure BDA00034893843000000419
每个点集中的点对应的法向量所组成的集合记为
Figure BDA00034893843000000420
将
Figure BDA00034893843000000421
中的点投影至标准点集
Figure BDA00034893843000000422
的对应点中，得到投影点集
Figure BDA00034893843000000423
计算
Figure BDA00034893843000000424
与
Figure BDA00034893843000000425
中对应点的距离，得到误差函数error(Rk→k+1,tk→k+1)；
通过非线性优化的方法对error(Rk→k+1,tk→k+1)进行优化，得到ΔRk→k+1和Δtk→k+1；
利用ΔRk→k+1与Δtk→k+1对Rk→k+1与tk→k+1进行更新；
而后继续对新的Rk→k+1与tk→k+1进行迭代优化，当到达迭代次数nt时，停止优化操作，其中，nt为给定值；
通过Rk→k+1与tk→k+1即可实现第k帧点云数据Pk与第k+1帧点云数据Pk+1间的匹配；
通过不断地将各连续帧的点云数据进行匹配，即可逐步实现对海底局部环境的初步构建。
与现有技术相比，本发明的有益效果是：本发明对ISS算法所进行的改进，可使该算法在一定程度上忽略点云数据尺度信息带来的影响。本发明对FPFH算法进行的改进，可在一定程度上加固该算法中参考法向量的稳定性。本发明在RANSAC算法中判断两对点匹配成功的距离可以忽视点云密度及尺度所带来的影响。本发明可实现对海底局部环境的初步构建。
附图说明
图1为本发明初步构建局部环境的流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，一种基于改进ICP的海底局部环境初步构建方法，包括如下步骤：
特征点提取步骤：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；
特征点描述步骤：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；
特征点匹配步骤：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；
点云数据匹配及局部环境构建步骤：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。
实施例1：特征点提取步骤
设k时刻采集到的一帧三维点云数据为
Figure BDA0003489384300000051
其中pki＝{(xki,yki,zki)|1≤i≤nk}，表示Pk中的第i个点在空间中的坐标位置；
在经典的ISS算法中，对于pki，在计算协方差矩阵Cov(pki)前，设定给定参数r；
计算出pki的邻域
Figure BDA0003489384300000052
其中Pki为以pki为半径，以r为半径空间内点的集合，r为给定值；
对邻域(||pki-pj||2＜r)内的每个点pki,l，令Δpki,l＝pki,l-pki，由此得到新集和
Figure BDA0003489384300000061
在ΔPki的所有元素的各个维度上选择绝对值最大的一个值，令该值为rmax；
令
Figure BDA0003489384300000062
利用Pki,nor进行ISS算法后续的计算，判断pki是否为特征点；
设从Pk中提取得到的特征点为
Figure BDA0003489384300000063
nfk表示特征点的数量。
实施例2：特征点描述步骤
对于Fk中的第i个特征点fi，计算SPFH(fi)时，所使用的参考单位向量为
Figure BDA0003489384300000064
而不直接使用fi处的法向量
Figure BDA0003489384300000065
设以fi为圆心，以rFPFH为半径的空间内点的集合(不包含fi)为
Figure BDA0003489384300000066
Figure BDA0003489384300000067
为
Figure BDA0003489384300000068
中元素数量；
fi距离
Figure BDA0003489384300000069
中所有点距离的最小距离为
Figure BDA00034893843000000610
则有
Figure BDA00034893843000000611
其中KFPFH为给定值；
令描述子χi＝FPFH′(fi)，则有描述子点集
Figure BDA00034893843000000612
实施例3：参考单位向量
Figure BDA00034893843000000613
中每个点对应法向量所组成的集合为
Figure BDA00034893843000000614
则有
Figure BDA00034893843000000615
其中KSPFH是给定参数；
对
Figure BDA00034893843000000616
单位化，即可得到
Figure BDA00034893843000000617
实施例4：特征点匹配
k和k+1时刻所得到的点云数据为Pk和Pk+1，描述子点集为Xk、Xk+1；
对Xk和Xk+1中的描述子进行配对操作，去除那些空间中相差较远的特征点配对结果，得到的配对结果记为
Figure BDA0003489384300000071
其中，对于Ck,k+1中的第i个元素有ci＝{fk,i,fk+1,i}，fk,i∈Xk，fk+1,i∈Xk+1；
计算Pk中每个点到其最近邻居点(除该点自身外)的距离，则有距离集合：
Figure BDA0003489384300000072
计算Dk,d中距离的平均值dk,mean；
给定距离尺度系数Kscale的值，并确定dinlier为：dinlier＝Kscale*dmean；
利用RANSAC来估计Pk和Pk+1间的位姿关系，且使用dinlier来判断Pk和Pk+1中配对成功的点数，即可得到旋转矩阵Rk→k+1和平移向量tk→k+1。
实施例5：点云数据匹配及局部环境构建
利用初始估计的位姿Rk→k+1和tk→k+1对Pk进行变换得到Pk,k→k+1，即；Pk,k→k+1＝Rk→k+1Pk+tk→k+1；
从Pk,k→k+1中选出位于曲率较低处的点；
使用非极大值抑制的方法使选取的点分布均匀；
得到的标准点集记为
Figure BDA0003489384300000073
其中np,k表示
Figure BDA0003489384300000074
中点的数量；
在Pk+1中搜索距离
Figure BDA0003489384300000075
中的每个点最近的λ个点；
从这λ个点中再筛选出距离
Figure BDA0003489384300000076
中相应点小于给定距离D1的点，得到点集
Figure BDA0003489384300000077
其中，l表示上述筛选操作后
Figure BDA0003489384300000078
中包含点集的数量；
从Pk+1中选出距离点
Figure BDA0003489384300000079
最近的λ个点，记为
Figure BDA00034893843000000710
再从
Figure BDA00034893843000000711
中选出距离点
Figure BDA00034893843000000712
小于给定距离D1的点集
Figure BDA00034893843000000713
其中，γi表示
Figure BDA00034893843000000714
中点的数量；
Figure BDA00034893843000000715
中距离
Figure BDA00034893843000000716
最近的一个点记为pnst-nbor，比较pnst-nbor与
Figure BDA00034893843000000717
两点处的法向量，若两法向量的夹角在区间[θt,180°-θt]，则将
Figure BDA0003489384300000081
从
Figure BDA0003489384300000082
中舍去，其中θt为给定值，θt可以取值10°；
计算
Figure BDA0003489384300000083
中所有点与pnst-nbor的距离，若存在距离大于给定距离R1的情况，则将
Figure BDA0003489384300000084
从
Figure BDA0003489384300000085
中舍去；
计算
Figure BDA0003489384300000086
中的每个点集中的点是否位于曲率较低处；
若没有位于曲率较低处，则将对应的点集剔除出
Figure BDA0003489384300000087
得到
Figure BDA0003489384300000088
其中，l′表示
Figure BDA0003489384300000089
中点集的数量；
对于
Figure BDA00034893843000000810
中的每个点集，计算这些点集中的点在Pk,k→k+1中的最近邻，若最近邻没有位于平面上，或最近邻距离
Figure BDA00034893843000000811
中相应点的距离超过给定距离R2，则将相应点集从
Figure BDA00034893843000000812
剔除，且R2＞R1；
得到
Figure BDA00034893843000000813
相应的最近邻点集记为
Figure BDA00034893843000000814
Figure BDA00034893843000000815
每个点集中的点对应的法向量所组成的集合记为
Figure BDA00034893843000000816
将
Figure BDA00034893843000000817
中的点投影至标准点集
Figure BDA00034893843000000818
的对应点中，得到投影点集
Figure BDA00034893843000000819
计算
Figure BDA00034893843000000820
与
Figure BDA00034893843000000821
中对应点的距离，得到误差函数error(Rk→k+1,tk→k+1)；
通过非线性优化的方法对error(Rk→k+1,tk→k+1)进行优化，得到ΔRk→k+1和Δtk→k+1；
利用ΔRk→k+1与Δtk→k+1对Rk→k+1与tk→k+1进行更新；
而后继续对新的Rk→k+1与tk→k+1进行迭代优化，当到达迭代次数nt时，停止优化操作，其中，nt为给定值；
通过Rk→k+1与tk→k+1即可实现第k帧点云数据Pk与第k+1帧点云数据Pk+1间的匹配；
k时刻到k+υ(υ＞0)时刻采集的点云数据集为Pk,k+υ＝{Pk,Pk+1,…,Pk+υ}；
通过不断地将Pk,k+υ中每两帧点云数据拼接起来，即可初步得到海底的局部环境构图。
实施例6.曲率较低处
对于点集P，取其中一点pi，计算以pi为中心，以R3为半径的球体所包含的所有点
Figure BDA0003489384300000091
其中，R3＞R2；
计算出μi、Σi；
其中，
Figure BDA0003489384300000092
对Σi进行奇异值分解得到
Figure BDA0003489384300000093
定义曲率为：
Figure BDA0003489384300000094
若α＜αthreshold，且λ1＜δλ2，则认为pi位于曲率较低处，αthreshold为给定的曲率阈值；
否则，认为pi没有位于曲率较低处；
其中，0＜δ＜1。
实施例7.将
Figure BDA0003489384300000095
中的点投影至标准点集
Figure BDA0003489384300000096
的对应点中
对于
Figure BDA0003489384300000097
的第i个点集
Figure BDA0003489384300000098
从
Figure BDA0003489384300000099
中取出第j个点
Figure BDA00034893843000000910
Figure BDA00034893843000000911
在Pk,k→k+1中的最近邻为
Figure BDA00034893843000000912
Figure BDA00034893843000000913
处的法向量为
Figure BDA00034893843000000914
计算
Figure BDA00034893843000000915
距离
Figure BDA00034893843000000916
点邻域所形成曲面的距离
Figure BDA00034893843000000917
得到
Figure BDA00034893843000000918
在Pk,k→k+1中的投影
Figure BDA00034893843000000919
所述距离点
Figure BDA00034893843000000920
的邻域所形成曲面的距离
Figure BDA00034893843000000921
设
Figure BDA0003489384300000101
邻域的点集为
Figure BDA0003489384300000102
则
Figure BDA0003489384300000103
其中：
Figure BDA0003489384300000104
h为给定值。
实施例8.误差函数error(Rk→k+1,tk→k+1)
对于
Figure BDA0003489384300000105
其对应的误差部分为
Figure BDA0003489384300000106
则
Figure BDA0003489384300000107

Claims (6)

1.一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤如下：
步骤1：特征点提取：对ISS算法进行改进，使该算法忽略尺度信息带来的影响，由此在点云数据中提取特征点；
步骤2：特征点描述：对FPFH算法进行改进，减弱该算法对于法向量的依赖性，由此对特征点进行描述；
步骤3：特征点匹配：利用RANSAC算法实现两帧点云特征点的匹配，初步估计出两帧点云数据的位姿关系；
步骤4：点云数据匹配及局部环境构建：利用改进的ICP方法，进一步优化当前两帧点云数据的匹配效果。
2.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤1中特征点提取具体为：设k时刻采集到的一帧三维点云数据为
Figure FDA00034893842900000111
其中pki＝{(xki，yki，zki)|1≤i≤nk}，表示Pk中的第i个点在空间中的坐标位置；
在经典的ISS算法中，对于pki，在计算协方差矩阵Cov(pki)前，设定给定参数r；计算出pki的邻域
Figure FDA0003489384290000011
其中Pki为以pki为半径，以r为半径空间内点的集合，r为给定值；对邻域(||pki-pj||2＜r)内的每个点pki，l，令Δpki，l＝pki，l-pki，由此得到新集和
Figure FDA0003489384290000012
在ΔPki的所有元素的各个维度上选择绝对值最大的一个值，令该值为rmax；令
Figure FDA0003489384290000013
利用Pki，nor进行ISS算法后续的计算，判断pki是否为特征点；设从Pk中提取得到的特征点为
Figure FDA00034893842900000112
nfk表示特征点的数量。
3.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤2中特征点描述具体为：对于Fk中的第i个特征点fi，计算SPFH(fi)时，所使用的参考单位向量为
Figure FDA00034893842900000114
而不直接使用fi处的法向量
Figure FDA00034893842900000110
设以fi为圆心，以rFPFH为半径的空间内点的集合为
Figure FDA0003489384290000016
Figure FDA0003489384290000017
为
Figure FDA0003489384290000018
中元素数量；fi距离
Figure FDA00034893842900000113
中所有点距离的最小距离为
Figure FDA0003489384290000019
则有
Figure FDA0003489384290000014
其中KFPFH为给定值；令描述子χi＝FPFH′(fi)，则有描述子点集
Figure FDA0003489384290000015
4.根据权利要求3所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，参考单位向量
Figure FDA00034893842900000220
的具体计算方法为：
Figure FDA00034893842900000219
中每个点对应法向量所组成的集合为
Figure FDA0003489384290000021
则有
Figure FDA0003489384290000022
其中KSPFH是给定参数；对
Figure FDA0003489384290000023
单位化，即可得到
Figure FDA0003489384290000024
5.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤3中特征点匹配具体为：k和k+1时刻所得到的点云数据为Pk和Pk+1，描述子点集为Xk、Xk+1；对Xk和Xk+1中的描述子进行配对操作，去除那些空间中相差较远的特征点配对结果，得到的配对结果记为
Figure FDA0003489384290000025
其中，对于Ck，k+1中的第i个元素有ci＝{fk，i，fk+1，i}，fk，i∈Xk，fk+1，i∈Xk+1；计算Pk中每个点到其最近邻居点的距离，则有距离集合：
Figure FDA0003489384290000026
计算Dk，d中距离的平均值dk，mean；给定距离尺度系数Kscale的值，并确定dinlier为：dinlier＝Kscale*dmean；利用RANSAC来估计Pk和Pk+1间的位姿关系，且使用dinlier来判断Pk和Pk+1中配对成功的点数，即可得到旋转矩阵Rk→k+1和平移向量tk→k+1。
6.根据权利要求1所述的一种基于改进ICP的海底局部环境初步构建方法，其特征在于，步骤4中点云数据匹配及局部环境构建为：
利用初始估计的位姿Rk→k+1和tk→k+1对Pk进行变换得到Pk，k→k+1，即；Pk，k→k+1＝Rk→k+1Pk+tk→k+1；从Pk，k→k+1中选出位于曲率较低处的点；使用非极大值抑制的方法使选取的点分布均匀；得到的标准点集记为
Figure FDA0003489384290000027
其中np，k表示
Figure FDA0003489384290000028
中点的数量；
在Pk+1中搜索距离
Figure FDA00034893842900000210
中的每个点最近的λ个点；从这λ个点中再筛选出距离
Figure FDA00034893842900000211
中相应点小于给定距离D1的点，得到点集
Figure FDA00034893842900000212
其中，l表示上述筛选操作后
Figure FDA00034893842900000213
中包含点集的数量；
从Pk+1中选出距离点
Figure FDA00034893842900000214
最近的λ个点，记为
Figure FDA00034893842900000215
再从
Figure FDA00034893842900000216
中选出距离点
Figure FDA00034893842900000217
小于给定距离D1的点集
Figure FDA00034893842900000218
其中，γi表示
Figure FDA0003489384290000031
中点的数量；
Figure FDA0003489384290000032
中距离
Figure FDA0003489384290000033
最近的一个点记为pnst-nbor，比较pnst-nbir与
Figure FDA0003489384290000034
两点处的法向量，若两法向量的夹角在区间[θt，180°-θt]，则将
Figure FDA0003489384290000035
从
Figure FDA0003489384290000036
中舍去，其中θt为给定值；
计算
Figure FDA0003489384290000037
中所有点与pnst-nbor的距离，若存在距离大于给定距离R1的情况，则将
Figure FDA00034893842900000325
从
Figure FDA0003489384290000038
中舍去；
计算
Figure FDA0003489384290000039
中的每个点集中的点是否位于曲率较低处；若没有位于曲率较低处，则将对应的点集剔除出
Figure FDA00034893842900000310
得到
Figure FDA00034893842900000311
其中，l′表示
Figure FDA00034893842900000312
中点集的数量；
对于
Figure FDA00034893842900000313
中的每个点集，计算这些点集中的点在Pk，k→k+1中的最近邻，若最近邻没有位于平面上，或最近邻距离
Figure FDA00034893842900000314
中相应点的距离超过给定距离R2，则将相应点集从
Figure FDA00034893842900000315
剔除，且R2＞R1；得到
Figure FDA00034893842900000316
相应的最近邻点集记为
Figure FDA00034893842900000317
Figure FDA00034893842900000318
每个点集中的点对应的法向量所组成的集合记为
Figure FDA00034893842900000319
将
Figure FDA00034893842900000320
中的点投影至标准点集
Figure FDA00034893842900000321
的对应点中，得到投影点集
Figure FDA00034893842900000322
计算
Figure FDA00034893842900000323
与
Figure FDA00034893842900000324
中对应点的距离，得到误差函数error(Rk→k+1，tk→k+1)；通过非线性优化的方法对error(Rk→k+1，tk→k+1)进行优化，得到ΔRk→k+1和Δtk→k+1；利用ΔRk→k+1与Δtk→k+1对Rk→k+1与tk→k+1进行更新；而后继续对新的Rk→k+1与tk→k+1进行迭代优化，当到达迭代次数nt时，停止优化操作，其中，nt为给定值；通过Rk→k+1与tk→k+1即可实现第k帧点云数据Pk与第k+1帧点云数据Pk+1间的匹配；通过不断地将各连续帧的点云数据进行匹配，即可逐步实现对海底局部环境的初步构建。
CN202210091474.6A 2022-01-26 2022-01-26 一种基于改进icp的海底局部环境初步构建方法 Active CN114511673B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210091474.6A CN114511673B (zh) 2022-01-26 2022-01-26 一种基于改进icp的海底局部环境初步构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210091474.6A CN114511673B (zh) 2022-01-26 2022-01-26 一种基于改进icp的海底局部环境初步构建方法

Publications (2)

Publication Number Publication Date
CN114511673A true CN114511673A (zh) 2022-05-17
CN114511673B CN114511673B (zh) 2022-12-09

Family

ID=81549338

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210091474.6A Active CN114511673B (zh) 2022-01-26 2022-01-26 一种基于改进icp的海底局部环境初步构建方法

Country Status (1)

Country Link
CN (1) CN114511673B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106296693A (zh) * 2016-08-12 2017-01-04 浙江工业大学 基于3d点云fpfh特征实时三维空间定位方法
US20190057511A1 (en) * 2017-08-18 2019-02-21 A.Tron3D Gmbh Method for registering at least two different 3d models
CN109493372A (zh) * 2018-10-24 2019-03-19 华侨大学 大数据量、少特征的产品点云数据快速全局优化配准方法
CN110930495A (zh) * 2019-11-22 2020-03-27 哈尔滨工业大学（深圳） 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN111415379A (zh) * 2020-03-23 2020-07-14 南京大学 一种基于布谷鸟优化的三维点云数据配准方法
JP2020139891A (ja) * 2019-02-28 2020-09-03 クモノスコーポレーション株式会社 球形マーカー検出方法
CN112950682A (zh) * 2021-02-07 2021-06-11 南京航空航天大学 一种基于小波变换与尺度表征的跨尺度点云配准算法
CN113702941A (zh) * 2021-08-09 2021-11-26 哈尔滨工程大学 一种基于改进icp的点云测速方法
CN113886510A (zh) * 2021-09-18 2022-01-04 鹏城实验室 一种终端交互方法、装置、设备及存储介质

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106296693A (zh) * 2016-08-12 2017-01-04 浙江工业大学 基于3d点云fpfh特征实时三维空间定位方法
US20190057511A1 (en) * 2017-08-18 2019-02-21 A.Tron3D Gmbh Method for registering at least two different 3d models
CN109493372A (zh) * 2018-10-24 2019-03-19 华侨大学 大数据量、少特征的产品点云数据快速全局优化配准方法
JP2020139891A (ja) * 2019-02-28 2020-09-03 クモノスコーポレーション株式会社 球形マーカー検出方法
CN110930495A (zh) * 2019-11-22 2020-03-27 哈尔滨工业大学（深圳） 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN111415379A (zh) * 2020-03-23 2020-07-14 南京大学 一种基于布谷鸟优化的三维点云数据配准方法
CN112950682A (zh) * 2021-02-07 2021-06-11 南京航空航天大学 一种基于小波变换与尺度表征的跨尺度点云配准算法
CN113702941A (zh) * 2021-08-09 2021-11-26 哈尔滨工程大学 一种基于改进icp的点云测速方法
CN113886510A (zh) * 2021-09-18 2022-01-04 鹏城实验室 一种终端交互方法、装置、设备及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李仁忠等: "基于ISS特征点结合改进ICP的点云配准算法", 《激光与光电子学进展》 *

Also Published As

Publication number Publication date
CN114511673B (zh) 2022-12-09

Similar Documents

Publication Publication Date Title
CN106780442B (zh) 一种立体匹配方法及系统
JP5406705B2 (ja) データ補正装置及び方法
CN106599836B (zh) 多人脸跟踪方法及跟踪系统
CN105993034B (zh) 用于增强表面重构的轮廓求全
Wei et al. Tensor voting guided mesh denoising
CN109800692B (zh) 一种基于预训练卷积神经网络的视觉slam回环检测方法
CN110334762B (zh) 一种基于四叉树结合orb和sift的特征匹配方法
CN111507921B (zh) 一种基于低秩恢复的隧道点云去噪方法
CN110069989B (zh) 人脸图像处理方法及装置、计算机可读存储介质
CN107301643B (zh) 基于鲁棒稀疏表示与拉普拉斯正则项的显著目标检测方法
US20030235337A1 (en) Non-rigid image registration using distance functions
CN112488127B (zh) 基于层次b样条的多尺度点云特征点检测与匹配
CN110909778A (zh) 一种基于几何一致性的图像语义特征匹配方法
CN109872343B (zh) 弱纹理物体姿态跟踪方法、系统及装置
CN114972882A (zh) 基于多注意力机制的磨损表面损伤深度估计方法及系统
CN114119669A (zh) 一种基于Shuffle注意力的图匹配目标跟踪方法与系统
Haindl et al. Model-based texture segmentation
CN114511673B (zh) 一种基于改进icp的海底局部环境初步构建方法
CN110390336B (zh) 一种提高特征点匹配精度的方法
CN111192302A (zh) 一种基于运动平滑性和ransac算法的特征匹配方法
CN109410333B (zh) 一种高质量超面片聚类生成方法
CN109785331B (zh) 基于自适应像素值约束和mrf的声呐图像分割方法
CN113702941A (zh) 一种基于改进icp的点云测速方法
CN106652048B (zh) 基于3d-susan算子的三维模型感兴趣点提取方法
JP2006031390A5 (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