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

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云函数

2.1.2 构建KD树函数

2.1.3 KD-ICP配准函数

2.1.4 点云可视化函数

2.2完整代码

三、实现效果

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

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

一、概述

        KD-ICP（基于KD树的ICP）算法 是 ICP（Iterative Closest Point）算法 的一种改进形式，主要通过 KD树（K-Dimensional Tree） 加速最近邻搜索，显著提高了ICP算法的配准效率。KD树的使用使得ICP在处理大规模点云数据时具备更高的性能，因为KD树能够在多维空间中快速找到最近邻点。相比于传统ICP，KD-ICP更适用于实时3D点云处理以及大型点云数据的配准。

1.1原理

        ICP算法通过迭代最近邻点配对来计算两个点云之间的刚体变换。KD-ICP使用KD树加速最近邻搜索，主要流程如下：

  1. 最近邻搜索：使用KD树结构快速查找源点云和目标点云中的最近邻点对。
  2. 刚体变换计算：通过最小化源点云和目标点云最近邻点之间的误差，计算出最优的刚体变换矩阵。
  3. 应用刚体变换：将该变换应用于源点云并更新其位置。
  4. 终止条件：当收敛条件满足时，停止迭代。

1.2实现步骤

  1. 加载源点云和目标点云。
  2. 构建KD树：为源点云和目标点云构建KD树结构，加速最近邻搜索。
  3. 初始化ICP算法：设置ICP的最大迭代次数、距离阈值、转换误差等参数。
  4. 执行KD-ICP配准：通过KD树进行最近邻搜索，计算刚体变换，并更新源点云的位置。
  5. 可视化：展示源点云、目标点云及配准后的源点云

1.3应用场景

  1. 3D物体扫描与拼接：在3D扫描重建过程中，将多个不同角度获取的点云通过KD-ICP配准拼接成一个完整模型。
  2. 机器人视觉：机器人视觉中通过KD-ICP对环境点云数据进行对齐，实现导航和物体定位。
  3. 自动驾驶：在自动驾驶中，KD-ICP可用于车辆环境感知的多传感器数据融合，例如激光雷达点云数据的实时配准。

二、代码实现

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("无法读取点云文件 %s\n", filename.c_str());return nullptr;}std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点\n";return cloud;
}

2.1.2 构建KD树函数

KD树加速最近邻搜索，分别为源点云和目标点云构建KD树

pcl::search::KdTree<pcl::PointXYZ>::Ptr buildKDTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());tree->setInputCloud(cloud);return tree;
}

2.1.3 KD-ICP配准函数

该函数用于执行基于KD树的ICP算法，实现精确的点云配准。

Eigen::Matrix4f performKDICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2) {pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setSearchMethodSource(tree1);icp.setSearchMethodTarget(tree2);icp.setInputSource(source);icp.setInputTarget(target);icp.setMaxCorrespondenceDistance(1);        // 设置对应点之间的最大距离icp.setMaximumIterations(35);               // 设置最大迭代次数icp.setTransformationEpsilon(1e-10);        // 设置收敛条件下的最小变换差异icp.setEuclideanFitnessEpsilon(0.05);       // 设置收敛的均方误差阈值pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);icp.align(*icp_cloud);if (icp.hasConverged()) {std::cout << "ICP 收敛，得分为 " << icp.getFitnessScore() << std::endl;std::cout << "变换矩阵：\n" << icp.getFinalTransformation() << std::endl;} else {std::cout << "ICP 未能收敛\n";}return icp.getFinalTransformation();
}

2.1.4 点云可视化函数

该函数用于可视化配准前后的点云，配准后的点云显示为绿色，目标点云显示为红色。

