【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.mzph.cn/pingmian/5707.shtml

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

相关文章

SEO之高级搜索指令(一)

初创企业需要建站的朋友看这篇文章&#xff0c;谢谢支持&#xff1a; 我给不会敲代码又想搭建网站的人建议 新手上云 用户除了可以在搜索引擎搜索普通关键词外&#xff0c;还可以使用一些特殊的高级搜索指令&#xff0c; 这些搜索指令普通用户很少会用到&#xff0c;对SEO人员…

贝叶斯回归

1. 贝叶斯推断的定义 简单来说&#xff0c;贝叶斯推断 (Bayesian inference) 就是结合“经验 (先验)”和“实践 (样本)”&#xff0c;得出“结论 (后 验)”。 2. 什么是先验&#xff1f; 贝叶斯推断把模型参数看作随机变量。在得到样本之前&#xff0c;根据主观经验和既有知…

翻译: 什么是ChatGPT 通过图形化的方式来理解 Transformer 架构 深度学习四

合集 ChatGPT 通过图形化的方式来理解 Transformer 架构 翻译: 什么是ChatGPT 通过图形化的方式来理解 Transformer 架构 深度学习一翻译: 什么是ChatGPT 通过图形化的方式来理解 Transformer 架构 深度学习二翻译: 什么是ChatGPT 通过图形化的方式来理解 Transformer 架构 深…

如何讲好ppt演讲技巧(4篇)

如何讲好ppt演讲技巧&#xff08;4篇&#xff09; 如何讲好PPT演讲技巧&#xff08;四篇&#xff09; **篇&#xff1a;精心准备&#xff0c;奠定演讲基础 一个成功的PPT演讲&#xff0c;离不开精心的准备。首先&#xff0c;要确定演讲的主题和目标&#xff0c;确保演讲内容清…

时序分析之setup/hold时钟path requirement解疑

目录 一、前言 二、时钟边沿选取 2.1 setup和hold 2.2 path requirement 2.3 同频率同相位 2.4 同频率不同相 2.5 倍频或分频关系 2.5 异步时钟 2.6 无公共周期 一、前言 在时序报告中&#xff0c;任选一条时序分析结果查看路径详细延时&#xff0c;会看到在source pa…

Vue3管理系统-路由设置+表单校验

