基于PCL的ICP及其变种算法实现

文章目录

  • 前言
  • 一、ICP算法基础
    • 1.1 提取待匹配点对
    • 1.2 计算旋转平移矩阵
    • 1.3 计算变换后的点和目标点之间的偏差
  • 二、ICP算法变种
    • 2.1 PLICP
    • 2.2 PointToPlane ICP
    • 2.3 NICP
    • 2.4 LM_ICP
  • 三、程序示例
    • 1. 传统方法
    • 2. PointToPlane ICP
  • 总结


前言

ICP(Iterative Closest Point,最近邻点迭代)是应用最广泛的三维点云配准算法之一,网上讲ICP算法原理的非常多,这里列举几个个人觉得讲的比较好的。
三维点云配准 – ICP 算法原理及推导
ICP(迭代最近点)算法
PCL学习笔记二:Registration (ICP算法)

原始的ICP算法的问题在于点对之间只分析距离之间的关系从而引起迭代次数高,最终导致的计算时间过长,所以很多学者提出了各种各样的改进算法,如:PLICP,PointToPlane ICP,NICP,LM_ICP算法等。
本文对各种ICP算法的原理以及其简单的应用进行分析。


一、ICP算法基础

ICP算法的基本逻辑是先通过对应关系估计、关键点检测等方法找到源点云和目标点云的待匹配点对,然后计算旋转和平移矩阵,对source点云进行旋转平移到target点云上,根据设置的阈值进行判断,如果不满足阈值要求就重复以上过程继续计算,最终达到最大迭代次数或者满足设定的均方差阈值才能停止迭代。
可以用一个公式表示:
在这里插入图片描述

1.1 提取待匹配点对

首先按需要进行blob。
然后进行Correspondences estimation(对应关系估计),得到共同部分的点云。

/** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */CorrespondenceEstimationPtr correspondence_estimation_;/** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the * transformation. The default value is 3.*/int min_number_correspondences_;correspondence_estimation_->setInputTarget (target_);if (correspondence_estimation_->requiresTargetNormals ())correspondence_estimation_->setTargetNormals (target_blob);

具体源码自行查看,下面列出Correspondences estimation的计算代码,里面包含了两种计算方法,分别为determineCorrespondences和determineReciprocalCorrespondences 。
determineCorrespondences会计算所有点的对应关系。
determineReciprocalCorrespondences计算两个点云共同部分的对应关系。

最后进行一个CorrespondenceRejector,去除错误的估计。

 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i){registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];if (rej->requiresTargetPoints ())rej->setTargetPoints (target_blob);if (rej->requiresTargetNormals () && target_has_normals_)rej->setTargetNormals (target_blob);}

1.2 计算旋转平移矩阵

我们默认变换为刚性变换,通过空间中两点间的变换关系计算R和T矩阵。假定第一次计算的矩阵为R0R_0R0T0T_0T0。PCL中ICP的初始矩阵为

Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
Eigen::Matrix4f tr = transform.template cast<float> ()

也就是:
[1010101]\begin{bmatrix} 1&&&0\\ &1&&0\\&&1&0\\&&&1\end{bmatrix}1110001
公式如下:
pip_ipi=R0R_0R0*qiq_iqi+T0T_0T0
其中:
在这里插入图片描述
T = [txtytz]\begin{bmatrix} tx&ty&tz\end{bmatrix}[txtytz]
具体的计算采用奇异值分解的方法,具体计算过程前言部分推荐的文章有写。

1.3 计算变换后的点和目标点之间的偏差

对点集p进行变换,计算变换后的点集p1p_1p1和q的距离值。
Distance=∑i=1n∣∣p1\displaystyle\sum_{i=1}^{n}||p_1i=1np1-q ||2^22
Distance和设定的阈值进行对比,如果大于,则跳到第一步重新开始循环迭代,如果达到最大迭代次数还没有满足阈值要求,也会停止迭代,保留最近一次的迭代结果。
PCL中还有一个收敛条件设置setTransformationEpsilon,意思是上一个变换矩阵和当前变换矩阵的差异值,如果差异值小于阈值,也认为达到收敛。

PCL迭代部分的代码如下:

 // Estimate the transformtransformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);// Transform the datatransformCloud (*input_transformed, *input_transformed, transformation_);// Obtain the final transformationfinal_transformation_ = transformation_ * final_transformation_;++nr_iterations_;

二、ICP算法变种

因为计算点对间的最小距离的方法过于耗时和低效,所以出现了很多加速方法,例如先提取点云特征,然后进行特征间的匹配,可以极大减少匹配时间;或者对计算源点云中点到目标点云对应点线或者面的最小距离,可以降低。

2.1 PLICP

PLICP计算当前帧中的点到参考帧中最近邻两点连线的最小距离值,在slam中应用较多,可以或者更小的迭代次数和更高的精度。
原理可以参考论文:
A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 19-25, doi: 10.1109/ROBOT.2008.4543181

2.2 PointToPlane ICP

计算Source点云中点到目标点云对应点形成的面的最小距离值。
在这里插入图片描述
这里ni为qi的法线,minE为最小欧式距离。

