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

目录

一、概述

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;
}

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

软件设计模式------简单工厂模式

软件设计模式------简单工厂模式

简单工厂模式&#xff08;Simple factory Pattern&#xff09;&#xff0c;又称静态工厂方法(Static Factory Method),属于创新型模式&#xff0c;但它不属于GoF23个设计模式其一。 一、模式动机&#xff1a; 有时需要创建一些来自相同父类的类的实例。 二、定义&#xff1a…
阅读更多...
(二十)、从宿主机访问 k8s(minikube) 发布的 redis 服务

(二十)、从宿主机访问 k8s(minikube) 发布的 redis 服务

文章目录 1、环境准备2、具体操作2.1、启动 minikube (start/stop)2.2、准备 redis-deployment.yaml2.3、执行 redis-deployment.yaml2.3.1、查看 pod 信息和日志 2.4、检查部署和服务状态2.4.1、如果需要删除 3、查看 IP 的几个命令3.1、查看IP的几个命令3.2、解读3.3、宿主机…
阅读更多...
【C语言】数据输出格式控制

【C语言】数据输出格式控制

数据的输出格式修饰 常用两种&#xff1a; 整型中&#xff0c;输出数据左对齐、右对齐、占m位、不足m位前补0。浮点型中&#xff0c;默认通过四舍五入保留小数点后6位&#xff0c;通过参数设置保留小数点后n位。 #include <stdio.h> #define PI 3.14159 /* 功能&#x…
阅读更多...
D43【python 接口自动化学习】- python基础之函数

D43【python 接口自动化学习】- python基础之函数

day43 装饰器&#xff08;上&#xff09; 学习日期&#xff1a;20241020 学习目标&#xff1a;函数&#xfe63;- 56 装饰器&#xff1a;函数嵌套的定义与调用的区别 学习笔记&#xff1a; 变量作用域 变量读取顺序&#xff1a;local-》enclosed-》global-》builtin # 变量…
阅读更多...
Spring MessageSource国际化原理

Spring MessageSource国际化原理

spring framework提供MessasgeSource来实现国际化。 MessageSource用法 准备properties文件&#xff0c;放在resources文件夹下面。这是默认语言和韩语的文件。 i18n/message.propertiesi18n/message_ko.properties 文件里面的内容是key-value格式,使用{0}、{1}作为变量占位…
阅读更多...
【Next.js 项目实战系列】05-删除 Issue

【Next.js 项目实战系列】05-删除 Issue

原文链接 CSDN 的排版/样式可能有问题&#xff0c;去我的博客查看原文系列吧&#xff0c;觉得有用的话&#xff0c;给我的库点个star&#xff0c;关注一下吧 上一篇【Next.js 项目实战系列】04-修改 Issue 删除 Issue 添加删除 Button​ 本节代码链接 这里我们主要关注布局…
阅读更多...
Win10 IDEA远程连接HBase

Win10 IDEA远程连接HBase

Win10 IDEA远程连接HBase Win10 IDEA连接虚拟机中的Hadoop&#xff08;HDFS&#xff09; 关闭Hadoop和Hbase 如果已经关闭不需要走这一步 cd /usr/local/hbase bin/stop-hbase.sh cd /usr/local/hadoop ./sbin/stop-dfs.sh获取虚拟机的ip 虚拟机终端输入 ip a关闭虚拟机…
阅读更多...
VS Code开发qt项目

VS Code开发qt项目

没整明白&#xff0c;尴尬 安装扩展 设置cmake路径 前提是已经安装了QT 报错 用msvc选windows启动&#xff0c;用mingw则选gdb启动
阅读更多...
Vue3 新特性、Pinia

Vue3 新特性、Pinia

一、新特性 - defineOptions 背景说明 因为我们用了<script setup>语法&#xff0c;没办法给setup去提供一些平级的属性&#xff0c;官方就提供了一个叫做 defineOptions 的语法 所以在 Vue3.3 中引入了 defineOptions 宏&#xff0c;用来定义 Options API 的选项。可以…
阅读更多...
[OpenCV] 数字图像处理 C++ 学习——17模板匹配详细讲解+附完整代码

[OpenCV] 数字图像处理 C++ 学习——17模板匹配详细讲解+附完整代码

文章目录 前言1.理论基础1.1模板匹配介绍1.2匹配算法介绍 2.代码实现2.1模块匹配(matchTemplate)2.2最佳匹配函数(minMaxLoc()) 3.完整代码 前言 模板匹配是图像处理和计算机视觉领域中的一种经典技术&#xff0c;它通过在大图像中搜索与小图像&#xff08;模板&#xff09;匹…
阅读更多...
gewechat免费开源微信机器人开发

gewechat免费开源微信机器人开发

