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

news2024/11/26 6:20:12

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 perform_standard_icp 函数

2.1.2 perform_trimmed_icp 函数

2.1.3 visualize_registration 函数

2.2完整代码

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

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

一、概述

        Trimmed ICP算法是一种改进的点云配准算法，用于减少噪声点和异常值对配准精度的影响。它通过选择一定比例的内点参与配准，避免由于噪声或遮挡导致的误差传播。这种方法在处理具有部分重叠和大量噪声的点云数据时具有显著优势。

1.1原理

        Trimmed ICP算法的核心思想是在每次迭代中只选择一部分最匹配的点对进行配准，通常通过设置内点的比例来控制匹配点的选择。该算法在配准时能够更好地处理局部噪声和不完全重叠的问题。

        Trimmed ICP算法与标准ICP的不同之处在于：

  1. 配准点对选择：Trimmed ICP只选择源点云中最匹配的点对进行迭代，而不是使用所有点对，从而减少噪声点的干扰。
  2. 能量优化：在每次迭代中，Trimmed ICP只考虑一定比例的最小能量点对进行优化，从而更加鲁棒地处理部分匹配和噪声问题。

1.2实现步骤

  1. 加载点云数据：读取源点云和目标点云。
  2. 标准ICP配准：使用标准ICP进行初步配准，获得初始位姿估计。
  3. Trimmed ICP配准：在初始位姿基础上，使用Trimmed ICP进行精配准，通过设置内点比例进行优化。
  4. 可视化配准结果：输出和可视化配准前后点云的结果，并打印变换矩阵。

1.3应用场景

  1. 存在大量噪声的点云配准：Trimmed ICP能够有效剔除噪声点，适用于工业场景中的噪声数据处理。
  2. 部分重叠点云：当源点云和目标点云存在部分重叠时，Trimmed ICP通过选择部分匹配的点对进行配准，从而减少误差。
  3. 3D扫描、SLAM等需要精确配准的场景：Trimmed ICP能在带有噪声和部分重叠的点云中提供更高的配准精度。

二、代码实现

2.1关键函数

2.1.1 perform_standard_icp 函数

用于执行标准的ICP配准，获得初始的刚体变换矩阵：

// 执行标准ICP配准，获取初始位姿估计
void perform_standard_icp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, 
                          pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud, Eigen::Matrix4d& transformation_matrix, int iterations)
{
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setMaximumIterations(iterations);           // 设置最大迭代次数
    icp.setMaxCorrespondenceDistance(15);           // 设置最大对应点距离
    icp.setTransformationEpsilon(1e-10);            // 设置精度
    icp.setEuclideanFitnessEpsilon(0.01);           // 设置收敛条件
    icp.setInputSource(source_cloud);               // 设置源点云
    icp.setInputTarget(target_cloud);               // 设置目标点云
    icp.align(*icp_cloud);                          // 执行ICP配准

    if (icp.hasConverged())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
        transformation_matrix = icp.getFinalTransformation().cast<double>();  // 获取变换矩阵
        print4x4Matrix(transformation_matrix);                                // 打印变换矩阵
    }
    else
    {
        PCL_ERROR("\nICP has not converged.\n");
        exit(-1);
    }
}

2.1.2 perform_trimmed_icp 函数

该函数执行Trimmed ICP配准，通过剔除噪声点进行更精细的配准：

// 执行Trimmed ICP配准，剔除噪声点进行精配准
void perform_trimmed_icp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, 
                         pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud, Eigen::Matrix4d& transformation_matrix)
{
    pcl::recognition::TrimmedICP<pcl::PointXYZ, double> Tricp;
    Tricp.init(target_cloud);                         // 初始化目标点云
    float sigma = 0.96;                               // 设置内点比例
    int Np = source_cloud->size();                    // 源点云点数量
    int Npo = Np * sigma;                             // 参与配准的点对数量
    Tricp.setNewToOldEnergyRatio(sigma);              // 设置内点能量比例
    Tricp.align(*icp_cloud, Npo, transformation_matrix);  // 执行Trimmed ICP配准

    std::cout << "Trimmed ICP finished. Transformation matrix: \n";
    print4x4Matrix(transformation_matrix);            // 打印变换矩阵
}

2.1.3 visualize_registration 函数

该函数用于可视化配准前后的点云：

