【PCL】教程 example2 3D点云之间的精确配准(FPFH特征对应关系估计变换矩阵)

这段代码主要实现了点云之间的配准功能,旨在通过估计点云的特征并找到最佳的对应关系来计算一个变换矩阵,从而可以将源点云(src)变换到目标点云(tgt)的坐标系统中。

代码功能和方法总结如下:

  1. 估计关键点(estimateKeypoints:使用pcl::UniformSampling过滤器从原始点云中提取均匀分布的关键点。关键点以1米的半径均匀采样后保存到磁盘上,以便于调试。

  2. 估计法线(estimateNormals:使用pcl::NormalEstimation为每个关键点估计法线。法线提取基于0.5米的搜索半径后,结果保存到磁盘上,以便调试。

  3. 估计FPFH特征(estimateFPFH:使用pcl::FPFHEstimation为每个关键点计算FPFH特征。特征估计在1米的搜索半径内完成,计算结果保存到磁盘上用于调试。

  4. 寻找对应关系(findCorrespondences:使用pcl::CorrespondenceEstimation计算两个点云中关键点的FPFH特征间的对应关系。

  5. 拒绝差的对应关系rejectBadCorrespondences):使用pcl::CorrespondenceRejectorDistance根据空间距离来拒绝不良的对应关系。设定最大距离为1米。

  6. 计算变换矩阵computeTransformation):使用pcl::TransformationEstimationSVD 基于剩余的良好对应关系来估计源点云到目标点云的刚性变换。

  7. 主程序(main):作为程序的入口,首先解析命令行参数加载.PCD文件,加载源点云和目标点云数据。然后调用computeTransformation计算最佳变换矩阵。最后,将源点云数据根据计算出的变换矩阵变换后保存到硬盘上。

综上所述,本段代码实现了3D点云之间的精确配准,包括了关键点提取、法线估计、特征计算、对应关系寻找、对应关系过滤和最后的变换矩阵估计等一系列步骤。这种点云配准在3D建模、环境映射、物体检测等领域有着重要的应用。

#include <pcl/console/parse.h> // 包含PCL库中处理命令行参数解析的功能
#include <pcl/point_types.h> // 包含定义了PCL支持的点类型的功能
#include <pcl/point_cloud.h> // 包含点云类的定义
#include <pcl/point_representation.h> // 包含点表示(特征)的定义#include <pcl/io/pcd_io.h> // 包含PCD文件输入输出的功能
#include <pcl/conversions.h> // 包含点云类型转换的功能
#include <pcl/filters/uniform_sampling.h> // 包含均匀采样的滤波器
#include <pcl/features/normal_3d.h> // 包含计算点云中每个点的法线的功能
#include <pcl/features/fpfh.h> // 包含计算FPFH特征的功能
#include <pcl/registration/correspondence_estimation.h> // 包含估算对应关系的功能
#include <pcl/registration/correspondence_rejection_distance.h> // 包含基于距离的对应关系拒绝功能
#include <pcl/registration/transformation_estimation_svd.h> // 包含使用SVD(单因素分解)方法估算变换矩阵的功能using namespace pcl; // 使用 PCL 命名空间
using namespace pcl::io; // 使用 PCL 的 IO 命名空间
using namespace pcl::console; // 使用 PCL 的控制台命名空间
using namespace pcl::registration; // 使用 PCL 的注册命名空间
PointCloud<PointXYZ>::Ptr src, tgt; // 定义源点云和目标点云的指针// 以下为函数定义:// 估算关键点
void
estimateKeypoints (const PointCloud<PointXYZ>::Ptr &src, const PointCloud<PointXYZ>::Ptr &tgt,PointCloud<PointXYZ> &keypoints_src,PointCloud<PointXYZ> &keypoints_tgt)
{// 获取一个均匀的关键点网格UniformSampling<PointXYZ> uniform; // 创建均匀采样的实例uniform.setRadiusSearch (1);  // 设置搜索半径为1米uniform.setInputCloud (src); // 设置输入的源点云uniform.filter (keypoints_src); // 进行滤波,并保留结果到keypoints_srcuniform.setInputCloud (tgt); // 设置输入的目标点云uniform.filter (keypoints_tgt); // 进行滤波,并保留结果到keypoints_tgt// 以下为调试目的,可将结果保存到PCD文件并在pcl_viewer中查看savePCDFileBinary ("keypoints_src.pcd", keypoints_src); // 保存源关键点到文件savePCDFileBinary ("keypoints_tgt.pcd", keypoints_tgt); // 保存目标关键点到文件
}// 估算法线
void
estimateNormals (const PointCloud<PointXYZ>::Ptr &src, const PointCloud<PointXYZ>::Ptr &tgt,PointCloud<Normal> &normals_src,PointCloud<Normal> &normals_tgt)
{NormalEstimation<PointXYZ, Normal> normal_est; // 创建法线估算实例normal_est.setInputCloud (src); // 设置输入的源点云normal_est.setRadiusSearch (0.5);  // 设置搜索半径为50厘米normal_est.compute (normals_src); // 计算结果保留在normals_src中normal_est.setInputCloud (tgt); // 设置输入的目标点云normal_est.compute (normals_tgt); // 计算结果保留在normals_tgt中// 以下为调试目的,可将结果保存到PCD文件并在pcl_viewer中查看PointCloud<PointNormal> s, t;copyPointCloud (*src, s); // 拷贝点到scopyPointCloud (normals_src, s); // 拷贝法线到scopyPointCloud (*tgt, t); // 拷贝点到tcopyPointCloud (normals_tgt, t); // 拷贝法线到tsavePCDFileBinary ("normals_src.pcd", s); // 保存源点云的法线到文件savePCDFileBinary ("normals_tgt.pcd", t); // 保存目标点云的法线到文件
}void
computeTransformation (const PointCloud<PointXYZ>::Ptr &src,const PointCloud<PointXYZ>::Ptr &tgt,Eigen::Matrix4f &transform
)
{// 获取均匀分布的关键点PointCloud<PointXYZ>::Ptr keypoints_src(new PointCloud<PointXYZ>),keypoints_tgt(new PointCloud<PointXYZ>);estimateKeypoints(src, tgt, *keypoints_src, *keypoints_tgt); // 调用 estimateKeypoints 方法估计关键点print_info("Found %zu and %zu keypoints for the source and target datasets.\n", static_cast<std::size_t>(keypoints_src->size()),static_cast<std::size_t>(keypoints_tgt->size())); // 打印信息,输出找到的关键点数量// 计算所有关键点的法线PointCloud<Normal>::Ptr normals_src (new PointCloud<Normal>),normals_tgt(new PointCloud<Normal>);estimateNormals(src, tgt, *normals_src, *normals_tgt); // 调用 estimateNormals 方法计算法线print_info("Estimated %zu and %zu normals for the source and target datasets.\n",static_cast<std::size_t>(normals_src->size()),static_cast<std::size_t>(normals_tgt->size())); // 打印信息,输出计算得到的法线数量// 计算每个关键点的 FPFH 特征PointCloud<FPFHSignature33>::Ptr fpfhs_src(new PointCloud<FPFHSignature33>), fpfhs_tgt(new PointCloud<FPFHSignature33>);estimateFPFH(src, tgt, normals_src, normals_tgt, keypoints_src, keypoints_tgt, *fpfhs_src, *fpfhs_tgt); // 调用 estimateFPFH 方法计算 FPFH 特征// 查找 FPFH 空间中关键点的对应关系CorrespondencesPtr all_correspondences(new Correspondences),good_correspondences(new Correspondences);findCorrespondences(fpfhs_src, fpfhs_tgt, *all_correspondences); // 调用 findCorrespondences 方法找到所有对应关系// 根据它们的 XYZ 距离拒绝错误的对应关系rejectBadCorrespondences(all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences); // 调用 rejectBadCorrespondences 方法拒绝错误的对应关系for (const auto& corr : (*good_correspondences))std::cerr << corr << std::endl; // 对于每一个剩余的好的对应关系,输出到 cerr// 获得给定剩余对应关系后,两组关键点之间的最佳变换TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est; // 定义变换估计对象trans_est.estimateRigidTransformation(*keypoints_src, *keypoints_tgt, *good_correspondences, transform); // 估算刚性变换矩阵
}int
main (int argc, char** argv)
{// 解析命令行参数以查找 .pcd 文件std::vector<int> p_file_indices; // 定义一个整数向量用于存储文件的索引p_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); // 解析获得所有后缀为 .pcd 的文件索引if (p_file_indices.size () != 2) // 如果没有找到两个 .pcd 文件,则报错{print_error ("Need one input source PCD file and one input target PCD file to continue.\n"); // 打印错误信息print_error ("Example: %s source.pcd target.pcd\n", argv[0]); // 提供正确使用的例子return (-1); // 返回错误码 -1}// 加载文件print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]); // 打印加载信息src.reset (new PointCloud<PointXYZ>); // 初始化源点云tgt.reset (new PointCloud<PointXYZ>); // 初始化目标点云if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1) // 尝试加载文件,如果失败则报错{print_error ("Error reading the input files!\n"); // 打印错误信息return (-1); // 返回错误码 -1}// 计算最佳变换Eigen::Matrix4f transform; // 定义一个 4x4 的变换矩阵computeTransformation (src, tgt, transform); // 调用 computeTransformation 函数计算从源点云到目标点云的变换矩阵std::cerr << transform << std::endl; // 输出变换矩阵// 对数据进行变换并将结果写入磁盘PointCloud<PointXYZ> output; // 定义输出点云transformPointCloud (*src, output, transform); // 使用计算得到的变换矩阵对源点云进行变换savePCDFileBinary ("source_transformed.pcd", output); // 保存变换后的点云到文件
}

此代码是使用点云库(PCL)进行点云注册的示例程序。程序首先加载两个点云文件,然后计算从源点云到目标点云的最佳变换矩阵,并将变换后的源点云保存到新的文件中。这个过程可以应用于多种场景,如 3D 模型重建、环境映射与导航。

 computeTransformation 函数,用于计算从源点云向目标点云变换的最佳矩阵。函数首先估计两组点云的均匀关键点,然后计算关键点的法线,接下来计算关键点的 FPFH 特征。之后,找到两组关键点在 FPFH 特征空间中的对应关系,并拒绝那些距离较远的错误对应关系。最后,根据剩余的正确对应关系,利用 SVD 方法计算得到最佳的变换矩阵。

TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);

TransformationEstimationSVD<PointXYZ, PointXYZ>是一个类,用于估计两组点云间的刚体变换。其中,PointXYZ是PCL库中定义的一种点类型,包含了点的XYZ坐标。

刚体变换是指在变换过程中保持物体形状和大小不变,只进行旋转和平移。在点云处理中,刚体变换通常用于将一组点云数据精确地对齐到另一组点云数据中,这个过程就称为点云配准。

TransformationEstimationSVD内部实现了奇异值分解(Singular Value Decomposition, SVD)方法来计算最优化的刚体变换,即寻找一个最佳的旋转矩阵和平移向量,使得在这个变换下,一组点云与另一组点云之间的对应点尽可能地接近。

a4fbc7186b43de3819cdd7c262b5cd0a.png

CorrespondenceRejectorDistance rej;

259cb548e293043a3cb96ad5aed29a9a.png

CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;

8e9693f48acd03072ea95cc6bfe91a27.png

FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;

f9a5e8f65922e47eb9781ab43a003d45.png

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

如若内容造成侵权/违法违规/事实不符,请联系一条长河网进行投诉反馈,一经查实,立即删除!

相关文章

上位机开发PyQt5(二)【单行输入框、多行输入框、按钮的信号和槽】

目录 一、单行输入框QLineEdit QLineEdit的方法&#xff1a; 二、多行输入框QTextEdit QTextEdit的方法 三、按钮QPushButton 四、按钮的信号与槽 信号与槽简介&#xff1a; 信号和槽绑定&#xff1a; 使用PyQt的槽函数 一、单行输入框QLineEdit QLineEdit控件可以输入…

黑马点评项目个人笔记+项目优化调整

博客须知 本篇博客内容来源与黑马点评项目实战篇-16.用户签到-实现签到功能_哔哩哔哩_bilibili&#xff0c;作者对视频内容进行了整合&#xff0c;由于记笔记时图片使用的是本地路径&#xff0c;所以导致博客的图片无法正常显示&#xff0c;如果有图片需求可以下载上方的pdf须…

程序员老鸟的 Pascal 语言菜鸟教程 -- 快速体验 Pascal

有些程序设计语言和编译器教材会以pascal语言的程序为例&#xff0c;这里写一个快速掌握简单应用的介绍。 1&#xff0c;安装 free pascal 编译器 ubuntu 22.04 直接通过 apt 源安装&#xff0c;此时的版本号为 3.2.2 1.1 安装 sudo apt install fp-compiler 1.2 简单测试 fpc…

【maven】pom文件详解和延伸知识

【maven】pom文件详解 【一】maven项目的pom文件详解【1】maven项目的目录结构【2】根元素和必要配置【3】父项目和parent元素【4】项目构建需要的信息【5】项目依赖相关信息&#xff08;1&#xff09;依赖坐标&#xff08;2&#xff09;依赖类型&#xff08;3&#xff09;依赖…

JavaScript this 上下文深度探索:综合指南涵盖隐式与显式call、apply、bind、箭头函数、构造函数等用法于多样场景

JavaScript中的this关键字代表函数执行的上下文环境&#xff0c;核心在于确定函数内部访问的当前对象。它根据函数调用方式动态变化&#xff0c;对事件处理、对象方法调用等至关重要。通过.call(), .apply(), .bind()或箭头函数控制this&#xff0c;可确保代码逻辑正确绑定对象…

python可视化学习笔记折线图问题-起始点问题

问题描述&#xff1a; 起始点的位置不对 from pyecharts.charts import Line import pyecharts.options as opts # 示例数据 x_data [1,2,3,4,5] y_data [1, 2, 3, 4, 5] # 创建 Line 图表 line Line() line.add_xaxis(x_data) line.add_yaxis("test", y_data) li…

Redis---------缓存更新,缓存穿透\雪崩\击穿

三种更新策略 内存淘汰是Redis内存的自动操作&#xff0c;当内存快满了就会触发内存淘汰。超时剔除则是在存储Redis时加上其有限期(expire)&#xff0c;有限期一过就会自动删除掉。而主动更新则是自己编写代码去保持更新&#xff0c;所以接下来研究主动更新策略。 主动更新策略…

PS入门|网络报名证件照上传总提示审核失败是什么原因?

前言 之前小白遇到过有小伙伴报考了某个证书的考试&#xff0c;但在报名的过程出现了问题&#xff1a;证件照都是按照要求制作的&#xff0c;但为啥总是没有审核通过&#xff1f; 这个很简单&#xff1a;分辨率出现了问题。 啥&#xff1f;明明都是按照软件提示的分辨率要求制…

Python中的观察者模式及其应用

观察者模式是设计模式之一&#xff0c;实现一对多依赖&#xff0c;当主题状态变化时通知所有观察者更新。在Python中&#xff0c;通过自定义接口或内置模块实现观察者模式&#xff0c;可提高程序灵活性和扩展性&#xff0c;尤其适用于状态变化时触发操作的场景&#xff0c;如事…

Linux(ubuntu)—— 用户管理user 用户组group

一、用户 1.1、查看所有用户 cat /etc/passwd 1.2、新增用户 useradd 命令&#xff0c;我这里用的是2.4的命令。 然后&#xff0c;需要设置密码 passwd student 只有root用户才能用passwd命令设置其他用户的密码&#xff0c;普通用户只能够设置自己的密码 二、组 2.1查看…

【右一的开发日记】全导航,持续更新...

文章目录 &#x1f4da;前端【跟课笔记】&#x1f407;核心技术&#x1f407;高级技术 &#x1f4da;捣鼓捣鼓&#x1f407;小小案例&#x1f407;喵喵大王立大功&#x1f407;TED自用学习辅助网站&#x1f407;世界top2000计算机科学家可视化大屏&#x1f407;基于CBDB的唐代历…

中间件之异步通讯组件RabbitMQ入门

一、概述 微服务一旦拆分&#xff0c;必然涉及到服务之间的相互调用&#xff0c;目前我们服务之间调用采用的都是基于OpenFeign的调用。这种调用中&#xff0c;调用者发起请求后需要等待服务提供者执行业务返回结果后&#xff0c;才能继续执行后面的业务。也就是说调用者在调用…

HTTP/1.1、HTTP/2、HTTP/3 的演变

HTTP/1.1、HTTP/2、HTTP/3 的演变 HTTP/1.1 相比 HTTP/1.0 提高了什么性能&#xff1f;HTTP/2 做了什么优化&#xff1f;HTTP/3 做了哪些优化&#xff1f; HTTP/1.1 相比 HTTP/1.0 提高了什么性能&#xff1f; HTTP/1.1 相比 HTTP/1.0 性能上的改进&#xff1a; 使用长连接的…

分拣机器人也这么卷了吗?!

导语 大家好&#xff0c;我是智能仓储物流技术研习社的社长&#xff0c;老K。专注分享智能仓储物流技术、智能制造等内容。 新书《智能物流系统构成与技术实践》 智能制造-话题精读 1、西门子、ABB、汇川&#xff1a;2024中国工业数字化自动化50强 2、完整拆解&#xff1a;智能…

4月20日,杭州Sui Meetup活动回顾

4 月 20 日在风景如画的杭州&#xff0c;「TinTin DESTINATION MOON」成功举办。此次活动深入探讨了 Sui 生态系统的演进及未来机遇&#xff0c;包括 Sui 上的资产管理协议 Mole、全链引擎 Obelisk Engine 以及 Generator 的开发范式等热点话题&#xff0c;行业专家提供了深刻见…

基于Spring Boot的校园闲置物品交易网站设计与实现

基于Spring Boot的校园闲置物品交易网站设计与实现 开发语言&#xff1a;Java框架&#xff1a;springbootJDK版本&#xff1a;JDK1.8数据库工具&#xff1a;Navicat11开发软件&#xff1a;eclipse/myeclipse/idea 系统部分展示 系统功能界面图&#xff0c;在系统首页可以查看…

修改 Windows 服务器远程端口,以阿里云服务器 ECS 为例

一、WinR - mstsc.exe 登录远程服务器 二、WinR - regedit.exe 打开注册表 三、打开注册表 tcp 路径&#xff1a; 计算机\HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Control\TerminalServer\Wds\rdpwd\Tds\tcp 双击右侧 PortNumber 子健&#xff0c;勾选十进制&a…

基于SpringBoot的私人健身与教练预约管理系统设计与实现

一、引言 私人健身与教练预约管理系统&#xff0c;可以摆脱传统手写记录的管理模式。利用计算机系统&#xff0c;进行用户信息、管理员信息的管理&#xff0c;其中包含首页&#xff0c;个人中心&#xff0c;用户管理&#xff0c;教练管理&#xff0c;健身项目管理&#xff0c;项…

GitLab服务器的搭建

GitLab服务器的搭建 为公司搭建一台代码托管服务器 服务器规格&#xff1a;2vCPUs4GiB20G 操作系统&#xff1a;RockyLinux8.8 下载软件 gitlab官网&#xff1a;http://about.gitlab.com 在官网下载比较麻烦&#xff0c;推荐从《清华大学开源软件镜像站》下载 清华大学开…

微服务保护和分布式事务(Sentinel、Seata)笔记

一、雪崩问题的解决的服务保护技术了解 二、Sentinel 2.1Sentinel入门 1.Sentinel的安装 &#xff08;1&#xff09;下载Sentinel的tar安装包先 &#xff08;2&#xff09;将jar包放在任意非中文、不包含特殊字符的目录下&#xff0c;重命名为 sentinel-dashboard.jar &…