​聊天机器人&#xff0c;是一种通过自然语言模拟人类进行对话的程序。通常运行在特定的软件平台上&#xff0c;如PC平台或者移动终端设备平台。聊天机器人系统的主要功能是同用户进行基本沟通并自动回复用户有关产品或服务的问题&#xff0c;以实现降低企业客服运营成本、提升…
阅读更多...
Tkinter 的布局pack() and grid()笔记

Tkinter 的布局pack() and grid()笔记

#encodingutf-8 import tkinter import re import tkinter.messagebox import tkinter.simpledialog import sys import os def get_resources_path(relative_path):if getattr(sys,frozen, False):base_pathsys._MEIPASS#获取临时文件else:base_pathos.path.dirname(".&q…
阅读更多...
鸿蒙应用开发----西西购物商城（一）

鸿蒙应用开发----西西购物商城（一）

目录 前言 一、项目介绍 二、项目结构 三、开发工具 四、样式展示 前言 harmonyos是华为推出的一款新一代操作系统&#xff0c;致力于打破设备间的边界&#xff0c;构建统一的智能生态。西西购物商城作为一款基于harmonyos开发的应用&#xff0c;能够利用鸿蒙的分布式技术…
阅读更多...
R语言绘制Venn图(文氏图、温氏图、维恩图、范氏图、韦恩图)

R语言绘制Venn图(文氏图、温氏图、维恩图、范氏图、韦恩图)

Venn图&#xff0c;又称文氏图&#xff0c;标题中其他名字也是它的别称&#xff0c;由封闭圆形组成&#xff0c;代表不同集合。圆形重叠部分表示集合交集&#xff0c;非重叠处为独有元素。在生物学、统计学等领域广泛应用&#xff0c;可展示不同数据集相似性与差异&#xff0c;…
阅读更多...
大数据新视界 --大数据大厂之大数据在智慧城市建设中的应用：打造智能生活的基石

大数据新视界 --大数据大厂之大数据在智慧城市建设中的应用：打造智能生活的基石

&#x1f496;&#x1f496;&#x1f496;亲爱的朋友们&#xff0c;热烈欢迎你们来到 青云交的博客&#xff01;能与你们在此邂逅&#xff0c;我满心欢喜&#xff0c;深感无比荣幸。在这个瞬息万变的时代&#xff0c;我们每个人都在苦苦追寻一处能让心灵安然栖息的港湾。而 我的…
阅读更多...
【MR开发】在Pico设备上接入MRTK3（一）——在Unity工程中导入MRTK3依赖

【MR开发】在Pico设备上接入MRTK3（一）——在Unity工程中导入MRTK3依赖

写在前面的话 在Pico上接入MRTK3&#xff0c;目前已有大佬开源。 https://github.com/Phantomxm2021/PicoMRTK3 也有值得推荐的文章。 MRTK3在PICO4上的使用小结 但由于在MacOS上使用MRTK3&#xff0c;无法通过Mixed Reality Feature Tool工具管理MRTK3安装包。 故记录一下…
阅读更多...
集合collection和泛型

集合collection和泛型

collection可以直接打印内容&#xff0c;而不是地址&#xff0c;内部已经重写了。 List家族&#xff1a; package com.itheima.d6_collection_update_delete;import java.util.ArrayList; import java.util.Iterator; import java.util.List;/**目标&#xff1a;研究集合遍历并…
阅读更多...
Fooocus模型部署指南

Fooocus模型部署指南

一、介绍 Fooocus是一款开源的AI绘画工具&#xff0c;由斯坦福大学博士生张吕敏&#xff08;Github上用户名lllyasviel&#xff09;开发。是一款结合StableDiffusion和Midjourney理念的图像生成工具&#xff0c;提供离线、开源且易于使用的界面。它简化安装步骤&#xff0c;用…
阅读更多...
【软件测试】JUnit

【软件测试】JUnit

Junit 是一个用于 Java 编程语言的单元测试框架&#xff0c;Selenium是自动化测试框架&#xff0c;专门用于Web测试 本篇博客介绍 Junit5 文章目录 Junit 使用语法注解参数执行顺序断言测试套件 Junit 使用 本篇博客使用 Idea集成开发环境 首先&#xff0c;创建新项目&#…
阅读更多...
【深度学习】评价指标

【深度学习】评价指标

https://zhuanlan.zhihu.com/p/479060683 https://blog.csdn.net/fyfugoyfa/article/details/136414958 Confusion Matrix&#xff0c;是一个用来衡量模型好坏的统计量&#xff0c;其中Accuracy&#xff0c;Precision&#xff0c;F1 score&#xff0c;Recall&#xff0c;ROC曲线…
阅读更多...
最新文章

Copyright @ 2022~2023