PCL 点云配准-ICP算法（精配准）

news2024/12/25 3:58:30

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 点云加载函数

2.1.2 ICP 配准函数

2.1.3 可视化函数

2.2完整代码

三、实现效果

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

一、概述

        ICP（Iterative Closest Point）算法是一种常用的点云精细配准算法。它通过迭代计算将一组点云变换到另一组点云的最佳位置，最终实现精确的对齐。ICP算法在每次迭代中选择与目标点云最接近的点作为对应点，通过计算最小化源点云和目标点云之间的均方误差（MSE）来进行刚性变换（包括平移和旋转）。

1.1原理

        ICP 算法的核心思想是不断迭代计算源点云和目标点云之间的最佳匹配变换。每次迭代分为以下几步：

  1. 最近点对匹配：为每个源点找到其在目标点云中的最近邻点。
  2. 变换矩阵计算：根据最近邻点对计算最小化误差的刚体变换（平移和旋转）。
  3. 变换应用：将计算出的变换应用到源点云，更新源点云的位置。
  4. 收敛判断：判断收敛条件是否满足，如果满足则停止迭代。

1.2实现步骤

  1. 加载点云：从PCD文件中加载源点云和目标点云。
  2. 初始化 ICP 对象：设置配准参数，如最大迭代次数、收敛条件、最大对应距离等。
  3. 执行 ICP 算法：通过迭代计算源点云到目标点云的刚体变换，并应用该变换。
  4. 结果可视化：通过 PCL 可视化工具显示源点云、目标点云和变换后的点云。

1.3应用场景

  1. 物体识别与定位：在机器人和自动驾驶领域，使用ICP可以精确地将传感器采集的场景点云与已知模型点云进行匹配，来确定物体的姿态。
  2. 3D扫描与重建：将不同视角下获取的点云数据进行拼接，形成完整的三维模型。
  3. 医疗影像处理：在医学领域，用于多模态医学影像之间的配准。

二、代码实现

2.1关键函数

2.1.1 点云加载函数

该函数用于从 PCD 文件中加载点云数据。

pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {
        PCL_ERROR("无法读取点云文件\n");
        return nullptr;
    }
    std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点" << std::endl;
    return cloud;
}

2.1.2 ICP 配准函数

该函数实现了 ICP 算法的核心计算部分，完成点云的精细配准。

pcl::PointCloud<pcl::PointXYZ>::Ptr applyICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(source);                    // 设置源点云
    icp.setInputTarget(target);                    // 设置目标点云
    icp.setTransformationEpsilon(1e-10);           // 为终止条件设置最小转换差异
    icp.setMaxCorrespondenceDistance(1);           // 设置对应点对之间的最大距离
    icp.setEuclideanFitnessEpsilon(0.001);         // 设置均方误差和小于阈值停止迭代
    icp.setMaximumIterations(35);                  // 最大迭代次数
    icp.setUseReciprocalCorrespondences(true);     // 使用相互对应关系

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    icp.align(*aligned_cloud);                     // 进行配准
    std::cout << "ICP 收敛，得分：" << icp.getFitnessScore() << std::endl;
    std::cout << "变换矩阵：" << std::endl << icp.getFinalTransformation() << std::endl;

    return aligned_cloud;
}

2.1.3 可视化函数

该函数用于显示配准前后的点云，目标点云为红色，源点云为蓝色，配准后源点云为绿色

void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP 配准结果"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    // 显示目标点云 (红色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    // 显示源点云 (蓝色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 0, 255);
    viewer->addPointCloud(source, source_color, "source cloud");

    // 显示配准后的源点云 (绿色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned_cloud, 0, 255, 0);
    viewer->addPointCloud(aligned_cloud, aligned_color, "aligned cloud");

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned cloud");

    while (!viewer->wasStopped()) {
        viewer->spinOnce();
    }
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // 用于控制台计算时间

// 加载点云函数
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {
        PCL_ERROR("无法读取点云文件\n");
        return nullptr;
    }
    std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点" << std::endl;
    return cloud;
}

// ICP配准函数
pcl::PointCloud<pcl::PointXYZ>::Ptr applyICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(source);
    icp.setInputTarget(target);
    icp.setTransformationEpsilon(1e-10);
    icp.setMaxCorrespondenceDistance(1);
    icp.setEuclideanFitnessEpsilon(0.001);
    icp.setMaximumIterations(35);
    icp.setUseReciprocalCorrespondences(true);

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    icp.align(*aligned_cloud);

    std::cout << "ICP 收敛，得分：" << icp.getFitnessScore() << std::endl;
    std::cout << "变换矩阵：" << std::endl << icp.getFinalTransformation() << std::endl;

    return aligned_cloud;
}

// 可视化函数
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP 配准结果"));
    viewer->setBackgroundColor(0, 0, 0);

    /*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");*/

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 0, 255);
    viewer->addPointCloud(source, source_color, "source cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned_cloud, 0, 255, 0);
    viewer->addPointCloud(aligned_cloud, aligned_color, "aligned cloud");

    //viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned cloud");

    while (!viewer->wasStopped()) {
        viewer->spinOnce();
    }
}

