PCL-基于FPFH的SAC-IA结合ICP的点云配准方法

目录

  • 一、相关方法原理
    • 1.凸包方法
    • 2.FPFH特征描述
    • 3.SAC-IA概述
    • 4.ICP概述
  • 二、实验代码
  • 三、实验结果

一、相关方法原理

点云是在同一空间参考系下表达目标空间分布和目标表面特性的海量点集合，在获取物体表面每个采样点的空间坐标后，得到的是点的集合，称之为点云（Point Cloud）。点云配准（Point Cloud Registration）指的是输入同一时刻采集到的两幅处于不同坐标系下的点云点集Pt和Ps，输出一个变换T，使得T（Ps）和Pt的重合程度尽可能高，其中，变换T可分为刚性变换和非刚性变换。
点云配准过程可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。
在粗配准阶段，首先使用凸包算法计算待配准点云和目标点云的凸包顶点，而后计算两幅点云中各顶点的FPFH特征，最后根据各点的FPFH特征利用SAC-IA算法求出变换矩阵，完成粗配准；在精配准阶段，在粗配准的基础上，使用ICP算法对粗配准得到的变换矩阵做进一步的优化，得到最优结果，完成配准过程。
本博客所使用的点云配准方法流程如下图（图中粗配准和精配准过程写反了哈）所示
在这里插入图片描述

1.凸包方法

凸包（Convex Hull）是一个计算几何中的概念。在一个实数向量空间V中，对于给定集合X，所有包含X的凸集的交集S被称为X的凸包。X的凸包可以用X内所有点(X1，…Xn)的线性组合来构造。在二维平面上的点集，其凸包就是将最外层的点连接起来构成的凸多边形，如图2-1所示，它能包含点集中所有的点。
在这里插入图片描述
图2-1 二维平面凸包
对于三维空间中点集，其凸包则是凸多面体。对于如图2-2所示点云，其凸包如2-3所示，其中红色点为凸包顶点。在后续粗配准过程中，将提取到的凸包顶点作为新的待配准点云和目标点云进行配准。
在这里插入图片描述
图2-2原始点云
在这里插入图片描述
图2-3 原始点云对应凸包

2.FPFH特征描述

点快速特征直方图(Fast Point Feature Histogram, FPFH)通常用于描述三维点的局部特征，通过参数化查询点与紧邻点之间的空间差异，形成多维直方图对点的近邻进行几何描述。

3.SAC-IA概述

采样一致性初始配准算法（Sample Consensus Initial Aligment , SAC-IA)是一种依赖于FPFH特征的配准算法，在执行此算法之前，需要先计算点云各点的FPFH特征，算法的大致流程如下：
（1）从待配准点云中选取n个（n≥3）采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d。
（2）在目标点云中查找与待配准点云中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为待配准点云在目标点云中的一一对应点。
（3）使用SVD分解求解待配准点集与目标点集之间的变换矩阵。
（4）计算此时待配准点云与目标点云之间的误差。
这里引用Huber损失函数来描述变换后的“距离误差和”，函数如下所示：
在这里插入图片描述

其中mi为预先设定的阈值，li为对应点经过变换后的距离误差，上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换矩阵即为粗配准过程中所求的变换矩阵。

4.ICP概述

ICP（迭代最近点）算法主要应用于点云数据的配准，通过构建最小二乘法目标函数，求解坐标转换关系（旋转矩阵R和平移向量t），将连续扫描的两帧或多帧点云数据统一到同一坐标系中，或是将扫描的点云与已经建立好的地图进行配准。
ICP算法步骤如下：
（1）选取待配准点集与目标点集。对于点数量较多的点云，可以选择部分点作为待配准点集。对于点的选取，通常使用滤波方法，或基于旋转不变等特性，提取点云的特征点作为待配准点集。
（2）依次遍历待配准点云，从目标点集中选择欧式距离最近的点，组成对应点对。
（3）根据对应点对，利用最小二乘法构建目标函数，而后利用SVD分解求解变换矩阵。
（4）根据求得的变换矩阵对待配准点云进行欧式变换，并将得到的新的点云作为待配准点云。
（5）判断是否满足停止迭代条件，条件可以是达到最大迭代次数，或两次迭代中误差优化的效果小于设定阈值，或本次迭代中误差小于设定阈值。做满足某一条件则停止迭代并输出结果，否则进行下一次迭代。