2.3 NICP

NICP与传统ICP的不同之处在于NICP会多考虑曲率和法线的方向一致问题,如果对应点的曲率和法线方向超过设定阈值,不会建立对应点的匹配。所以在计算的时候需要计算点云的法线信息,然后进行匹配时需要额外对对应点对的法线和曲率限定阈值。

2.4 LM_ICP

LM_ICP在计算最小距离的时候采用LM模型来进行,算法原理可以查看论文:
结合Kinect的双目视觉场景三维重建。

三、程序示例

1. 传统方法

传统方法计算全部点云的对应关系导致计算时间非常长。

icp.setInputSource(source); 
icp.setInputTarget(target);
icp.setTransformationEpsilon(1e-8);
icp.setMaxCorrespondenceDistance(1); //添加一个最大距离的阈值,超过最大距离的点不作为对应点。
icp.setEuclideanFitnessEpsilon(0.00005);
icp.setMaximumIterations(150);
icp.setUseReciprocalCorrespondences(true);

迭代1次:
在这里插入图片描述
迭代134次:
在这里插入图片描述

2. PointToPlane ICP

PointToPlane ICP的核心类是IterativeClosestPointWithNormals,默认情况下,此实现使用传统的点对面目标,并使用目标点云的法线计算点对面距离。
另外在计算法线的时候采用OpenMP来进行多线程加速。

pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal>ptoplane_icp;ptoplane_icp.setInputSource(source_with_normals);
ptoplane_icp.setInputTarget(target_with_normals);
ptoplane_icp.setTransformationEpsilon(1e-8);
ptoplane_icp.setMaxCorrespondenceDistance(1);
ptoplane_icp.setEuclideanFitnessEpsilon(0.0001); 
ptoplane_icp.setMaximumIterations(150); 

在这里插入图片描述
在这里插入图片描述
可见只进行了7次迭代,用时1.6s.


总结

ICP算法功能强大,算法种类也很多,在实际使用时需要根据实际应用场景开发适合的ICP自适应算法。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/256077.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【数据结构】——排序算法系列总结

目录 1、空间复杂度 2、稳定性 3、运行时间 4、目前默认的sort内置函数排序函数 5、六种常用排序方法 1、空间复杂度 空间复杂度产生的原因有两个&#xff1a;①重新定义了一块空间用于存储数据&#xff1b;②递归产生了栈空间 冒泡排序、选择排序、堆排序和插入排序属于…

Spring Boot实践教程(二):SpringApplication分析

2019独角兽企业重金招聘Python工程师标准>>> 本文会通过分析上一篇中跑起来的示例程序来分析一下Spring Boot程序运行的基本原理。 概要 在上一篇的介绍中&#xff0c;程序是通过SpringBoot1HelloworldApplication.main()方法运行起来的&#xff1a; public static …

基于PCL的MLS(移动最小二乘)算法简介与示例

一、MLS基础 mls算法本质上和最小二乘一样&#xff0c;是一种拟合数据的算法。区别在于mls是局部的&#xff0c;即通过系数向量和基函数分别对数据中不同位置的节点区域进行拟合&#xff0c;需要计算出全部节点域的拟合函数的参数。而传统的最小二乘是全局的&#xff0c;采用所…

基于PCL的RANSAC(随机采样一致)算法简介与示例

前言 RANSAC&#xff08;Random sample consensus&#xff0c;随机采样一致&#xff09;是3D点云拟合的一种重要的手段&#xff0c;可以对直线、圆、平面&#xff0c;圆球、圆柱等形状的点云进行拟合&#xff0c;其优点在于可以最大程度上减少噪声点对拟合效果的影响。 一、RA…

Testin云測与ARM 战略合作:推动全球移动应用加速进入中国市场

Testin云測与ARM 战略合作&#xff1a;推动全球移动应用加速进入中国市场 2014/10/14 Testin 业界资讯&#xff08;中国北京–2014年10月14日 &#xff09;全球最大的移动游戏、应用真机和用户云測试平台Testin云測今日宣布与ARM建立战略伙伴合作关系&#xff0c;设立“ARM应…

正则表达式快速入门,转载

正则表达式快速入门 首先简单介绍下正则表达式&#xff1a; 在编写处理字符串的程序或网页时&#xff0c;经常会有查找符合某些复杂规则的字符串的需要。正则表达式就是用于描述这些规则的工具。换句话说&#xff0c;正则表达式就是记录文本规则的代码。 下面就看看正则表达式里…

【pyqt5】配置Qt Designer之【designer.exe的保存位置及ui文件转py文件及no Qt platform plugin could be initialized 问题解决】

目录 一、寻找designer.exe 二、no Qt platform plugin could be initialized 问题解决 三、ui文件转换为py文件 四、pyqt5的使用教程 一、寻找designer.exe 头疼&#xff0c;找了一上午都没有找到这个的路径&#xff0c;最后还是在评论区看到的&#xff0c;这也不能怪人家…

PCL中GreedyProjection三角化算法简介与示例