// 可视化配准前后的点云
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, 
                            pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));

    int v1, v2;
    viewer->createViewPort(0, 0.0, 0.5, 1.0, v1);      // 左侧视图
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);    // 右侧视图

    // 添加点云到视图中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 0, 255, 0);  // 源点云绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 255, 0, 0);  // 目标点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> result_h(result, 0, 0, 255);  // 配准后点云蓝色

    viewer->addPointCloud(source, src_h, "source cloud", v1);  // 左侧显示源点云
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);  // 左侧显示目标点云
    viewer->addPointCloud(result, result_h, "result cloud", v2);  // 右侧显示配准后点云

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/console/time.h>
#include <pcl/recognition/ransac_based/trimmed_icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Dense>
#include "Trimmed.h"  // 自定义的工具函数头文件

using namespace std;

// 执行标准ICP配准
void perform_standard_icp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud,
    Eigen::Matrix4d& transformation_matrix, int iterations)
{
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setMaximumIterations(iterations);           // 设置最大迭代次数
    icp.setMaxCorrespondenceDistance(15);           // 设置最大对应点距离
    icp.setTransformationEpsilon(1e-10);            // 设置精度
    icp.setEuclideanFitnessEpsilon(0.01);           // 设置收敛条件
    icp.setInputSource(source_cloud);               // 设置源点云
    icp.setInputTarget(target_cloud);               // 设置目标点云
    icp.align(*icp_cloud);                          // 执行ICP配准

    if (icp.hasConverged())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
        transformation_matrix = icp.getFinalTransformation().cast<double>();  // 获取变换矩阵
        print4x4Matrix(transformation_matrix);                                // 打印变换矩阵
    }
    else
    {
        PCL_ERROR("\nICP has not converged.\n");
        exit(-1);
    }
}

// 执行Trimmed ICP配准
void perform_trimmed_icp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud,
    Eigen::Matrix4d& transformation_matrix)
{
    pcl::recognition::TrimmedICP<pcl::PointXYZ, double> Tricp;
    Tricp.init(target_cloud);                         // 初始化目标点云
    float sigma = 0.96;                               // 设置内点比例
    int Np = source_cloud->size();                    // 源点云点数量
    int Npo = Np * sigma;                             // 参与配准的点对数量
    Tricp.setNewToOldEnergyRatio(sigma);              // 设置内点能量比例
    Tricp.align(*icp_cloud, Npo, transformation_matrix);  // 执行Trimmed ICP配准

    std::cout << "Trimmed ICP finished. Transformation matrix: \n";
    print4x4Matrix(transformation_matrix);            // 打印变换矩阵
}

// 可视化配准前后的点云
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));

    int v1, v2;
    viewer->createViewPort(0, 0.0, 0.5, 1.0, v1);      // 左侧视图
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);    // 右侧视图

    // 添加点云到视图中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 0, 255, 0);  // 源点云绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 255, 0, 0);  // 目标点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> result_h(result, 0, 0, 255);  // 配准后点云蓝色

    viewer->addPointCloud(source, src_h, "source cloud", v1);  // 左侧显示源点云
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);  // 左侧显示目标点云
    viewer->addPointCloud(result, result_h, "result cloud", v2);  // 右侧显示配准后点云

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
}

int main(int argc, char* argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    int iterations = 35;
    pcl::console::TicToc time;
    time.tic();

    pcl::io::loadPCDFile("1.pcd", *source_cloud);  // 加载源点云
    pcl::io::loadPCDFile("2.pcd", *target_cloud);  // 加载目标点云

    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();  // 变换矩阵

    // 标准ICP实现
    perform_standard_icp(source_cloud, target_cloud, icp_cloud, transformation_matrix, iterations);

    // Trimmed ICP实现
    perform_trimmed_icp(source_cloud, target_cloud, icp_cloud, transformation_matrix);

    // 可视化
    visualize_registration(source_cloud, target_cloud, icp_cloud);

    return 0;
}

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

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

相关文章

国庆旅游高峰期，如何利用可视化报表来展现景区、游客及消费数据

国庆旅游高峰期，如何利用可视化报表来展现景区、游客及消费数据

国庆黄金周&#xff0c;作为国内旅游市场的年度盛宴&#xff0c;总是吸引着无数游客的目光。今年&#xff0c;随着旅游市场的强劲复苏&#xff0c;各大景区又再次迎来游客流量的高峰。全国国内出游7.65亿人次&#xff0c;同比增长5.9%&#xff0c;国内游客出游总花费7008.17亿元…
阅读更多...
大型企业软件开发是什么样子的？ - Web Dev Cody