void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud) {boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("KD-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> icp_color(icp_cloud, 0, 255, 0);viewer->addPointCloud(icp_cloud, icp_color, "icp cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "icp cloud");while (!viewer->wasStopped()) {viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>         // 引入ICP配准算法
#include <pcl/search/kdtree.h>            // 引入KD树加速最近邻搜索
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>             // 用于计算配准时间// 加载点云数据函数
// 该函数用于从PCD文件中加载点云数据，如果文件加载失败会返回nullptr
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("无法读取点云文件 %s\n", filename.c_str());return nullptr;}std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点\n";return cloud;
}// 构建KD树函数
// 为了加速最近邻搜索，该函数为输入的点云构建一个KD树
pcl::search::KdTree<pcl::PointXYZ>::Ptr buildKDTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());tree->setInputCloud(cloud);return tree;
}// KD-ICP配准函数
// 该函数执行基于KD树的ICP配准，通过设置ICP参数和构建的KD树，进行点云配准并返回最终的变换矩阵
Eigen::Matrix4f performKDICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2) {pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;// 设置KD树用于加速最近邻搜索icp.setSearchMethodSource(tree1);icp.setSearchMethodTarget(tree2);// 设置ICP输入点云，源点云与目标点云icp.setInputSource(source);icp.setInputTarget(target);// 设置ICP的参数icp.setMaxCorrespondenceDistance(1);       // 设置对应点对之间的最大距离icp.setMaximumIterations(35);              // 设置最大迭代次数icp.setTransformationEpsilon(1e-10);       // 为终止条件设置最小转换差异icp.setEuclideanFitnessEpsilon(0.05);      // 设置收敛条件：当均方误差和小于该阈值时停止迭代// 存储配准结果的点云pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);// 执行ICP配准icp.align(*icp_cloud);// 判断ICP是否收敛并输出结果if (icp.hasConverged()) {std::cout << "ICP 收敛，得分为 " << icp.getFitnessScore() << std::endl;std::cout << "变换矩阵：\n" << icp.getFinalTransformation() << std::endl;} else {std::cout << "ICP 未能收敛\n";}// 返回最终的刚体变换矩阵return icp.getFinalTransformation();
}// 点云可视化函数
// 该函数用于可视化原始点云（源点云与目标点云）及配准后的点云
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud) {// 创建PCL可视化对象boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("KD-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> icp_color(icp_cloud, 0, 255, 0);viewer->addPointCloud(icp_cloud, icp_color, "icp cloud");// 设置点云的显示属性（点大小为1）viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "icp cloud");// 运行可视化窗口，直到关闭窗口while (!viewer->wasStopped()) {viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}int main(int argc, char** argv) {pcl::console::TicToc time;   // 用于计算执行时间// 加载源点云和目标点云pcl::PointCloud<pcl::PointXYZ>::Ptr source = loadPointCloud("1.pcd");pcl::PointCloud<pcl::PointXYZ>::Ptr target = loadPointCloud("2.pcd");// 构建KD树pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 = buildKDTree(source);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 = buildKDTree(target);// 使用KD树加速的ICP配准time.tic();  // 开始计时Eigen::Matrix4f final_transform = performKDICP(source, target, tree1, tree2);std::cout << "配准时间: " << time.toc() << " ms" << std::endl;  // 输出配准时间// 配准后将源点云进行变换pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>());pcl::transformPointCloud(*source, *icp_cloud, final_transform);// 可视化原始点云与配准后的点云visualizePointClouds(source, target, icp_cloud);return 0;
}

三、实现效果

ICP 收敛，得分为 8.32129e-06
变换矩阵：0.914549   -0.38993   0.107491   0.0238710.345635    0.89144   0.293038 -0.0615766-0.210087  -0.230845   0.950039  0.05358960          0          0          1

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

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

相关文章

SpringAI快速上手

SpringAI快速上手

一、导入依赖 镜像&#xff08;导入maven依赖&#xff09; <repositories><repository><id>spring-snapshots</id><name>Spring Snapshots</name><url>https://repo.spring.io/snapshot</url><releases><enabled>…
阅读更多...
Lnmp（mysql分离）（nginx 1.13.6+mysql5.5+php5.3）环境一键搭建

Lnmp（mysql分离）（nginx 1.13.6+mysql5.5+php5.3）环境一键搭建

Lnmp&#xff08;mysql分离&#xff09;&#xff08;nginx 1.13.6mysql5.5php5.3&#xff09;环境一键搭建 如果对运维课程感兴趣&#xff0c;可以在b站上、csdn或微信视频号 上搜索我的账号&#xff1a; 运维实战课程&#xff0c;可以关注我&#xff0c;学习更多免费的运维实…
阅读更多...
当你不会介绍自己的产品和系统时，不妨看看大厂是如何做的

当你不会介绍自己的产品和系统时，不妨看看大厂是如何做的

当你为不知如何介绍自己的产品和系统而困惑时&#xff0c;不妨把目光投向那些大厂。 大厂就像是璀璨的灯塔&#xff0c;为我们指引着方向。 他们在介绍产品和系统时&#xff0c;犹如技艺精湛的艺术家&#xff0c;以独特的方式勾勒出一幅幅令人惊艳的画卷。 他们用富有感染力…
阅读更多...
股票金融市场中的tick，分钟，日线数据

