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

embedded/2024/10/17 20:20:24/

目录

一、概述

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.ppmy.cn/embedded/128258.html

相关文章

【微服务】全面构建微服务监控体系：确保系统稳定与性能优化的关键

【微服务】全面构建微服务监控体系：确保系统稳定与性能优化的关键

目录 引言一、微服务监控概述1.1 微服务监控的定义1.2 微服务监控的重要性1.3 监控的核心目标1.4 微服务监控的关键指标1.5 监控的策略 二、微服务监控的架构2.1 监控架构图2.2 架构组件2.3 监控架构示意图 三、微服务监控的工具3.1 工具概述3.2 Prometheus3.3 Grafana3.4 ELK …
阅读更多...
解决：Ubuntu跑slam，遇到rviz闪退

解决：Ubuntu跑slam，遇到rviz闪退

在使用ros运行slam算法的时候&#xff0c;运行roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch和rosbag play nsh_indoor_outdoor.bag中的某一条指令的时候&#xff0c;发现rviz启动后&#xff0c;里面没有内容&#xff0c;并且闪退。 我尝试关闭vmware中的3D加速&…
阅读更多...
使用docker搭建lnmp运行WordPress

使用docker搭建lnmp运行WordPress

一&#xff0c;部署目的 使用 Docker 技术在单机上部署 LNMP 服务&#xff08;Linux Nginx MySQL PHP&#xff09;。部署并运行 WordPress 网站平台。掌握 Docker 容器间的互联及数据卷共享。 二&#xff0c;部署环境 操作系统&#xff1a;CentOS 7Docker 版本&#xff1…
阅读更多...
Python3 标准库概览和例子

Python3 标准库概览和例子

一&#xff0c;Python3 标准库中的模块&#xff1a;1&#xff0c;os 模块&#xff1a;os 模块提供了许多与操作系统交互的函数&#xff0c;例如创建、移动和删除文件和目录&#xff0c;以及访问环境变量等。 2&#xff0c;sys 模块&#xff1a;sys 模块提供了与 Python 解释器…
阅读更多...
前端布局，y轴超出滚动、x轴超出展示方案

前端布局，y轴超出滚动、x轴超出展示方案

想要实现布局效果&#xff0c;红区高度固定可滑动可收起。红区引用绿区组件。 一般会想到如下方案&#xff0c;红区样式&#xff1a; width&#xff1a;200px; height: 100%; overflow-y: auto; overflow-x: visible; 但是效果并不好&#xff0c;绿区直接隐藏了 最终采用布局方…
阅读更多...
Jenkins整合Docker实现CICD自动化部署（若依项目）

Jenkins整合Docker实现CICD自动化部署（若依项目）

前期准备 提前准备好jenkins环境 并且jenkins能使用docker命令&#xff0c;并且已经配置好了jdk、node、maven环境&#xff0c;我之前写了安装jenkins的博客&#xff0c;里面讲得比较详细&#xff0c;推荐用我这种方式安装 docker安装jenkins&#xff0c;并配置jdk、node和m…
阅读更多...
javaWeb项目-ssm+jsp股票交易管理系统功能介绍

javaWeb项目-ssm+jsp股票交易管理系统功能介绍

本项目源码&#xff08;点击下方链接下载&#xff09;&#xff1a;java-ssmjsp股票交易管理系统实现源码(项目源码-说明文档)资源-CSDN文库 项目关键技术 开发工具&#xff1a;IDEA 、Eclipse 编程语言: Java 数据库: MySQL5.7 框架&#xff1a;ssm、Springboot 前端&#xff…
阅读更多...
IBM AIX服务器监控易监测指标解读

IBM AIX服务器监控易监测指标解读

监控易是一款功能全面的IT基础设施监控软件&#xff0c;它能够通过多种方式&#xff08;如SSH、SNMP等&#xff09;对服务器、网络设备等IT资源进行全面监控。针对IBM AIX服务器&#xff0c;监控易提供了一系列特定的监测指标&#xff0c;以确保服务器的稳定运行并及时发现潜在…
阅读更多...
最新文章

Copyright @ 2022~2023