大型企业软件开发是什么样子的？ - Web Dev Cody

引用自大型企业软件开发是什么样子的&#xff1f; - Web Dev Cody_哔哩哔哩_bilibili 一般来说 学技术的时候 我们会关注 开发语言特性 &#xff0c;各种高级语法糖&#xff0c;底层技术 但是很少有关注到企业里面的开发流程&#xff0c;本着以终为始&#xff08;以就业为导向…
阅读更多...
界面控件Telerik UI for WPF 2024 Q3亮点 - 支持禁用数据过滤等

界面控件Telerik UI for WPF 2024 Q3亮点 - 支持禁用数据过滤等

Telerik UI for WPF拥有超过100个控件来创建美观、高性能的桌面应用程序&#xff0c;同时还能快速构建企业级办公WPF应用程序。UI for WPF支持MVVM、触摸等&#xff0c;创建的应用程序可靠且结构良好&#xff0c;非常容易维护&#xff0c;其直观的API将无缝地集成Visual Studio…
阅读更多...
[Linux网络编程]03-TCP协议

[Linux网络编程]03-TCP协议

一.TCP协议数据通信的过程 TCP数据报如下&#xff0c;数据报中的标志位双端通信的关键。 三次握手: 1.客户端向服务端发送SYN标志位&#xff0c;请求建立连接&#xff0c;同时发送空包 2.服务端向客户端回发ACK标志位(即确认标志位&#xff0c;任何一端发送数据后都需要另一端…
阅读更多...
asyn驱动示例-int32driver

asyn驱动示例-int32driver

驱动程序源代码int32Driver.c&#xff1a; #include <stddef.h> #include <string.h> #include <stdlib.h> #include <stdio.h> #include <errno.h> #include <ctype.h>#include <cantProceed.h> #include <epicsStdio.h> #i…
阅读更多...
官龙村捐赠图书整理有感

官龙村捐赠图书整理有感

今天&#xff08;2024年10月20日&#xff09;&#xff0c;我有幸参加了在深圳南山区西丽官龙村举行的义工活动&#xff0c;主要任务是整理捐赠的图书&#xff0c;并根据小学和中学的需求进行分类打包。这次活动不仅让我体会到了劳动的辛苦&#xff0c;更让我感受到了助人为乐的…
阅读更多...
【AIGC】第一性原理下的ChatGPT提示词Prompt设计：系统信息与用户信息的深度融合

【AIGC】第一性原理下的ChatGPT提示词Prompt设计：系统信息与用户信息的深度融合

博客主页&#xff1a; [小ᶻZ࿆] 本文专栏: AIGC | ChatGPT 文章目录 &#x1f4af;前言&#x1f4af;第一性原理与ChatGPT提示词Prompt设计应用第一性原理于ChatGPT提示词Prompt设计系统信息和用户信息的融合实际应用结论 &#x1f4af;系统信息与用户信息的定义和重要性系…
阅读更多...
鸿蒙中富文本编辑与展示

鸿蒙中富文本编辑与展示

富文本在鸿蒙系统如何展示和编辑的&#xff1f;在文章开头我们提出这个疑问&#xff0c;带着疑问来阅读这篇文章。 富文本用途可以展示图文混排的内容&#xff0c;在日常App 中非常常见&#xff0c;比如微博的发布与展示&#xff0c;朋友圈的发布与展示&#xff0c;都在使用富文…
阅读更多...
C++高阶：红黑树实现

C++高阶：红黑树实现

目录 一.红黑树的概念 1.1红黑树的规则 1.2红黑树的效率 二.红黑树的实现 2.1红黑树的结构 2.2红黑树的插入 2.2.1插入的大致过程 2.2.2情况一&#xff1a;变色 ​编辑 2.2.3情况二&#xff1a;单旋变色 2.2.4情况三&#xff1a;双旋变色 2.3插入代码实现 2.4红黑树的…
阅读更多...
【网络安全】简单P1：通过开发者工具解锁专业版和企业版功能

【网络安全】简单P1：通过开发者工具解锁专业版和企业版功能

未经许可，不得转载。 文章目录 前言发现过程前言 在探索一个SaaS平台的过程中，我发现了一个漏洞，使得我能够在无需订阅的情况下解锁高级（专业/企业）功能。 发现过程 我使用一个没有任何高级功能的基本用户账户进行常规登录。在浏览平台时，我注意到某些按钮和功能上带有…
阅读更多...
【C++STL】list的基本介绍与使用方式