// 主函数
int main(int argc, char** argv) {
    // 加载点云
    auto source = loadPointCloud("1.pcd");
    auto target = loadPointCloud("2.pcd");

    if (!source || !target) return -1;

    // 执行ICP配准
    auto aligned_cloud = applyICP(source, target);

    // 可视化结果
    visualizePointClouds(source, target, aligned_cloud);

    return 0;
}

三、实现效果

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

股票Tick数据如何获取做量化交易

股票Tick数据如何获取做量化交易

【高频tick数据源】银河金融数据库 【tick高频回测策略】在利用股票Tick数据做量化交易时&#xff0c;一个关键的细节点是“实现高频交易中的市场冲击成本最小化”。细节点&#xff1a;市场冲击成本最小化策略 1. 策略原理市场冲击成本是指大额交易对市场价格造成的影响&#…
阅读更多...
Linux驱动开发——设备树

Linux驱动开发——设备树

文章目录 1 什么是设备树&#xff1f;2 DTS、DTB和DTC3 DTS语法3.1 dtsi头文件3.2 设备节点3.3 标准属性3.4 根节点compatible属性3.5 向节点追加或修改内容 4 创建小型模板设备树5 设备树在系统中的体现6 绑定信息文档7 设备树常用OF操作函数7.1 查找节点的OF函数7.2 查找父/子…
阅读更多...
软考高级系统规划与管理师，都是精华知识点！

软考高级系统规划与管理师，都是精华知识点！

知识点&#xff1a;信息的定义和属性 1、 信息的基本概念 l 信息是客观事物状态和运动特征的一种普遍形式&#xff0c;客观世界中大量地存在、产生和传递着以这些方式表示出来的各种各样的信息。 l 维纳&#xff08;控制论创始人&#xff09;&#xff1a;信息就是信息&#…
阅读更多...
基于BS21芯片方案的SLE星闪模块功能特点

基于BS21芯片方案的SLE星闪模块功能特点

1、E105-BS21系列SLE星闪模块产品简介 E105-BS21系列SLE星闪模块模块是基于星闪协议 1.0 版本的串口转 SLE&#xff08;SparkLink Low Energy&#xff09;星闪模块&#xff0c;具有体积小、功耗低、 传输距离远、传输速度快、抗干扰能力强、低延时等特点&#xff0c;工作在 2…
阅读更多...
R语言机器学习算法实战系列（四）随机森林算法+SHAP值 (Random Forest)

R语言机器学习算法实战系列（四）随机森林算法+SHAP值 (Random Forest)

文章目录 介绍教程下载数据加载R包导入数据数据预处理数据描述特征选择数据切割调节参数构建模型预测测试数据评估模型模型准确性混淆矩阵模型评估指标ROC CurvePRC Curve特征的重要性模型解释保存模型总结系统信息介绍 随机森林是常用的非线性用于构建分类器的算法，它是由数…
阅读更多...
2024年法拍房爆了 1-9月挂牌金额超过5200亿

2024年法拍房爆了 1-9月挂牌金额超过5200亿

据互联网数据统计2024年1-9月全国法拍房挂拍量为494737套&#xff0c;同比增长66.84%&#xff0c;挂牌金额超过5200亿&#xff01; 法拍房暴增的背后是原业主的债务困境&#xff0c;或是房地产风险释放触底&#xff0c;或与规模高达10.61万亿的经营D相关。而对于不良资产行业机…
阅读更多...
算法-利用深度优先搜索求解二叉树路径问题

算法-利用深度优先搜索求解二叉树路径问题

这道题可以用深度优先搜索来写&#xff0c;比如说加入节点左右节点都为空且值等于targetsum则返回true,这里可以使用一个简单的方法来写&#xff0c;就比如说我们每次遍历到一个节点时&#xff0c;用targetsum减去当前节点的值 &#xff0c;这样的话只要遍历到叶子节点值等于ta…
阅读更多...
国际期货收费行情源CTP推送式/期货配资软件开发对接行情源的技术性说明

国际期货收费行情源CTP推送式/期货配资软件开发对接行情源的技术性说明

在现代金融市场中&#xff0c;期货交易因其高风险和高回报特性而备受关注。为了满足期货交易者的需求&#xff0c;开发高效、稳定和安全的期货交易软件变得尤为重要。本文将对国际期货收费行情源CTP推送式及期货配资软件的开发对接行情源的技术细节进行详细说明。 一、CTP&…
阅读更多...
Windows 下 golang 多版本管理

Windows 下 golang 多版本管理

三年前的旧文&#xff0c;最新要切版本&#xff0c;翻了出来&#xff0c;现在依然有用&#xff0c;分享出来~ 当前 golang 的各个版本还有些不兼容的问题&#xff0c;最近遇到 go-micro 框架只能运行在 go1.13~1.14 的版本情况&#xff0c;而我本地 windows 环境安装的 Golang …
阅读更多...
Java中字符串.split分割转List＜String＞判空问题

