PCL 点云配准 基于目标对称的ICP算法（精配准）

news2024/11/27 19:49:37

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1计算点云的法线

2.1.2基于对称误差估计的ICP配准

2.1.3可视化

2.2完整代码

三、实现效果

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

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

一、概述

        基于目标对称的ICP算法（Symmetric ICP）是一种改进的迭代最近点算法（Iterative Closest Point, ICP）。在传统ICP中，配准过程依赖于最小化源点云和目标点云之间的欧氏距离。然而，传统ICP对有对称性特征的场景配准时容易陷入局部最优解，无法充分利用目标的对称性特征进行精确配准。

1.1原理

        基于目标对称的ICP算法通过对称点到平面的误差估计方法（Symmetric Point-to-Plane）优化变换矩阵的估计。该算法同时最小化源点到目标点云表面的距离以及目标点云到源点云表面的距离，从而对对称性场景有更好的适应性。它能够有效地处理含有对称结构的点云，提升配准的精度和稳定性。

1.2实现步骤

  1. 加载源点云和目标点云：读取待配准的源点云和目标点云数据。
  2. 计算法线信息：为源点云和目标点云计算法线，并将点云与法线信息合并。
  3. 基于对称误差估计的ICP配准：利用对称的点到平面距离误差估计，进行点云配准。
  4. 结果输出与可视化：输出配准后的变换矩阵，并可视化源点云、目标点云和配准后的结果点云。

1.3应用场景

  1. 对称物体的3D点云配准，如汽车、飞机等结构体的点云匹配。
  2. 自动驾驶、机器人等场景中，基于对称目标进行定位与姿态估计。
  3. 在含有对称结构的复杂场景中，提高点云配准的精度与效率。

二、代码实现

2.1关键函数

2.1.1计算点云的法线

// 计算点云的法线并与点云数据拼接，生成带法线的点云
void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, 
                       pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals) 
{
    // 使用OMP加速法线计算
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    // 设置KD树搜索方法
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setNumberOfThreads(8);  // 设置使用的线程数
    ne.setInputCloud(cloud);   // 输入点云
    ne.setSearchMethod(tree);  // KD树搜索
    ne.setKSearch(10);         // 设置K近邻点个数
    ne.compute(*normals);      // 计算法线

    // 拼接点云数据和法线信息，生成带法线的点云
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

2.1.2基于对称误差估计的ICP配准

// 执行基于对称点到平面的ICP配准
void perform_symmetric_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& aligned_cloud,
    Eigen::Matrix4f& final_transformation)
{
    // 创建对称点到平面ICP对象
    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> symm_icp;

    // 使用对称点到平面的变换估计方法
    pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr symm_point_to_plane
    (new pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);

    // 设置ICP算法参数
    symm_icp.setTransformationEstimation(symm_point_to_plane);  // 设置对称点到平面距离
    symm_icp.setInputSource(source_with_normals);               // 设置源点云
    symm_icp.setInputTarget(target_with_normals);               // 设置目标点云
    symm_icp.setMaxCorrespondenceDistance(10);                  // 设置最大对应点对之间的距离
    symm_icp.setTransformationEpsilon(1e-10);                   // 设置终止条件：最小转换差异
    symm_icp.setEuclideanFitnessEpsilon(0.001);                 // 设置收敛条件：均方误差
    symm_icp.setMaximumIterations(50);                          // 设置最大迭代次数

    // 执行配准
    symm_icp.align(*aligned_cloud);

    // 输出配准结果
    if (symm_icp.hasConverged()) {
        std::cout << "Symmetric ICP has converged, score is " << symm_icp.getFitnessScore() << std::endl;
        final_transformation = symm_icp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << final_transformation << std::endl;
    }
    else {
        PCL_ERROR("Symmetric ICP未收敛。\n");
        exit(-1);
    }
}

2.1.3可视化

// 可视化配准结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    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, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

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

    viewer->spin();
}

2.2完整代码

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h> // icp算法
#include <pcl/features/normal_3d_omp.h> // 法线计算头文件
#include <pcl/visualization/pcl_visualizer.h> // 可视化

using namespace std;