二、实验代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ia_ransac.h>//采样一致性
#include <pcl/registration/icp.h>//icp配准
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <pcl/surface/convex_hull.h>
#include <time.h>//时间
#include <pcl/search/kdtree.h>using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_mid,PointCloud::Ptr pcd_final)
{//创建初始化目标pcl::visualization::PCLVisualizer viewer("registration Viewer");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> mid_h(pcd_final, 0, 255, 255);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud(pcd_src, src_h, "source cloud");viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");viewer.addPointCloud(pcd_mid,mid_h,"mid cloud");viewer.addPointCloud(pcd_final, final_h, "final cloud");while (!viewer.wasStopped()){viewer.spinOnce(100);}
}int main(int argc, char** argv)
{//加载点云文件PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准pcl::io::loadPCDFile("../bunny_src.pcd", *cloud_src_o);PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云pcl::io::loadPCDFile("../bunny_tgt.pcd", *cloud_tgt_o);clock_t start = clock();clock_t hull_start= clock();pcl::ConvexHull<pcl::PointXYZ> src_hull;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_hull_src(new pcl::search::KdTree<pcl::PointXYZ>());tree_hull_src->setInputCloud(cloud_src_o);src_hull.setSearchMethod(tree_hull_src);src_hull.setInputCloud(cloud_src_o);src_hull.setDimension(3);std::vector<pcl::Vertices> src_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr src_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);src_hull.reconstruct(*src_surface_hull, src_polygons);pcl::ConvexHull<pcl::PointXYZ> tgt_hull;tgt_hull.setInputCloud(cloud_tgt_o);tgt_hull.setDimension(3);std::vector<pcl::Vertices> tgt_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);tgt_hull.reconstruct(*tgt_surface_hull, tgt_polygons);clock_t hull_end = clock();clock_t fpfh_start = clock();//计算表面法线pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;ne_src.setInputCloud(src_surface_hull);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());ne_src.setSearchMethod(tree_src);pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);ne_src.setRadiusSearch(0.02);ne_src.compute(*cloud_src_normals);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;ne_tgt.setInputCloud(tgt_surface_hull);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());ne_tgt.setSearchMethod(tree_tgt);pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);ne_tgt.setRadiusSearch(0.02);ne_tgt.compute(*cloud_tgt_normals);//计算FPFHpcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;fpfh_src.setInputCloud(src_surface_hull);fpfh_src.setInputNormals(cloud_src_normals);pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);fpfh_src.setSearchMethod(tree_src_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_src.setRadiusSearch(0.05);fpfh_src.compute(*fpfhs_src);std::cout << "compute *cloud_src fpfh" << endl;pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;fpfh_tgt.setInputCloud(tgt_surface_hull);fpfh_tgt.setInputNormals(cloud_tgt_normals);pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);fpfh_tgt.setSearchMethod(tree_tgt_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_tgt.setRadiusSearch(0.05);fpfh_tgt.compute(*fpfhs_tgt);std::cout << "compute *cloud_tgt fpfh" << endl;clock_t fpfh_end = clock();clock_t sac_start = clock();//SACpcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;scia.setInputSource(src_surface_hull);scia.setInputTarget(tgt_surface_hull);scia.setSourceFeatures(fpfhs_src);scia.setTargetFeatures(fpfhs_tgt);//scia.setMinSampleDistance(1);//scia.setNumberOfSamples(2);//scia.setCorrespondenceRandomness(20);PointCloud::Ptr sac_result(new PointCloud);scia.align(*sac_result);std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore()<< endl;Eigen::Matrix4f sac_trans;sac_trans = scia.getFinalTransformation();std::cout << sac_trans << endl;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_mid(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud_src_o, *cloud_mid, sac_trans);/*pcl::ConvexHull<pcl::PointXYZ> mid_hull;tgt_hull.setInputCloud(cloud_mid);tgt_hull.setDimension(3);std::vector<pcl::Vertices> mid_polygons;pcl::PointCloud<pcl::PointXYZ>::Ptr mid_surface_hull(new pcl::PointCloud<pcl::PointXYZ>);tgt_hull.reconstruct(*mid_surface_hull, mid_polygons);*/clock_t sac_end = clock();clock_t icp_start = clock();//icp配准pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_icp_mid(new pcl::search::KdTree<pcl::PointXYZ>);tree_icp_mid->setInputCloud(cloud_mid);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_icp_tgt(new pcl::search::KdTree<pcl::PointXYZ>);tree_icp_tgt->setInputCloud(cloud_tgt_o);icp.setSearchMethodSource(tree_icp_mid);icp.setSearchMethodTarget(tree_icp_tgt);icp.setInputSource(cloud_mid);icp.setInputTarget(cloud_tgt_o);// 最大迭代次数icp.setMaximumIterations(20);// 均方误差icp.setEuclideanFitnessEpsilon(0.01);PointCloud::Ptr icp_result(new PointCloud);icp.align(*icp_result, sac_trans);std::cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;Eigen::Matrix4f icp_trans;icp_trans = icp.getFinalTransformation();//cout<<"ransformationProbability"<<icp.getTransformationProbability()<<endl;std::cout << icp_trans << endl;pcl::transformPointCloud(*cloud_mid, *icp_result, icp_trans);clock_t icp_end = clock();clock_t end = clock();cout << "hull time:" << (double)(hull_end - hull_start) / CLOCKS_PER_SEC << " s" << endl;cout << "fpfh time:" << (double)(fpfh_end - fpfh_start) / CLOCKS_PER_SEC << " s" << endl;cout << "sac time:" << (double)(sac_end - sac_start) / CLOCKS_PER_SEC << " s" << endl;cout << "icp time:" << (double)(icp_end - icp_start) / CLOCKS_PER_SEC << " s" << endl;cout << "time:" << (double)(end - start) / CLOCKS_PER_SEC << " s" << endl;pcl::io::savePCDFileASCII("cloud_src_hull.pcd", *src_surface_hull);pcl::io::savePCDFileASCII("cloud_tgt_hull.pcd", *tgt_surface_hull);pcl::io::savePCDFileASCII("cloud_mid.pcd", *cloud_mid);pcl::io::savePCDFileASCII("cloud_result.pcd", *icp_result);//可视化visualize_pcd(cloud_src_o, cloud_tgt_o, cloud_mid,icp_result);return 0;
}