Java中字符串.split分割转List＜String＞判空问题

第一种分割直接分割&#xff0c;如果cph字符串为空&#xff0c;分割后cphList会>0 List<String> cphList Arrays.asList(cph.split(","));第二种判空后分割&#xff0c;如果cph字符串为空&#xff0c;判空后再分割cphList会0 List<String> cphList…
阅读更多...
面向城市运行“一网统管”的实景三维示范应用

面向城市运行“一网统管”的实景三维示范应用

在新型智慧城市建设的浪潮中&#xff0c;实景三维技术正成为推动城市治理现代化的重要力量。“一网统管”作为城市运行管理的新理念&#xff0c;强调了跨部门协作和数据共享&#xff0c;而实景三维技术为此提供了强有力的支撑。本文将探讨实景三维技术如何赋能“一网统管”&…
阅读更多...
如何用宝塔面板和HYBBS构建高颜值论坛打造个性化社区平台

如何用宝塔面板和HYBBS构建高颜值论坛打造个性化社区平台

文章目录 前言1. HYBBS网站搭建1.1 HYBBS网站安装1.2 HYBBS网站测试1.3. cpolar的安装和注册 2. 本地网页发布2.1.Cpolar临时数据隧道2.2.Cpolar稳定隧道&#xff08;云端设置&#xff09;2.3.Cpolar稳定隧道&#xff08;本地设置&#xff09; 3.公网访问测试总结 前言 本文主…
阅读更多...
centos ping能通但是wget超时-解决

centos ping能通但是wget超时-解决

问题截图&#xff1a; 域名解析地址为IPV6地址&#xff0c;建议您调整IPV4优先级之后&#xff0c;再尝试访问&#xff0c;请参考Linux系统IPv4/IPv6双栈接入优先使用IPv4设置&#xff1a;移动云帮助中心 实操截图&#xff1a;
阅读更多...
卸载Python

卸载Python

1、查看安装框架位置并删除 Sudo rm -rf /Library/Developer/CommandLineTools/Library/Frameworks/Python3.framework/Versions/3.8 2、查看应用并删除 在 /Applications/Python 3.x 看是否存在&#xff0c;如果存在并删除。 3、删除软连接 ls -l /usr/bin/py* 或 ls -…
阅读更多...
5G/4G边缘计算网关的各项功能-天拓四方

5G/4G边缘计算网关的各项功能-天拓四方

随着物联网&#xff08;IoT&#xff09;技术的飞速发展&#xff0c;边缘计算作为其中不可或缺的一环&#xff0c;正逐步成为推动数字化转型的重要力量。而5G/4G边缘计算网关作为边缘计算的关键组成部分&#xff0c;其功能之全面、性能之强大&#xff0c;使其成为连接物联网设备…
阅读更多...
非线性激活pytorch

非线性激活pytorch

**前置知识&#xff1a; 1、 self.sigmoid1Sigmoid() outputself.sigmoid1(input) 2、常见的非线性激活函数&#xff1a; 3、非线性激活的作用&#xff1a; 线性与非线性 线性函数&#xff1a;假设你用直线去描述波浪的形状。无论你怎么改变直线的斜率&#xff0c;结果都是…
阅读更多...
uni-app写的微信小程序如何体积太大如何处理

uni-app写的微信小程序如何体积太大如何处理

方法一&#xff1a;对主包进行分包处理&#xff0c;将使用url: /pages/components/equipment/equipment跳转页面的全部拆分为分包&#xff0c;如url: /pagesS/components/equipment/equipment 在pages.json中添加 "subPackages": [{ "root"…
阅读更多...
【排序】——2.快速排序法（含优化）

【排序】——2.快速排序法（含优化）

快速排序法 递归法 霍尔版本(左右指针法) 1.思路 1、选出一个key&#xff0c;一般是最左边或是最右边的。 2、定义一个begin和一个end&#xff0c;begin从左向右走&#xff0c;end从右向左走。&#xff08;需要注意的是&#xff1a;若选择最左边的数据作为key&#xff0c;则…
阅读更多...
软考（网工）——局域网和城域网

软考（网工）——局域网和城域网

&#x1f550;局域网基础 1️⃣局域网和城域网体系架构 IEEE&#xff08;负责链路层&#xff09; 2️⃣局域网拓扑结构 局域网的主要特征由网络的拓扑结构、所采用的协议类型&#xff0c;以及介质访问控制方法决定。局域网的拓扑结构是指连接网络设备的传输介质的铺设形式&am…
阅读更多...
爬虫逆向学习(十二)：一个案例入门补环境

爬虫逆向学习(十二)：一个案例入门补环境

此分享只用于学习用途&#xff0c;不作商业用途&#xff0c;若有冒犯&#xff0c;请联系处理 反爬前置信息 站点&#xff1a;aHR0cDovLzEyMC4yMTEuMTExLjIwNjo4MDkwL3hqendkdC94anp3ZHQvcGFnZXMvaW5mby9wb2xpY3k 接口&#xff1a;/xjzwdt/rest/xmzInfoDeliveryRest/getInfoDe…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023