// 计算点云法线并生成带法线的点云
void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
    pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals)
{
    // 使用OMP加速法线计算
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    // KD树用于近邻搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setNumberOfThreads(8);   // 使用多线程加速法线计算
    ne.setInputCloud(cloud);    // 输入点云
    ne.setSearchMethod(tree);   // KD树搜索
    ne.setKSearch(10);          // 设置K近邻点的个数
    ne.compute(*normals);       // 计算法线

    // 合并点云与法线信息，生成带法线的点云
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

// 执行基于对称点到平面的ICP配准
void perform_symmetric_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& aligned_cloud,
    Eigen::Matrix4f& final_transformation)
{
    // 创建对称点到平面ICP对象
    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> symm_icp;

    // 使用对称点到平面的变换估计方法
    pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr symm_point_to_plane
    (new pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);

    // 设置ICP算法参数
    symm_icp.setTransformationEstimation(symm_point_to_plane);  // 设置对称点到平面距离
    symm_icp.setInputSource(source_with_normals);               // 设置源点云
    symm_icp.setInputTarget(target_with_normals);               // 设置目标点云
    symm_icp.setMaxCorrespondenceDistance(10);                  // 设置最大对应点对之间的距离
    symm_icp.setTransformationEpsilon(1e-10);                   // 设置终止条件：最小转换差异
    symm_icp.setEuclideanFitnessEpsilon(0.001);                 // 设置收敛条件：均方误差
    symm_icp.setMaximumIterations(50);                          // 设置最大迭代次数

    // 执行配准
    symm_icp.align(*aligned_cloud);

    // 输出配准结果
    if (symm_icp.hasConverged()) {
        std::cout << "Symmetric ICP has converged, score is " << symm_icp.getFitnessScore() << std::endl;
        final_transformation = symm_icp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << final_transformation << std::endl;
    }
    else {
        PCL_ERROR("Symmetric ICP未收敛。\n");
        exit(-1);
    }
}

// 可视化配准结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    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, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

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

    viewer->spin();
}

int main()
{
    // --------------------加载源点云-----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);
    cout << "从源点云中读取 " << source->size() << " 个点" << endl;

    // -------------------加载目标点云----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);
    cout << "从目标点云中读取 " << target->size() << " 个点" << endl;

    // 计算源点云的法线
    pcl::PointCloud<pcl::PointNormal>::Ptr source_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(source, source_with_normals);

    // 计算目标点云的法线
    pcl::PointCloud<pcl::PointNormal>::Ptr target_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(target, target_with_normals);

    // 创建对齐后的点云
    pcl::PointCloud<pcl::PointNormal>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointNormal>);
    Eigen::Matrix4f final_transformation = Eigen::Matrix4f::Identity();

    // 执行对称ICP配准
    perform_symmetric_icp(source_with_normals, target_with_normals, aligned_cloud, final_transformation);

    // 使用创建的变换对输入点云进行变换
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*source, *final_cloud, final_transformation);

    // 可视化配准结果
    visualize_registration(source, target, final_cloud);

    return 0;
}

三、实现效果

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

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

相关文章

OpenCV高级图形用户界面(20)更改窗口的标题函数setWindowTitle()的使用

OpenCV高级图形用户界面(20)更改窗口的标题函数setWindowTitle()的使用

操作系统&#xff1a;ubuntu22.04 OpenCV版本&#xff1a;OpenCV4.9 IDE:Visual Studio Code 编程语言&#xff1a;C11 算法描述 在OpenCV中&#xff0c;cv::setWindowTitle函数用于更改窗口的标题。这使得您可以在程序运行时动态地更改窗口的标题文本。 函数原型 void cv::…
阅读更多...
SpringBoot日常：封装redission starter组件

SpringBoot日常：封装redission starter组件

文章目录 逻辑实现POM.xmlRedissionConfigRedissionPropertiesRedissionUtilsspring.factories 功能测试application.yml配置POM.xmlTestController运行测试 本章内容主要介绍如何通过封装相关的redission连接配置和工具类&#xff0c;最终完成一个通用的redission starter。并…
阅读更多...
论文速读：通过目标感知双分支蒸馏进行跨域目标检测（CVPR2022）

论文速读：通过目标感知双分支蒸馏进行跨域目标检测（CVPR2022）