三、实验结果

a. 原点云
在这里插入图片描述
b. 点云凸包顶点
在这里插入图片描述
c. 点云粗配准结果与变换矩阵如下：
在这里插入图片描述
在这里插入图片描述
d. 点云精配准结果与变换矩阵如下：
在这里插入图片描述
在这里插入图片描述

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

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

相关文章

【java技术】xxl-job的实现

【java技术】xxl-job的实现

Xxl-Job 是一个轻量级的分布式任务调度平台&#xff0c;它支持定时任务的创建、管理、执行和监控。Xxl-Job 的设计理念是简单易用、轻量级、高性能&#xff0c;适合于微服务架构下的任务调度场景。 Xxl-Job 的实现原理涉及到几个关键组件和技术细节。下面是 Xxl-Job 的核心组件…
阅读更多...
构建智能运维系统：创新架构与效率优化

构建智能运维系统：创新架构与效率优化

随着信息技术的迅猛发展&#xff0c;企业对于运维效率和服务质量的要求越来越高。智能运维系统的设计和实施&#xff0c;不仅能够提升系统可靠性和响应速度&#xff0c;还能有效降低成本和人力投入。本文将深入探讨智能运维系统的架构设计原则和关键技术&#xff0c;为企业在运…
阅读更多...
数据结构重置版（概念篇）

数据结构重置版（概念篇）