股票金融市场中的tick，分钟，日线数据

在金融市场中&#xff0c;股票数据的分析对于投资者来说至关重要。股票数据可以根据时间粒度的不同&#xff0c;分为几种不同的类型&#xff0c;包括Tick数据、分钟数据和日线数据。下面将详细介绍这些数据类型&#xff0c;并对比它们之间的差别。 Tick数据 Tick数据&#xf…
阅读更多...
详述python的列表、元组、字典、集合的基本语法及其函数

详述python的列表、元组、字典、集合的基本语法及其函数

目录 列表: 列表的两种创建方式&#xff1a; 列表的删除&#xff1a; 列表的三种遍历&#xff1a; 列表的基础函数&#xff1a; 列表排序的两种方式&#xff1a; 列表生成式的语法结构&#xff1a; 二维列表的遍历&#xff1a; 元组&#xff1a; 元组的创建&#xff1…
阅读更多...
Html 标题加图标

Html 标题加图标

每个网页选项卡都有一个图标&#xff1a; <meta charset"UTF-8"><meta name"viewport" content"widthdevice-width, initial-scale1.0"><title>主页</title><link rel"icon" href"images/记事本.png&…
阅读更多...
Spring Boot框架下JavaWeb在线考试系统的创新实现

Spring Boot框架下JavaWeb在线考试系统的创新实现

2相关技术 2.1 MYSQL数据库 MySQL是一个真正的多用户、多线程SQL数据库服务器。 是基于SQL的客户/服务器模式的关系数据库管理系统&#xff0c;它的有点有有功能强大、使用简单、管理方便、安全可靠性高、运行速度快、多线程、跨平台性、完全网络化、稳定性等&#xff0c;非常…
阅读更多...
计算机毕业设计 | SpringBoot大型旅游网站 旅行后台管理系统（附源码）

计算机毕业设计 | SpringBoot大型旅游网站 旅行后台管理系统（附源码）

1&#xff0c; 概述 1.1 项目背景 随着互联网技术的快速发展和普及&#xff0c;旅游行业逐渐转向线上&#xff0c;越来越多的游客选择在线预订旅游产品。传统的线下旅行社模式已不能满足市场需求&#xff0c;因此&#xff0c;开发一个高效、便捷的旅游网站成为行业的迫切需求…
阅读更多...
15分钟学Go 第3天：编写第一个Go程序

15分钟学Go 第3天：编写第一个Go程序

第3天&#xff1a;编写第一个Go程序 1. 引言 在学习Go语言的过程中&#xff0c;第一个程序通常是“Hello, World!”。这个经典的程序不仅教会你如何编写代码&#xff0c;还引导你理解Go语言的基本语法和结构。本节将详细介绍如何编写、运行并理解第一个Go程序&#xff0c;通过…
阅读更多...
[旧日谈]关于Qt的刷新事件频率，以及我们在Qt的框架上做实时的绘制操作时我们该关心什么。

[旧日谈]关于Qt的刷新事件频率，以及我们在Qt的框架上做实时的绘制操作时我们该关心什么。

[旧日谈]关于Qt的刷新事件频率&#xff0c;以及我们在Qt的框架上做实时的绘制操作时我们该关心什么。 最近在开发的时候&#xff0c;发现一个依赖事件来刷新渲染的控件会导致程序很容易异常和崩溃。 当程序在运行的时候&#xff0c;其实软件本身的负载并不高&#xff0c;所以…
阅读更多...
小新学习Docker之Docker--harbor私有仓库部署与管理

小新学习Docker之Docker--harbor私有仓库部署与管理

目录 一、Harbor简介 1.1、Harbor概述 1.2、Harbor的特性 1.3、Harbor的构成 二、Harbor构建Docker私有仓库 2.1、部署Harbor服务 2.2、启动 Harbor 2.3、查看 Harbor 启动镜像&#xff0c;检查harbor是否安装成功 2.4、创建一个新项目 2.5、非本地主机进行下载镜像 …
阅读更多...
爬虫逆向学习(十二)：一个案例入门补环境

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