文章目录前言一、PCL点云三角化1.1 Delaunay三角剖分1.2 贪婪三角化二、程序示例总结前言 Delaunay三角剖分最初应用于2维领域&#xff0c;而与Greedy三角化算法的结合&#xff0c;使之成为目前在三维重建领域最为基础的算法原理之一&#xff0c;很多学者针对其原理进行改进用…

【pyqt5】 读取numpy arrray 显示图片

目录 1、GUI界面&#xff08;QT designer设计&#xff09; 2、逻辑函数&#xff08;回调等&#xff09; 3、显示图片在label上 0&#xff09;直接利用QPixmap显示图像 1&#xff09;显示彩色图 彩色图显示色调不正常——opencv&#xff08;BGR&#xff09;QT(RGB)需要进行…

【pyqt5】——入门级模板(ui文件+ui转py文件+逻辑py文件)(消息提示框)

目录 1、ui文件 2、ui转py文件 3、逻辑py文件 4、实例 1&#xff09;ui文件——demo.ui 2&#xff09;ui转py文件——demo.py 3)逻辑py文件——demoLogic.py 4)运行结果 1、ui文件 这个文件是直接通过pyqt5 designer进行设计的&#xff0c;相关配置可见《配置Qt Design…

PCL中点特征描述子PFH、FPFH和VFH简述和示例

文章目录前言一、点特征直方图1.1 PFH1.1.1 法线估计1.1.2 特征计算1.2 FPFH1.3 VFH二、示例2.1 PFH计算2.2 FPFH2.3 VFH前言 点特征直方图是PCL中非常重要的特征描述子&#xff0c;在点云匹配、分割、重建等任务中起到关键作用&#xff0c;可以对刚体变换、点云密度和噪声均有…

来一个炫酷的导航条

本文分享一个带动画效果的中英文切换导航条。 鼠标放上去试一下&#xff1a; INDEX 首页 BBS 社区 HOME 我 1.用CSS3实现 效果看上去复杂&#xff0c;其实我们先来做出一个样式&#xff0c;就很简单了。如下&#xff1a; 代码&#xff1a; <nav><ul class"list…

EXCEL小技巧:如何统计非空单元格

http://club.excelhome.net/thread-1187271-1-1.html 下面教大家如果用函数统计非空单元格的数量 首先我们来介绍几个统计函数&#xff1a; 1.COUNT(value1,value2,...) 统计包含数字的单元格个数 2.COUNTA(value1,value2,...) 统计非空单元格的个数 3.COUNTBLANK(range&…

未在本地计算机上注册“Microsoft.Jet.OLEDB.4.0”提供程序。

报错信息&#xff1a; 解决方案&#xff1a; 1、“设置应用程序池默认属性”/“常规”/”启用32位应用程序”&#xff0c;设置为 true。 如下图所示&#xff1a;&#xff08;已测试&#xff0c;好使&#xff09; 方法二&#xff1a;生成->配置管理器->平台->点击Any C…

008. 限制上传文件的大小

第一种方法: 利用web.config的配置文件项, 进行设置; 前端aspx示例: <% Page Language"C#" AutoEventWireup"true" CodeFile"sendOutEmail.aspx.cs" Inherits"sendOutEmail" %> <!DOCTYPE html PUBLIC "-//W3C//DTD XHT…

浙江中医药大学第十一届程序设计竞赛题解

官方题解&#xff1a;http://www.jnxxhzz.com/Article/article/9.html 2019: 特产 Time Limit: 1 Sec Memory Limit: 128 MBSubmit: 548 Solved: 154[Submit][Status][Web Board]Description Input Output 输出一个整数表示dd带回来的特产重量 Sample Input 2 3 6 1 3Sample …

【项目实战】——USB双路继电器电脑控制灯的开关(Python)

环境&#xff1a;window10、Python3.7.9 依赖库&#xff1a;pyserial 硬件&#xff1a;220V灯带、220V吊灯、USB双路继电器、电笔 1、安装Python第三方库pyserial 2、清楚插座的零火线&#xff08;用电笔去测试&#xff0c;灯亮为火线&#xff09; 3、清楚灯的零火线&#…

字符串去掉空格

2019独角兽企业重金招聘Python工程师标准>>> String s1s.trim().replaceAll("\\s*", ""); 转载于:https://my.oschina.net/u/2842177/blog/1587850

基于CMake构建MSVC_CUDA及MinGW编译环境下的的OpenCV项目

前言 第一次搭建OpenCV开发环境的时候各种报错&#xff0c;内心那个烦啊&#xff0c;简直了。当时只能针对某个特定的错误去寻找特定的解决方法&#xff0c;在OpenCV构建过程中出现最多的问题就是各个模块文件的下载问题&#xff0c;本质上这类问题的解决思路都是一样的&#…

【opencv】——钢管计数(霍夫圆变换 + 阈值 + canny)

目录 方法一:霍夫圆变换 + canny 方法二 阈值 + 寻边 对图中的钢管进行计数 方法一:霍夫圆变换 + canny