一、配置路由规则 1.在views 下创建文件夹分类,搭好架子 2.配置路由规则 在router下Index.js import { createRouter, createWebHistory } from vue-routerconst router createRouter({history: createWebHistory(import.meta.env.BASE_URL),routes: [//一级路由//这里可以…

浅谈 HTTPS

文章目录 HTTPS 简介HTTPS 特点与 HTTP 的区别HTTPS 工作流程1. 服务端生成密钥对2. 服务端申请数字证书3. 服务端发送数字证书4. 客户端验证数字证书5. 客户端解析证书内容6. 客户端传送加密信息7. 服务端解密信息8. 双方协商生成会话密钥并交换9. 使用会话密钥进行通信 总结 …

【Java】对象的实例化过程

0、前情 对于经常写代码的同学有没有思考这样一个问题&#xff1a;为什么成员变量有默认值&#xff1f;为什么局部变量必须手动赋值&#xff1f; 先不考虑变量类型&#xff0c;如果没有默认值会怎么样&#xff1f;变量存储的是内存地址对应的任意随机值&#xff0c;如果不对其…

Dom获取属性操作

目录 1. 基本认知 1.1 目的和内容 1.2 什么是DOM 1.3 DOM对象 1.4 DOM树 2. 获取DOM元素对象 2.1 选择匹配到的第一个元素 2.2 选择匹配到的多个元素 2.3 其他获取DOM元素方法 3. 操作元素内容 3.1 元素对象.innerText 属性 3.2 元素对象.innerHTML 属性 4. 操作元…

缓存分享(1)——Guava Cache原理及最佳实践

Guava Cache原理及最佳实践 1. Guava Cache是什么1.1 简介1.2 核心功能1.3 适用场景 2. Guava Cache的使用2.1 创建LoadingCache缓存2.2 创建CallableCache缓存 缓存的种类有很多&#xff0c;需要根据不同的应用场景来选择不同的cache&#xff0c;比如分布式缓存如redis、memca…

设计模式之装饰者模式DecoratorPattern(四)

一、概述 装饰者模式&#xff08;Decorator Pattern&#xff09;是一种用于动态地给一个对象添加一些额外的职责的设计模式。就增加功能来说&#xff0c;装饰者模式相比生成子类更为灵活。装饰者模式是一种对象结构型模式。 装饰者模式可以在不改变一个对象本身功能的基础上增…

linux dma的使用

设备树配置 驱动代码 static void bcm2835_dma_init(struct spi_master *master, struct device *dev) { struct dma_slave_config slave_config; const __be32 *addr; dma_addr_t dma_reg_base; int ret; /* base address in dma-space */ addr of_get_address(master->de…

基于 React 的图形验证码插件

react-captcha-code NPM 地址 &#xff1a; react-captcha-code - npm npm install react-captcha-code --save 如下我自己的封装&#xff1a; import Captcha from "react-captcha-code";type CaptchaType {captchaChange: (captchaInfo: string) > void;code…

目前全球各类遥感卫星详细介绍

一、高分一号 高分一号&#xff08;GF-1&#xff09;是中国高分辨率对地观测系统重大专项&#xff08;简称高分专项&#xff09;的第一颗卫星。“高分专项”于2010年5月全面启动&#xff0c;计划到2020年建成中国自主的陆地、大气和海洋观测系统。 高分一号&#xff08;GF-1&…

React的useEffect

概念&#xff1a;useEffect是一个React Hook函数&#xff0c;组件渲染之后执行的函数 参数1是一个函数&#xff0c;可以把它叫做副作用函数&#xff0c;在函数内部可以放置要执行的操作参数2是一个数组&#xff08;可选参&#xff09;&#xff0c;在数组里放置依赖项&#x…

Linux系统中搭建Mosquitto MQTT服务并实现远程访问本地消息代理进行通信

文章目录 1. Linux 搭建 Mosquitto2. Linux 安装Cpolar3. 创建MQTT服务公网连接地址4. 客户端远程连接MQTT服务5. 代码调用MQTT服务6. 固定连接TCP公网地址7. 固定地址连接测试 今天和大家分享一下如何在Linux系统中搭建Mosquitto MQTT协议消息服务端,并结合Cpolar内网穿透工具…

使用 Python 和 OpenCV 进行实时目标检测的详解

使用到的模型文件我已经上传了&#xff0c;但是不知道能否通过审核&#xff0c;无法通过审核的话&#xff0c;就只能 靠大家自己发挥实力了&#xff0c;^_^ 目录 简介 代码介绍 代码拆解讲解 1.首先&#xff0c;让我们导入需要用到的库&#xff1a; 2.然后&#xff0c;设…

【C语言】指针篇- 深度解析Sizeof和Strlen:热门面试题探究(5/5)

&#x1f308;个人主页&#xff1a;是店小二呀 &#x1f308;C语言笔记专栏&#xff1a;C语言笔记 &#x1f308;C笔记专栏&#xff1a; C笔记 &#x1f308;喜欢的诗句:无人扶我青云志 我自踏雪至山巅 文章目录 一、简单介绍Sizeof和Strlen1.1 Sizeof1.2 Strlen函数1.3 Sie…

快速建站介绍

随着在线业务和电子商务的规模不断扩大&#xff0c;初创公司、个人网站和小型企业都需要快速地搭建自己的网站&#xff0c;以便更好地展示自己、推广产品和服务&#xff0c;并实现在线交易。快速建站已成为在线业务发展的一种主流方式&#xff0c;因为它能够快速地创建一个响应…

uniapp 自定义 App启动图

由于uniapp默认的启动界面太过普通 所以需要自定义个启动图 普通的图片不可以过不了苹果的审核 所以使用storyboard启动图 生成 storyboard 的网站&#xff1a;初雪云-提供一站式App上传发布解决方案