原文标题&#xff1a;Cross Domain Object Detection by Target-Perceived Dual Branch Distillation 中文标题&#xff1a;通过目标感知双分支蒸馏进行跨域目标检测 论文地址&#xff1a; https://arxiv.org/abs/2205.01291 代码地址&#xff1a; GitHub - Feobi1999/TDD 这篇…
阅读更多...
使用多块AMD GPU通过Megatron-DeepSpeed进行大型语言模型的预训练

使用多块AMD GPU通过Megatron-DeepSpeed进行大型语言模型的预训练

Pre-training a large language model with Megatron-DeepSpeed on multiple AMD GPUs 2024年1月24日&#xff0c;作者&#xff1a;Douglas Jia 在这篇博客中&#xff0c;我们将向你展示如何使用Megatron-DeepSpeed框架在多块AMD GPU上预训练GPT-3模型。我们还将展示如何使用你…
阅读更多...
5、JavaScript（二) 对象+DOM

5、JavaScript（二) 对象+DOM

17.对象 1、对象&#xff1a;⽤来存储多个数据的 是由多个键值对/key value对组成的 ⽤来描述⼀个事物的 相当于多个变量的集合 2、格式 &#xff1a;{key:value,key:value} 键/值对 属性名&#xff1a;属性值 3、对象的属性值是不限制数据类型的&#xff0c;甚至还可以是对…
阅读更多...
常用的字符集（ASCII、GBK）

常用的字符集（ASCII、GBK）

目录 1.ASCII字符集 2.各版本的字符集 3. GBK字符集在计算机中的存储规则 4. 总结 1.ASCII字符集 计算机中最小的存储单元是一个字节&#xff0c;一个字节8bit 0-127 一共是128个 2.各版本的字符集 只需要掌握GBK和Unicode两个字符集。GBK是简体中文window操作系统默认使…
阅读更多...
85.【C语言】数据结构之顺序表的中间插入和删除及遍历查找

85.【C语言】数据结构之顺序表的中间插入和删除及遍历查找

目录 3.操作顺序表 1.分析中间插入函数 函数的参数 代码示例 图片分析 main.c部分改为 在SeqList.h添加SLInsert函数的声明 运行结果 2.分析中间删除函数 函数的参数 代码示例 图片分析 main.c部分改为 在SeqList.h添加SLErase函数的声明 运行结果 承接84.【C语…
阅读更多...
Atlas800昇腾服务器（型号：3000）—YOLO全系列NPU推理【检测】（五）

Atlas800昇腾服务器（型号：3000）—YOLO全系列NPU推理【检测】（五）

服务器配置如下&#xff1a; CPU/NPU&#xff1a;鲲鹏 CPU&#xff08;ARM64&#xff09;A300I pro推理卡 系统&#xff1a;Kylin V10 SP1【下载链接】【安装链接】 驱动与固件版本版本&#xff1a; Ascend-hdk-310p-npu-driver_23.0.1_linux-aarch64.run【下载链接】 Ascend-…
阅读更多...
spring boot实现不停机更新

spring boot实现不停机更新

主要实现思路:发布新的应用程序(与原端口不同),启动成功后,将原端口进行优雅关闭,同时将应用程序端口动态切换至原端口 application.yml server:port: 8000shutdown: graceful DatapickCliApplication package com.zy.datapickcli;import org.springframework.boot.SpringAp…
阅读更多...
保研考研机试攻略：python笔记（1）

保研考研机试攻略：python笔记（1）

&#x1f428;&#x1f428;&#x1f428;宝子们好呀 ~ 我来更新欠大家的python笔记了&#xff0c;从这一篇开始我们来学下python&#xff0c;当然&#xff0c;如果只是想应对机试并且应试语言以C和C为主&#xff0c;那么大家对python了解一点就好&#xff0c;重点可以看高分篇…
阅读更多...
pikachu靶场CSRF-get测试报告

pikachu靶场CSRF-get测试报告

目录 一、测试环境 1、系统环境 2、使用工具/软件 二、测试目的 三、操作过程 1、抓包使用burp生成csrf脚本 四、源代码分析 五、结论 一、测试环境 1、系统环境 渗透机&#xff1a;本机(127.0.0.1) 靶 机&#xff1a;本机(127.0.0.1) 2、使用工具/软件 Burp sui…
阅读更多...
【Trick】在vscode上配置copilot时，输出端出现Invalid copilot token: missing token: 403

