CN114898041A - 一种基于光度误差的改进icp方法 - Google Patents
一种基于光度误差的改进icp方法 Download PDFInfo
- Publication number
- CN114898041A CN114898041A CN202210499210.4A CN202210499210A CN114898041A CN 114898041 A CN114898041 A CN 114898041A CN 202210499210 A CN202210499210 A CN 202210499210A CN 114898041 A CN114898041 A CN 114898041A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- point
- error
- icp
- matching
- 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
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
- G06T17/005—Tree description, e.g. octree, quadtree
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T15/00—3D [Three Dimensional] image rendering
- G06T15/005—General purpose rendering architectures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T15/00—3D [Three Dimensional] image rendering
- G06T15/06—Ray-tracing
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Graphics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Image Analysis (AREA)
Abstract
本申请提供一种基于光度误差的改进ICP方法,方法包括:采集物体点云图像,所述点云图像包括临时物体与目标物体;对所述点云图像进行处理,提取点云信息;对点云信息进行滤波;将目标点云信息构造成KD树,实现源点云信息中一点的匹配;所述目标点云信息为滤波后的点云信息;对于源点云信息中的一点,采用最近邻搜索算法实现点云配对;根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差;依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度,结合ICP几何误差和光度误差的点云配准,通过误差函数来表征点云匹配程度。
Description
技术领域
本申请涉及机器人视觉SLAM中的三维建图领域技术领域,特别涉及一种基于光度误差的改进ICP方法。
背景技术
随着计算机技术和图像处理器的发展,机器人技术的研究有了迅速的发展。其中地图构建作为机器人执行导航定位的前置步骤,是机器人技术的重要研究方向之一。地图构建算法通过传感器采集到的空间环境信息,在没有先验信息的情况下,完成空间物体和环境在计算机系统上的三维地图重建。视觉传感器获取环境信息的能力更高,其所获取的图像能够实现更智能化的功能。
在机器人系统中,针对非结构动态环境的环境感知和建模,要求建图算法具有一定实时性。实时三维重建算法中传感器采集数据与数据分析同时进行,通过视觉里程计完成不同帧图像位姿估计完成图像拼接和模型重建。实时三维重建算法拥有更快的相应速度和能在运行过程中完成自动化三维重建,更加满足机器人系统对建图算法的需求。在建图算法设计点云配准算法,ICP(迭代最近点算法)算法是其中的经典算法,可以通过原始点云数据进行检索匹配点云,但是针对非结构化环境,由于光照、天气等原因,传统的算法生成的是无序点云,不存在数据结构,检索速度慢,并且在非结构化环境下,点云位置差距较大,容易出现空间点对误匹配,使系统终止于局部最优解,造成运行效率低下,检索效率低。
发明内容
本申请提供了一种基于光度误差的改进ICP方法,可用于解决现有技术中点云之间匹配效率低下的技术问题。
本申请提供一种基于光度误差的改进ICP方法,所述方法包括:
步骤1、采集物体点云图像,所述点云图像包括临时物体与目标物体;
步骤2、对所述点云图像进行处理,提取点云信息;
步骤3、对点云信息进行滤波;
步骤4、将目标点云信息构造成KD树,实现源点云信息中一点的匹配;所述目标点云信息为滤波后的点云信息;
步骤5、对于源点云信息中的一点,采用最近邻搜索算法实现点云配对;
步骤6、根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差;
步骤7、依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度,结合ICP几何误差和光度误差的点云配准,通过误差函数来表征点云匹配程度。
可选的,对所述点云图像进行处理,提取点云信息,包括:
步骤2-1、建立像素坐标系Opupvp、相机坐标系Ocxcyczc、;根据像素坐标系和相机坐标系的转换关系,求得世界坐标系一点P在像平面的坐标:
步骤2-2、根据相机径向畸变参数修正像素坐标,修正坐标p′c的像素坐标;
步骤2-3采用相机内参和像素对应深度信息,提取像素信息在世界坐标系的三维坐标。
可选的,对点云信息进行滤波,包括:
步骤3-1、利用局部加权平均的方式对点云信息进行滤波,具体为:
步骤3-2、对利用局部加权平均的方式对点云信息进行滤波后的结果,滤除超出区域图像特征权重阈值随机噪声和测量噪声。
可选的,将目标点云数据构造成KD树,实现源点云信息中一点的匹配,包括:
步骤4-1、对二维平面切割首先从x轴开始切分,假定6个二维数据点{(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)},将位于二维空间内x轴上各点云数据的数值排序,分别为2、4、5、7、8、9;
选择中位数5、7中的任意一个作为切分点;假设选择中位数为7作为切分点,则二维平面划分成以x=7为边界的左右两个子平面,将点(7,2)存入二叉树结构中作为根节点;
在y轴对分割出的两个子平面进行切分;
步骤4-2、依据上述操作循环切割切分三维空间,选择中位点云为切分点;完成三维空间KD树构建。
可选的,对于源点云信息中的一点,采用最近邻搜索算法实现点云配对,包括:
步骤5-1、根据KD树结构,递归的向下访问切分点;
步骤5-2、若目标点在当前维的坐标小于切分点的坐标,则移至左子结点,否则移至右子结点,直到子结点为叶结点为止;
步骤5-3、以此叶结点为当前最近点,并计算当前最近点到目标点的距离dnn;
步骤5-4、递归的向上回退,在每个结点进行一下操作,若当前节点到目标点的距离dcur小于dnn,则更新该节点为当前最近点;
步骤5-5、检查该子结点的父结点的另一子结点对应的区域是否与以目标点为球心、以dnn半径的超球体相交;若相交;则可能在另一个子结点对应的区域内有距离目标点更近的点,移动到另一个子结点,递归的进行最近邻搜索;如不相交,则向上回退;
步骤5-6、当回退到根结点时,搜索结束,当前最近邻点即为最近邻点;完成点云匹配。
可选的,根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差,包括:
步骤6-1、根据ICP迭代计算求解点云的刚性变换矩阵;
步骤6-2、根据所述刚性变换矩阵,求解三维模型的光度误差函数Ergb,计算两帧彩色图像的灰度误差平方和计算光度误差系数Ergb:
可选的,根据ICP迭代计算求解点云的刚性变换矩阵,包括:
设定(qi,pi)为两个点云中的一组配对点,选取距离pi最近的qi作为匹配点;
ICP迭代匹配要使几何误差最小,ICP几何误差Eicp采用以下方法确定:
对于点pi,通过最近邻搜索寻找目标点云中的欧氏距离最小的点作为配对点,并组成匹配点对集;
通过迭代优化求解使误差Eicp最小的旋转矩阵R和平移矩阵t;
更新点云的位置,生成新源点云P’;
重复上述步骤,求解出刚性变换矩阵Tc,直到误差小于给定阈值或达到迭代次数,求出ICP几何误差Eicp;
R为旋转矩阵,t为平移矩阵。
可选的,依据跟踪误差来评估点云信息的匹配程度,结合ICP几何误差和光度误差的点云配准,通过误差函数来表征点云匹配程度
步骤7-1、根据光度误差系数Ergb和几何误差Eicp求解跟踪误差Etrack:
其中,λ为比例系数;
步骤7-2、若跟踪误差大于设定的光度误差系数,则判断是否达到迭代次数,若达到,迭代终止;反之更新源点云,返回将目标点云数据构造成KD树,实现源点云中一点的匹配这一步骤;若小于设定值,输出配准误差。
本申请突破了非结构环境下,需要对生成对象由于光照等原因难以处理的问题;本申请融合三维模型色彩光度代价,精确配准;利用KD树结构,本申请保证点云匹配的效率,提供匹配点对的检索率。
附图说明
图1为本申请实施例提供的一种基于光度误差的改进ICP方法的流程示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚,下面将结合附图对本申请实施方式作进一步地详细描述。
下面首先结合图1对本申请实施例进行介绍。
请参考图1,其示例性示出了本申请实施例提供的一种基于光度误差的改进ICP方法的流程示意图。
步骤1、采集物体点云图像,点云图像包括临时物体与目标物体。
采集的点云图像尺寸为1280*720,采集设备固定于机械臂末端。
步骤2、对点云图像进行处理,提取点云信息。
具体的,对点云图像进行处理,提取点云信息,包括:
步骤2-1、建立像素坐标系Opupvp、相机坐标系Ocxcyczc、;根据像素坐标系和相机坐标系的转换关系,求得世界坐标系一点P在像平面的坐标:
步骤2-2、根据相机径向畸变参数修正像素坐标,修正坐标p′c的像素坐标;
步骤2-3采用相机内参和像素对应深度信息,提取像素信息在世界坐标系的三维坐标。
步骤3、对点云信息进行滤波。
具体的,对点云信息进行滤波,包括:
步骤3-1、利用局部加权平均的方式对点云信息进行滤波,具体为:
步骤3-2、对利用局部加权平均的方式对点云信息进行滤波后的结果,滤除超出区域图像特征权重阈值随机噪声和测量噪声。
通过滤波最大限度的保留物体边缘点云信息的同时,滤除噪声。
步骤4、将目标点云信息构造成KD树,实现源点云信息中一点的匹配。
目标点云信息为滤波后的点云信息。
具体的,将目标点云数据构造成KD树,实现源点云信息中一点的匹配,包括:
步骤4-1、对二维平面切割首先从x轴开始切分,假定6个二维数据点{(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)},将位于二维空间内x轴上各点云数据的数值排序,分别为2、4、5、7、8、9;
选择中位数5、7中的任意一个作为切分点;假设选择中位数为7作为切分点,则二维平面划分成以x=7为边界的左右两个子平面,将点(7,2)存入二叉树结构中作为根节点;
在y轴对分割出的两个子平面进行切分;
步骤4-2、依据上述操作循环切割切分三维空间,选择中位点云为切分点。完成三维空间KD树构建。
本申请实施例通过KD树的数据结构来提高点云的快速匹配。
步骤5、对于源点云信息中的一点,采用最近邻搜索算法实现点云配对。
具体的,对于源点云信息中的一点,采用最近邻搜索算法实现点云配对,包括:
步骤5-1、根据KD树结构,递归的向下访问切分点;
步骤5-2、若目标点在当前维的坐标小于切分点的坐标,则移至左子结点,否则移至右子结点,直到子结点为叶结点为止;
步骤5-3、以此叶结点为当前最近点,并计算当前最近点到目标点的距离dnn;
步骤5-4、递归的向上回退,在每个结点进行一下操作,若当前节点到目标点的距离dcur小于dnn,则更新该节点为当前最近点;
步骤5-5、检查该子结点的父结点的另一子结点对应的区域是否与以目标点为球心、以dnn半径的超球体相交;若相交;则可能在另一个子结点对应的区域内有距离目标点更近的点,移动到另一个子结点,递归的进行最近邻搜索;如不相交,则向上回退;
步骤5-6、当回退到根结点时,搜索结束,当前最近邻点即为最近邻点;完成点云匹配。
步骤6、根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差;
具体的,根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差,包括:
步骤6-1、根据ICP迭代计算求解点云的刚性变换矩阵;
步骤6-1具体包括:
设定(qi,pi)为两个点云中的一组配对点,选取距离pi最近的qi作为匹配点;
ICP迭代匹配要使几何误差最小,ICP几何误差Eicp采用以下方法确定:
对于点pi,通过最近邻搜索寻找目标点云中的欧氏距离最小的点作为配对点,并组成匹配点对集;
通过迭代优化求解使误差Eicp最小的旋转矩阵R和平移矩阵t;
更新点云的位置,生成新源点云P’;
重复上述步骤,求解出刚性变换矩阵Tc,直到误差小于给定阈值或达到迭代次数,求出ICP几何误差Eicp;
R为旋转矩阵,t为平移矩阵。
步骤6-2、根据刚性变换矩阵,求解三维模型的光度误差函数Ergb;计算两帧彩色图像的灰度误差平方和计算光度误差系数Ergb:
步骤7、依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度,结合ICP几何误差和光度误差的点云配准,通过误差函数来表征点云匹配程度。
具体的,包括:
步骤7-1、根据光度误差系数Ergb和几何误差Eicp求解跟踪误差Etrack:
其中,λ为比例系数;
步骤7-2、若跟踪误差大于设定的光度误差系数,则判断是否达到迭代次数,若达到,迭代终止;反之更新源点云,返回将目标点云数据构造成KD树,实现源点云中一点的匹配这一步骤;若小于设定值,输出配准误差。
本申请突破了非结构环境下,需要对生成对象由于光照等原因难以处理的问题;本申请融合三维模型色彩光度代价,精确配准;利用KD树结构,本申请保证点云匹配的效率,提供匹配点对的检索率。
以上的本申请实施方式并不构成对本申请保护范围的限定。
Claims (8)
1.一种基于光度误差的改进ICP方法,其特征在于,所述方法包括:
步骤1、采集物体点云图像,所述点云图像包括临时物体与目标物体;
步骤2、对所述点云图像进行处理,提取点云信息;
步骤3、对点云信息进行滤波;
步骤4、将目标点云信息构造成KD树,实现源点云信息中一点的匹配;所述目标点云信息为滤波后的点云信息;
步骤5、对于源点云信息中的一点,采用最近邻搜索算法实现点云配对;
步骤6、根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差;
步骤7、依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度,结合ICP几何误差和光度误差的点云配准,通过误差函数来表征点云匹配程度。
4.根据权利要求1所述的基于光度误差的改进ICP方法,其特征在于,将目标点云数据构造成KD树,实现源点云信息中一点的匹配,包括:
步骤4-1、对二维平面切割首先从x轴开始切分,假定6个二维数据点{(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)},将位于二维空间内x轴上各点云数据的数值排序,分别为2、4、5、7、8、9;
选择中位数5、7中的任意一个作为切分点;假设选择中位数为7作为切分点,则二维平面划分成以x=7为边界的左右两个子平面,将点(7,2)存入二叉树结构中作为根节点;
在y轴对分割出的两个子平面进行切分;
步骤4-2、依据上述操作循环切割切分三维空间,选择中位点云为切分点;完成三维空间KD树构建。
5.根据权利要求1所述的基于光度误差的改进ICP方法,其特征在于,对于源点云信息中的一点,采用最近邻搜索算法实现点云配对,包括:
步骤5-1、根据KD树结构,递归的向下访问切分点;
步骤5-2、若目标点在当前维的坐标小于切分点的坐标,则移至左子结点,否则移至右子结点,直到子结点为叶结点为止;
步骤5-3、以此叶结点为当前最近点,并计算当前最近点到目标点的距离dnn;
步骤5-4、递归的向上回退,在每个结点进行一下操作,若当前节点到目标点的距离dcur小于dnn,则更新该节点为当前最近点;
步骤5-5、检查该子结点的父结点的另一子结点对应的区域是否与以目标点为球心、以dnn半径的超球体相交;若相交;则可能在另一个子结点对应的区域内有距离目标点更近的点,移动到另一个子结点,递归的进行最近邻搜索;如不相交,则向上回退;
步骤5-6、当回退到根结点时,搜索结束,当前最近邻点即为最近邻点;完成点云匹配。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210499210.4A CN114898041A (zh) | 2022-05-09 | 2022-05-09 | 一种基于光度误差的改进icp方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210499210.4A CN114898041A (zh) | 2022-05-09 | 2022-05-09 | 一种基于光度误差的改进icp方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114898041A true CN114898041A (zh) | 2022-08-12 |
Family
ID=82721263
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210499210.4A Pending CN114898041A (zh) | 2022-05-09 | 2022-05-09 | 一种基于光度误差的改进icp方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114898041A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115147491A (zh) * | 2022-09-02 | 2022-10-04 | 山东西曼克技术有限公司 | 用于agv小车的搬运目标位姿信息估计方法 |
CN117541614A (zh) * | 2024-01-09 | 2024-02-09 | 南京航空航天大学 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
-
2022
- 2022-05-09 CN CN202210499210.4A patent/CN114898041A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115147491A (zh) * | 2022-09-02 | 2022-10-04 | 山东西曼克技术有限公司 | 用于agv小车的搬运目标位姿信息估计方法 |
CN117541614A (zh) * | 2024-01-09 | 2024-02-09 | 南京航空航天大学 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
CN117541614B (zh) * | 2024-01-09 | 2024-03-29 | 南京航空航天大学 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
CN109410321B (zh) | 基于卷积神经网络的三维重建方法 | |
CN110340891B (zh) | 基于点云模板匹配技术的机械臂定位抓取系统及方法 | |
WO2024077812A1 (zh) | 基于点云语义分割与结构拟合的单体建筑三维重建方法 | |
CN111862201B (zh) | 一种基于深度学习的空间非合作目标相对位姿估计方法 | |
CN110827398B (zh) | 基于深度神经网络的室内三维点云自动语义分割方法 | |
Wu et al. | Stereo matching with fusing adaptive support weights | |
CN112348921B (zh) | 一种基于视觉语义点云的建图方法及系统 | |
CN114898041A (zh) | 一种基于光度误差的改进icp方法 | |
CN111310631B (zh) | 一种旋翼作业飞行机器人目标跟踪方法及系统 | |
CN113506318B (zh) | 一种车载边缘场景下的三维目标感知方法 | |
CN111368759B (zh) | 基于单目视觉的移动机器人语义地图构建系统 | |
CN114743259A (zh) | 位姿估计方法、位姿估计系统、终端、存储介质及应用 | |
CN109784297A (zh) | 一种基于深度学习的三维目标识别与最优抓取方法 | |
Shao et al. | A deep learning-based semantic filter for RANSAC-based fundamental matrix calculation and the ORB-SLAM system | |
CN111105452A (zh) | 基于双目视觉的高低分辨率融合立体匹配方法 | |
CN116310098A (zh) | 一种基于注意力机制与可变卷积深度网络的多视图三维重建方法 | |
CN117351078A (zh) | 基于形状先验的目标尺寸与6d姿态估计方法 | |
CN117292076A (zh) | 工程机械局部作业场景动态三维重构方法及系统 | |
CN108921852B (zh) | 基于视差与平面拟合的双分支室外非结构化地形分割网络 | |
CN116452826A (zh) | 基于机器视觉的遮挡情况下煤矸石轮廓估计方法 | |
CN116563104A (zh) | 一种基于粒子群算法的图像配准方法及图像拼接方法 | |
CN115239902A (zh) | 移动设备的周边地图建立方法、装置、设备及存储介质 | |
CN111127474A (zh) | 机载LiDAR点云辅助的正射影像镶嵌线自动选取方法及系统 |
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 |