【C++STL】list的基本介绍与使用方式

✨ Blog’s 主页: 白乐天_ξ( ✿&#xff1e;◡❛) &#x1f308; 个人Motto&#xff1a;他强任他强&#xff0c;清风拂山冈&#xff01; &#x1f525; 所属专栏&#xff1a;C深入学习笔记 &#x1f4ab; 欢迎来到我的学习笔记&#xff01; 一、list的介绍 文档内容以及大致翻…
阅读更多...
ROS笔记之kill掉所有ros节点rosnode

ROS笔记之kill掉所有ros节点rosnode

ROS笔记之kill掉所有ros节点rosnode 文章目录 ROS笔记之kill掉所有ros节点rosnode1. 杀死所有 ROS 节点&#xff08;不包括 rosmaster&#xff09;2. 杀死 rosmaster3. 验证所有节点和 rosmaster 是否已终止4. roscore 和 rosmaster 是同一个概念吗&#xff1f;5. 为什么执行 k…
阅读更多...
【踩坑日记36】ModuleNotFoundError: No module named ‘taming‘

【踩坑日记36】ModuleNotFoundError: No module named ‘taming‘

问题描述 ModuleNotFoundError: No module named ‘taming‘问题分析 未正确安装taming-transformers包 问题解决 从github网站中安装taming-transformers包&#xff1a; 从github网站中下载taming-transformers代码 git clone https://github.com/CompVis/taming-transfo…
阅读更多...
微机原理与接口技术知识点总结——绪论

微机原理与接口技术知识点总结——绪论

1.1、计算机发展概述 早期计算机的用途主要是用于算数&#xff0c;一直到20世纪电子领域基础研究的突破&#xff0c;也就是电子计算机的诞生&#xff0c;计算机才变得如此广泛。以微电子技术为基础制造的电子计算机已经完全取代了机械计算机和机电计算机&#xff0c;因此电子计…
阅读更多...
第二十二届GOPS全球运维大会2024一日感

第二十二届GOPS全球运维大会2024一日感

第二十二届GOPS全球运维大会将于2024年4月25-26日在南山区深圳湾万丽酒店召开。大会将为期2天&#xff0c;侧重大模型、AIOps、DevOps、可观测、SRE、云原生等热门技术领域。 GOPS是我一直关注的一场技术峰会&#xff0c;主要是它的侧重点在运维方面&#xff0c;这和我本身的职…
阅读更多...
【C++干货篇】——类和对象的魅力（二）

【C++干货篇】——类和对象的魅力（二）

【C干货篇】——类和对象的魅力&#xff08;二&#xff09; 1.类的默认成员函数 默认成员函数就是用户没有显式实现&#xff0c;编译器会⾃动⽣成的成员函数称为默认成员函数。 ⼀个类&#xff0c;我们不写的情况下编译器会默认⽣成以下6个默认成员函数&#xff0c;需要注意的…
阅读更多...
从 Microsoft 官网下载 Windows 10

从 Microsoft 官网下载 Windows 10

方法一&#xff1a; 打开 Microsoft 官网&#xff1a; 打开开发人员工具&#xff08;按 F12 或右键点击“检查”&#xff09;。 点击“电脑模拟手机”按钮&#xff0c;即下图&#xff1a; 点击后重新加载此网页&#xff0c;即可看到下载选项。
阅读更多...
[k8s理论知识]3.docker基础（二）隔离技术

[k8s理论知识]3.docker基础（二）隔离技术

容器其实是一种沙盒技术&#xff0c;其核心是通过约束和修改进程的动态表现&#xff0c;为其创建一个边界。这个边界确保了应用与应用之间不会相互干扰&#xff0c;同时可以方便在不同的环境中迁移&#xff0c;这是PaaS最理想的状态。 程序是代码的可执行镜像&#xff0c;通常…
阅读更多...
springboot项目get请求遇到：Message Request header is too large报错。

springboot项目get请求遇到：Message Request header is too large报错。

一、错误日志 HTTP Status 400 – Bad Request Type Exception ReportMessage Request header is too largeDescription The server cannot or will not process the request due to something that is perceived to be a client error (e.g., malformed request syntax, inva…
阅读更多...
Django学习-ORM_常用字段及字段选项

Django学习-ORM_常用字段及字段选项

字段选项&#xff1a; 注意&#xff1a;当我们新增数据的时候&#xff0c;如果没有新增设置了default的字段&#xff0c;此时会使用default设置的默认值填充到新增的数据中
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023