本篇文章是对数据结构的重置&#xff0c;且只涉及概念 顺序表与链表的区别 不同点 顺序表 链表 存储空间上 物理上一定连续 逻辑上连续&#xff0c;但物理上不一定连续…
阅读更多...
MYSQL(2) 高级查询

MYSQL(2) 高级查询

文章目录 概述高级查询基础查询条件查询范围查询判空查询模糊查询分页查询查询后排序分组查询 小结 概述 接上篇&#xff0c;上篇写到增删改查。这篇继续。 高级查询 基础查询 -- 全部查询 select * from student; -- 只查询部分字段 select sname, class_id from student;…
阅读更多...
.env.local 配置本地环境变量 用于团队开发

.env.local 配置本地环境变量 用于团队开发

.env.local 用途&#xff1a;.env.local 通常用于存储本地开发环境中的环境变量。这些变量可能包括敏感数据或特定于单个开发者的设置&#xff0c;不应该被提交到版本控制系统中。优先级&#xff1a;在大多数框架中&#xff0c;.env.local 文件中的变量会覆盖其他 .env 文件中…
阅读更多...
Java唯一订单编号生成

Java唯一订单编号生成

在Java中生成唯一的订单编号通常需要结合时间戳、随机数和/或序列号等元素来确保唯一性。下面是一个简单的示例&#xff0c;使用当前时间的毫秒值加上一个随机数来生成订单号。为了简化&#xff0c;我们将使用​​java.util.UUID​​类来生成一个全局唯一的UUID&#xff0c;并将…
阅读更多...
分类模型的完整流程及Python实现

分类模型的完整流程及Python实现

1、加载函数和数据集 import numpy as np from sklearn.datasets import load_breast_cancer from sklearn.svm import SVC from sklearn.model_selection import train_test_split from sklearn.preprocessing import StandardScaler import matplotlib.pyplot as plt cancer…
阅读更多...
linux系统查历史cpu使用数据（使用sar 查询cpu和网络占用最近1个月历史数据）。

linux系统查历史cpu使用数据（使用sar 查询cpu和网络占用最近1个月历史数据）。

一 sar 指令介绍 在 Linux 系统中&#xff0c;sar 是 System Activity Reporter 的缩写&#xff0c;是一个用于收集、报告和保存系统活动信息的工具。它是 sysstat 软件包的一部分&#xff0c;提供了丰富的系统性能数据&#xff0c;包括 CPU、内存、网络、磁盘等使用情况&am…
阅读更多...
Jdk有哪些版本

Jdk有哪些版本

JDK（Java Development Kit）是Java编程语言的软件开发工具包，其版本随着Java语言的不断发展而更新。以下是JDK的一些主要版本及其相关信息： JDK 8（发布于2014年3月）：引入了一系列新功能，如Lambda表达式、函数式接口、Stream API和新的日期/时间API等。是Java历史上一个…
阅读更多...
SQL中的LEFT JOIN、RIGHT JOIN和INNER JOIN

SQL中的LEFT JOIN、RIGHT JOIN和INNER JOIN

在SQL中&#xff0c;JOIN操作是连接两个或多个数据库表&#xff0c;并根据两个表之间的共同列&#xff08;通常是主键和外键&#xff09;返回数据的重要方法。其中&#xff0c;LEFT JOIN&#xff08;左连接&#xff09;、RIGHT JOIN&#xff08;右连接&#xff09;和INNER JOIN…
阅读更多...
《JavaEE篇》--多线程(2)

《JavaEE篇》--多线程(2)

《JavaEE篇》--多线程(1) 线程安全 线程不安全 我们先来观察一个线程不安全的案例&#xff1a; public class Demo {private static int count 0;public static void main(String[] args) throws InterruptedException {Thread t1 new Thread(() -> {//让count自增5W次…
阅读更多...
HarmonyOS网络请求的简单用法，HttpUtil简单封装

HarmonyOS网络请求的简单用法，HttpUtil简单封装

请求网络获取数据 点击按钮发送一个post请求&#xff0c;发送一条string由于此处的返回result.data本身就是一个string&#xff0c;因此不需要转换类型 Button(请求网络).margin({ top: 10 }).fontSize(24).fontWeight(FontWeight.Bold).onClick(() > {httpRequestPost(http…
阅读更多...
JVM 内存结构、垃圾回收机制与并发容器