此分享只用于学习用途&#xff0c;不作商业用途&#xff0c;若有冒犯&#xff0c;请联系处理 反爬前置信息 站点&#xff1a;aHR0cDovLzEyMC4yMTEuMTExLjIwNjo4MDkwL3hqendkdC94anp3ZHQvcGFnZXMvaW5mby9wb2xpY3k 接口&#xff1a;/xjzwdt/rest/xmzInfoDeliveryRest/getInfoDe…
阅读更多...
宁德时代25届校招入职Verify测评大揭秘：:数字推理25分钟+言语推理19分钟SHL题库

宁德时代25届校招入职Verify测评大揭秘：:数字推理25分钟+言语推理19分钟SHL题库

非常感谢您对宁德时代的关注。祝贺您通过宁德时代校园招聘的专业面试环节&#xff0c;现邀请您参与完成以下测评。本轮共两份测评&#xff0c;每份测评对您的最终结果都非常重要&#xff0c;请务必在收到测评后48小时内完成!具体如下:A. 登录信息: 测评包含语言理解数字推理两…
阅读更多...
前缀和算法——优选算法

前缀和算法——优选算法

个人主页&#xff1a;敲上瘾-CSDN博客 个人专栏&#xff1a;游戏、数据结构、c语言基础、c学习、算法 一、什么是前缀和&#xff1f; 前缀和是指从数组的起始位置到某一位置&#xff08;或矩阵的某个区域&#xff09;的所有元素的和。这种算法通过预处理数组或矩阵&#xff0c;…
阅读更多...
【日志】编辑器开发——修复根据Excel表格数据生成Json文件和配置表代码报错

【日志】编辑器开发——修复根据Excel表格数据生成Json文件和配置表代码报错

2024.10.15 又是蕉绿且摆烂的一天&#xff0c;不仅需要克制网瘾&#xff0c;还要努力学习&#xff0c;不然真的会被抛弃啊。但是我还是不想卷&#xff0c;给我的时间大概还有半年&#xff0c;突然好奇半年时间到底能学点什么或者做点什么。 【力扣刷题】 暂无 【数据结构】 …
阅读更多...
Python入门笔记(七)

Python入门笔记(七)

文章目录 第十五章. 下载数据15.1 csv文件15.2 json文件 第十六章. 使用API16.1 requests 前些天发现了一个巨牛的人工智能学习网站&#xff0c;通俗易懂&#xff0c;风趣幽默&#xff0c;忍不住分享一下给大家。 点击跳转&#xff1a;人工智能从入门到精通教程 本文电子版获取…
阅读更多...
C++初阶学习第七弹——string的模拟实现

C++初阶学习第七弹——string的模拟实现

C初阶学习第六弹------标准库中的string类_c语言返回string-CSDN博客 通过上篇我们已经学习到了string类的基本使用&#xff0c;这里我们就试着模拟实现一些&#xff0c;我们主要实现一些常用到的函数。 目录 一、string类的构造 二、string类的拷贝构造 三、string类的析构函…
阅读更多...
请求的响应----状态码分为五大类(爬虫)

请求的响应----状态码分为五大类(爬虫)

前言 一个爬虫的成功与否&#xff0c;在于你是否拿到了想要的数据&#xff1b;一个请求的成功与否&#xff0c;在于响应的状态码&#xff0c;它标明了当前请求下这个响应的结果&#xff0c;是好还是坏。上节课程学习了HTTPS和HTTP协议的各自优势&#xff0c;本节课程进入到请求…
阅读更多...
《Linux从小白到高手》综合应用篇：详解Linux系统调优之服务器硬件优化

《Linux从小白到高手》综合应用篇：详解Linux系统调优之服务器硬件优化

List item 本篇介绍Linux服务器硬件调优。硬件调优主要包括CPU、内存、磁盘、网络等关键硬件组。 1. CPU优化 选择适合的CPU&#xff1a; –根据应用需求选择多核、高频的CPU&#xff0c;以满足高并发和计算密集型任务的需求。CPU缓存优化&#xff1a; –确保CPU缓存&#x…
阅读更多...
前端学习-css的元素显示模式（十五）

前端学习-css的元素显示模式（十五）

提示&#xff1a;文章写完后&#xff0c;目录可以自动生成&#xff0c;如何生成可参考右边的帮助文档 目录 前言 什么是元素显示模式 块元素 常见的块元素 块元素的特点 注意 行内元素 行内元素的特点 注意 行内块元素 行内块元素的特点 元素显示模式的转换 语法格…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023