【Trick】在vscode上配置copilot时，输出端出现Invalid copilot token: missing token: 403

今天心血来潮想要给vscode配置一个copilot&#xff0c;正好上学期在github上通过教育邮箱实现了学生认证&#xff0c;可以免费使用copilot服务&#xff08;bushi&#xff09;。 首先是按照官网&#xff08;Getting code suggestions in your IDE with GitHub Copilot - GitHub…
阅读更多...
机器学习与金融风控项目篇-day01-风控业务

机器学习与金融风控项目篇-day01-风控业务

一. 整体项目介绍 1.风控业务和风控报表 零售金融产品相关的指标风控建模流程 2.特征工程 特征构造特征筛选 3.评分卡模型构建 逻辑回归集成学习 XGBoost LightGBM模型评估 4.样本不均衡问题/异常点检测 二. 信贷与风控介绍 信贷业务 信贷业务&#xff0c;就是贷款业务&#x…
阅读更多...
桃子叶片病害分类检测数据集（猫脸码客 第221期）

桃子叶片病害分类检测数据集（猫脸码客 第221期）

桃子叶片病害分类检测数据集 一、引言 桃子作为世界上广泛种植的果树之一&#xff0c;其叶片的健康状况直接关系到果实的产量和品质。然而&#xff0c;桃子叶片易受多种病害的侵袭&#xff0c;这些病害不仅影响叶片的光合作用&#xff0c;还可能导致果实减产、品质下降&#…
阅读更多...
XPM_CDC_SYNC_RST

XPM_CDC_SYNC_RST

免责声明&#xff1a;本文所提供的信息和内容仅供参考。作者对本文内容的准确性、完整性、及时性或适用性不作任何明示或暗示的保证。在任何情况下&#xff0c;作者不对因使用本文内容而导致的任何直接或间接损失承担责任&#xff0c;包括但不限于数据丢失、业务中断或其他经济…
阅读更多...
基于SSM的医院药品管理系统

基于SSM的医院药品管理系统

作者&#xff1a;计算机学姐 开发技术&#xff1a;SpringBoot、SSM、Vue、MySQL、JSP、ElementUI、Python、小程序等&#xff0c;“文末源码”。 专栏推荐&#xff1a;前后端分离项目源码、SpringBoot项目源码、Vue项目源码、SSM项目源码、微信小程序源码 精品专栏&#xff1a;…
阅读更多...
Axure垂直菜单展开与折叠

Axure垂直菜单展开与折叠

亲爱的小伙伴&#xff0c;在您浏览之前&#xff0c;烦请关注一下&#xff0c;在此深表感谢&#xff01; 课程主题&#xff1a;Axure垂直菜单展开与折叠 主要内容&#xff1a;垂直菜单单击实现展开/折叠&#xff0c;点击各菜单项显示选中效果 应用场景&#xff1a;后台菜单设…
阅读更多...
靠卡车赚钱，小马智行等待Robotaxi的春天

靠卡车赚钱，小马智行等待Robotaxi的春天

文&#xff5c;刘俊宏 编&#xff5c;王一粟 继文远知行之后&#xff0c;又一家L4无人驾驶公司也准备上市。 10月18日&#xff0c;无人驾驶服务商小马智行向美国SEC&#xff08;证券交易委员会&#xff09;提交了招股书。继百度、Waymo、特斯拉之后&#xff0c;根据招股书的…
阅读更多...
【EPLAN 2.9】清理Data文件夹的小工具

【EPLAN 2.9】清理Data文件夹的小工具

背景&#xff1a; 随着EPLAN不断使用“C:\Users\Public\EPLAN\Data”文件夹会越来越大&#xff08;EPLAN2.9部件管理中删除部件不会去删除文件夹内相关资源文件&#xff09;&#xff0c;本工具在于清理文件夹内未被EPLAN关联的宏、图片、文档。仅支持EPLAN 2.9 代码&#xff…
阅读更多...
[环境配置]macOS上怎么查看vscode的commit id

[环境配置]macOS上怎么查看vscode的commit id

macOS的commit id和windows上有点不一样&#xff0c;windows可以在帮助-关于查看 macOS则需要再左边第一个查看
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023