JVM 内存结构、垃圾回收机制与并发容器

目录 一、JVM 内存结构 1. 程序计数器&#xff08;Program Counter Register&#xff09;&#xff1a; 2. Java 虚拟机栈&#xff08;JVM Stack&#xff09;&#xff1a; 3.本地方法栈&#xff08;Native Method Stack&#xff09;&#xff1a; 4.堆&#xff08;Heap&#xff…
阅读更多...
Android笔试面试题AI答之控件Views（6）

Android笔试面试题AI答之控件Views（6）

答案来着文心一言&#xff0c;仅供参考 目录 1.简述什么是RemoteViews?使用场景有哪些?RemoteViews的特性使用场景总结 2.获取View宽高的几种方法?1. 在onWindowFocusChanged方法中获取2. 使用ViewTreeObserver.OnGlobalLayoutListener3. 使用ViewTreeObserver.OnPreDrawLi…
阅读更多...
风格迁移开发记录（DCT-Net）

风格迁移开发记录（DCT-Net）

1.DCT-Net部署 阿里旗下的 modelscope社区&#xff0c;丰富的开源风格迁移算法模型 DCT-Net GitHub链接 git clone https://github.com/menyifang/DCT-Net.git cd DCT-Netpython run_sdk.py下载不同风格的模型如下图每个文件夹代表一种风格&#xff0c;有cartoon_bg.pb, car…
阅读更多...
C++STL详解（一）——String接口详解（上）！！！

C++STL详解（一）——String接口详解（上）！！！

目录 一.string类介绍 二.string类的构造赋值 2.1string类的拷贝和构造函数 2.2深拷贝 三.string类的插入 3.1push_back 3.2append 3.3操作符 3.4insert 四.string的删除 4.1pop_back 4.2erase 五.string的查找 5.1find 5.2rfind 六.string的比较 6.1compare函…
阅读更多...
独家｜二十年国货羊奶粉老品牌发力成人奶粉，瞄准低GI、特医食品

独家｜二十年国货羊奶粉老品牌发力成人奶粉，瞄准低GI、特医食品

前言 中国羊奶看陕西。 作为陕西省农业产业化重点企业以及陕西省专精特新企业&#xff0c;成立于2004年的羊奶粉品牌雅泰乳业正在不断进行深入布局。 雅泰乳业成人粉部门销售总监于维涛近日向AgeFood表示&#xff0c;雅泰成人奶粉业务主要分为两部分。一部分是以雅泰牧歌、龙…
阅读更多...
深入浅出WebRTC—Pacer

深入浅出WebRTC—Pacer

平滑发包&#xff08;Pacer&#xff09;是 WebRTC 实现高质量实时通信不可或缺的一部分。在视频通信中&#xff0c;单帧视频可能包含大量的数据&#xff0c;如果未经控制地立即发送&#xff0c;可能瞬间对网络造成巨大压力。Pacer 能够根据网络条件动态调整发送速率&#xff0c…
阅读更多...
SpringBoot事务管理、任务调度、Mail整合。

SpringBoot事务管理、任务调度、Mail整合。

一.Spring Boot中的事务管理 编程式事务 : 在代码中硬编码(不推荐使用):通过 TransactionTemplate 或者 TransactionManager 手动管理事务&#xff0c;实际应用中很少使用&#xff0c;用于理解Spring 事务管理。 声明式事务:在 XML 配置文件或者基于注解 Transactional(推荐使…
阅读更多...
Java实现汉字转拼音工具类的编写与应用

Java实现汉字转拼音工具类的编写与应用

前言 在处理中文数据时&#xff0c;经常需要将汉字转换为拼音&#xff0c;无论是为了搜索优化、数据分析还是提升用户体验。本文将详细介绍如何编写一个实用的Java工具类来实现这一功能&#xff0c;并通过一个完整的示例来展示其使用方法。我们将使用Apache Commons Lang库中的…
阅读更多...
最新文章

Copyright @ 2022~2023