PCL-基于SAC_IA和NDT结合的点云配准算法

news2024/9/21 18:51:49

  • 一、原理概述
    • 1.点云配准流程图
    • 2.快速点特征直方图FPFH
    • 3.采样一致性SAC_IA粗配准
    • 4.正态分布变换NDT精配准
  • 二、实验代码
  • 三、实验结果
  • 四、总结
  • 五、参考

一、原理概述

1.点云配准流程图

在这里插入图片描述

2.快速点特征直方图FPFH

快速点特征直方图（Fast Point Feature Histogram，FPFH）是一种用于点云数据描述和匹配的特征表示方法。它是基于点云中每个点的局部几何信息来计算的。

FPFH的计算过程包括以下几个步骤：

  1. 首先需要确定每个点最近邻点集合。可以使用kd树等数据结构来加速最近邻搜索过程。
  2. 对于每个点，计算其与最近邻点之间的相对位置和法线信息。这可以通过计算点与其最近邻点之间的距离、角度等来实现。同时，为了保持旋转不变性，可以使用点云曲率法线估计方法来计算点的法线向量。
  3. 其次，建立一个直方图来描述每个点与最近邻点之间的关系。直方图的每个条目表示一种关系，比如距离、角度等。直方图的bin数量和范围可以根据具体需求进行选择。
  4. 然后，将每个点的直方图与最近邻点的直方图进行组合。这可以通过将两个直方图的值进行加权平均来实现，其中权重可以根据点之间的距离、角度等进行选择。
  5. 最后，将每个点的组合直方图作为其特征表示。这个特征向量可以用于点云的描述、匹配和识别等任务。
    FPFH具有以下特点：
    局部性：FPFH是一种局部特征表示方法，它主要描述了点与其最近邻点之间的几何关系。
    鲁棒性：FPFH对于点云的旋转、尺度和噪声具有一定的鲁棒性。
    可扩展性：FPFH可以与其他点云特征描述方法结合使用，例如法线特征、曲率特征等，以提高点云的表达能力和鉴别能力。

3.采样一致性SAC_IA粗配准

采样一致性（Sample Consensus，SAC）是一种常用的点云配准方法，它通过采样一致性评估模型和数据之间的一致性来估计最佳的刚体变换参数。SAC-IA（Sample Consensus Initial Alignment）是SAC的一种变种，用于进行粗配准（coarse registration）。
SAC_IA其算法步骤如下：
1）对采样点进行选取。在待配准点云M中选取ｎ个采样点，并且要满足采样点两两之间的距离大于预先设定的最小距离阈值ｄ，用以保证各采样点的FPFH特征均不相同。
2）对采样点的对应点进行查找。依据采样点 的FPFH特征，在目标点云N中通过近邻搜索找到与采样点中每个点FPFH相差最小的点，即为 特征相似的点，将其作为目标点云N 中与待配准 点 云M中的 采样点相对应的点，并通过RANSAC算法剔除错误的对应点对，从而保证相似特征点对的正确匹配。
3）求解变换关系。首先求出对应点之间的变换矩阵，然后依据该变换计算对应点变换后的距离误差和函数，并将此函数作为评判配准性能的指标。
SAC-IA粗配准方法的优点在于它能够处理初始对齐差距较大的点云数据，并且具有较好的鲁棒性。然而，由于采样的随机性，SAC-IA可能存在局部最优解的问题，因此在需要更高精度的配准任务中，可能需要结合其他的配准方法进行进一步的优化。

4.正态分布变换NDT精配准

在SAC-IA初始配准中，通过优化刚体变换参数，使得待配准的两个点云位置尽可能地靠近，从而缩小点云之间的旋转和平移误差，实现较好的初始位置对齐。然后可以使用N-Dimensional Transformation (NDT) 配准方法进一步提高配准的精度。
NDT（N-Dimensional Transformation）是一种基于概率的点云配准方法，通过将三维点云分割为大小均匀的立方体，并将每个立方体中的点云数据转换成概率密度函数来进行配准。相对于ICP（Iterative Closest Point）算法，NDT方法不需要每次迭代都重新匹配对应点对，因此具有较高的配准效率。

下面是NDT算法的主要步骤：
a. 把点云模型划分为均匀大小的立方体

b. 求出各个立方体中点的均值向量q和协方差矩阵C:
在这里插入图片描述
式中，n为立方体中点的总数，Xk为立方体中的一点。
c. 对点Xk进行正态分布建模Ｎ(ｑ，Ｃ)，其中Xk的概率密度函数表示为：
在这里插入图片描述
d. 根据变换矩阵T对待配准点云的每个点进行变换，然后依据变换后的点T(Xi)所在的立方体的概率密度分布函数计算相应的概率分布函数。
e. 把各个待配准点的相应概率分布相乘，并将相乘的结果作为变换矩阵T的分数值s§：
在这里插入图片描述
f. 依据 Hessian矩阵法计算s§的最优解，从而得出最佳变换矩阵。

二、实验代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>    // 体素下采样滤波
#include <pcl/features/normal_3d_omp.h>// 使用OMP需要添加的头文件
#include <pcl/features/fpfh_omp.h>     // fpfh加速计算的omp(多核并行计算)
#include <pcl/registration/ia_ransac.h>// sac_ia算法
#include <pcl/registration/ndt.h>      // NDT配准算法
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;

#pragma region// 下采样滤波
pointcloud::Ptr voxel_grid_fiter(pointcloud::Ptr& inCloud)
{
    pcl::VoxelGrid<pcl::PointXYZ> vs;
    vs.setLeafSize(0.005, 0.005, 0.005);
    //vs.setLeafSize(10.005, 10.005, 10.005);
    vs.setInputCloud(inCloud);
    pointcloud::Ptr outCloud(new pointcloud);
    vs.filter(*outCloud);
  
    return outCloud;
    
}
// 计算FPFH特征
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr inCloud)
{
    // 法向量估计
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
    pointnormal::Ptr normals(new pointnormal);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(inCloud);
    n.setNumberOfThreads(8);//设置openMP的线程数
    n.setSearchMethod(tree);
    n.setKSearch(10);
    n.compute(*normals);
    //------------------FPFH估计-------------------------
    fpfhFeature::Ptr fpfh(new fpfhFeature);
    pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8); //指定8核计算
    f.setInputCloud(inCloud);
    f.setInputNormals(normals);
    f.setSearchMethod(tree);
    f.setKSearch(10);
    f.compute(*fpfh);

    return fpfh;

}

// 可视化
void  visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& regist)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Registration"));
    int v1 = 0;
    int v2 = 1;
    viewer->setWindowName("SAC-IA+NDT配准结果");
    viewer->createViewPort(0, 0, 0.5, 1, v1);
    viewer->createViewPort(0.5, 0, 1, 1, v2);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->setBackgroundColor(0.05, 0, 0, v2);
    viewer->addText("Raw point clouds", 10, 10, "v1_text", v1);
    viewer->addText("Registed point clouds", 10, 10, "v2_text", v2);
    //原始点云绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 0, 255, 0);
    //目标点云蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);
    //转换后的源点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(regist, 255, 0, 0);
    viewer->setBackgroundColor(255, 255, 255);
    viewer->addPointCloud(source, src_h, "source cloud", v1);
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);

    viewer->addPointCloud(target, tgt_h, "target cloud1", v2);
    viewer->addPointCloud(regist, transe, "pcs cloud", v2);

    //添加坐标系
    //viewer->addCoordinateSystem(0.1);
    //viewer->initCameraParameters();
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }

}
#pragma endregion
int main(int argc, char** argv)
{
    // 加载点云数据
    pointcloud::Ptr source_cloud(new pointcloud);
    pointcloud::Ptr target_cloud(new pointcloud);
   /* pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\******.pcd", *source_cloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\******0.pcd", *target_cloud);*/
     pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\*****\\ArmadilloOnHeadMultiple_0.pcd", *source_cloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\*****\\ArmadilloOnHeadMultiple_30.pcd", *target_cloud);
    if (source_cloud->empty() || target_cloud->empty())
    {
        cout << "请确认点云文件名称是否正确" << endl;
        return -1;
    }
	else
	{
		cout << "从目标点云读取 " << source_cloud->size() << " 个点" << endl;
		cout << "从源点云中读取 " << target_cloud->size() << " 个点" << endl;
	}
    pointcloud::Ptr source(new pointcloud);
    pointcloud::Ptr target(new pointcloud);

    source = voxel_grid_fiter(source_cloud);
    cout << "source提取的特征点个数:" << source->points.size() << endl;
    target = voxel_grid_fiter(target_cloud);
    cout << "target提取的特征点个数:" << target->points.size() << endl;


    // 1、计算源点云和目标点云的FPFH
    fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source);
    fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target);

    // 2、采样一致性SAC_IA初始配准
    clock_t start, end;
    start = clock();
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(source);
    sac_ia.setSourceFeatures(source_fpfh);
    sac_ia.setInputTarget(target);
    sac_ia.setTargetFeatures(target_fpfh);
    sac_ia.setMinSampleDistance(0.007);       // 设置样本之间的最小距离
    sac_ia.setMaxCorrespondenceDistance(0.1);// 设置对应点对之间的最大距离
    sac_ia.setNumberOfSamples(100);          // 设置每次迭代计算中使用的样本数量（可省）,可节省时间
    sac_ia.setCorrespondenceRandomness(6);   // 设置在6个最近特征对应中随机选取一个
    pointcloud::Ptr align(new pointcloud);
    sac_ia.align(*align);
    Eigen::Matrix4f initial_RT = Eigen::Matrix4f::Identity();// 定义初始变换矩阵
    initial_RT = sac_ia.getFinalTransformation();
    cout << "\nSAC_IA has converged," << sac_ia.hasConverged() << " score is " << sac_ia.getFitnessScore() << endl;
    cout << "SAC_IA初始配准变换矩阵：\n" << initial_RT << endl;


    // 3、正态分布变换（NDT）
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setInputSource(source);	             // 设置要配准的点云
    ndt.setInputTarget(target);              // 设置点云配准目标
    ndt.setStepSize(4);                      // 为More-Thuente线搜索设置最大步长
    ndt.setResolution(0.01);                 // 设置NDT网格结构的分辨率（VoxelGridCovariance）
    ndt.setMaximumIterations(35);            // 设置匹配迭代的最大次数
    ndt.setTransformationEpsilon(0.01);      // 为终止条件设置最小转换差异
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, initial_RT);    // align()函数有第二个参数，输入的是初始变换的估计参数
    end = clock();
    cout << "NDT has converged:" << ndt.hasConverged()
        << " score: " << ndt.getFitnessScore() << endl;
    cout << "正态分布变换变换矩阵：\n" << ndt.getFinalTransformation() << endl;
    cout << "运行时间: " << float(end - start) / CLOCKS_PER_SEC << "s" << endl;
    // 4、使用变换矩阵对未进行滤波的原始源点云进行变换
    pcl::transformPointCloud(*source_cloud, *output_cloud, ndt.getFinalTransformation());

    // 5、可视化
    visualize_registration(source_cloud, target_cloud, output_cloud);

    return 0;
}

三、实验结果

下图，绿色为源点云，蓝色为目标点云，红色为配准后的点云
在这里插入图片描述

在这里插入图片描述

四、总结

数据采用的斯坦福大学的Armadil，从配准后的效果来看，基本上能正确配准，但在其手部、脚部等一些区域，没有正确配准，最后的配准精度在4.8693e-05。因为NDT是基于概率的方法计算，精确性不高且稳定性也不高。若感兴趣，可以继续调整一些参数或者加以改进来提高精度。

五、参考

[1]荆路,武斌,李先帅.基于SAC-IA和NDT融合的点云配准方法[J].大地测量与地球动力学,2021,41(04):378-381.
浅谈FPFH算法实现原理及其在点云配准中的应用
基于SAC_IA和NDT融合的点云配准方法

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

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

相关文章

Oracle SQL：了解执行计划和性能调优

Oracle SQL：了解执行计划和性能调优

查询优化类似于制作完美食谱的艺术——它需要对成分&#xff08;数据&#xff09;、厨房&#xff08;数据库系统&#xff09;和使用的技术&#xff08;查询优化器&#xff09;有深入的了解。每个数据库系统都有自己的处理和运行 SQL 查询的方式&#xff0c;“解释”计划向我们展…
阅读更多...
Mysql注意事项（一）

Mysql注意事项（一）

Mysql注意事项&#xff08;一&#xff09; 最近回顾了一下MySQL&#xff0c;发现了一些MySQL需要注意的事项&#xff0c;同时也作为学习笔记&#xff0c;记录下来。–2020年05月13日 1、通配符* 检索所有的列。 不建议使用 通常&#xff0c;除非你确定需要表中的每个列&am…
阅读更多...
每日刷题记录（codetop版）

每日刷题记录（codetop版）

7.21 7.22 7.23 复习7.21和7.22
阅读更多...
每日OJ_牛客DD1 连续最大和

每日OJ_牛客DD1 连续最大和

目录 牛客DD1 连续最大和 解析代码 牛客DD1 连续最大和 连续最大和_牛客题霸_牛客网 解析代码 本题是一个经典的动规问题&#xff0c;简称dp问题&#xff0c;但这个问题是非常简单的dp问题&#xff0c;而且经常会考察&#xff0c;所以一定要把这个题做会。本题题意很简单&am…
阅读更多...
探寻安全新时代：叉车AI智能影像防撞系统，守护生命之光

探寻安全新时代：叉车AI智能影像防撞系统，守护生命之光

在繁忙的工业现场&#xff0c;叉车司机常常面临着视线受阻的困境&#xff0c;那些被货物遮挡的盲区&#xff0c;仿佛隐藏着无法预知的危险。然而&#xff0c;这样的隐患在一次惨痛的事故中暴露无遗&#xff0c;一名无辜的行人因叉车司机的视线受阻而不幸被撞身亡。这起悲剧让我…
阅读更多...
机械设计基础B(学习笔记)

机械设计基础B(学习笔记)

绪论 机构&#xff1a;是一些具备各自特点的和具有确定的相对运动的基本组合的统称。 组成机构的各个相对运动部分称为构件。构件作为运动单元&#xff0c;它可以是单一的整体&#xff0c;也可以是由几个最基本的事物&#xff08;通常称为零件&#xff09;组成的刚性结构。 构件…
阅读更多...
python·数据分析基础知识

python·数据分析基础知识

numpy 一个数值计算包 python列表与numpy矩阵区别 python中修改列表元素和列表相加 for循环 &#xff1a;[x1 for x in a] 多个元素需要用zip捆绑&#xff1a;[xy for(x,y) in zip(a,b)] numpy矩阵自动进行相应元素计算 np.array()1各元素1 ab各元素相加 a*b矩阵相乘或者是…
阅读更多...
爬虫学习4：爬取王者荣耀技能信息

爬虫学习4：爬取王者荣耀技能信息

爬虫&#xff1a;爬取王者荣耀技能信息&#xff08;代码和代码流程&#xff09; 代码 # 王者荣耀英雄信息获取 import time from selenium import webdriver from selenium.webdriver.common.by import By if __name__ __main__:fp open("./honorKing.txt", "…
阅读更多...
C++的UI框架和开源项目介绍

C++的UI框架和开源项目介绍

文章目录 1.QT2.wxWidgets3.Dear ImGui 1.QT QT的开源项目&#xff1a;QGIS&#xff08;地理信息系统&#xff09; https://github.com/qgis/QGIS?tabreadme-ov-file 2.wxWidgets wxWidgets的开源项目&#xff1a;filezilla https://svn.filezilla-project.org/svn/ wxWidg…
阅读更多...
Matplotlib折线图绘制秘籍：让你的数据线条比过山车还刺激！

Matplotlib折线图绘制秘籍：让你的数据线条比过山车还刺激！

1. Matplotlib_折线图 折线图&#xff08;line chart&#xff09;是我们日常工作中经常使用的一种图表&#xff0c;它可以直观的反应数据的变化趋势 # 导包 import numpy as np import pandas as pd import matplotlib.pyplot as plt# 如果浏览器不显示图片&#xff0c;就需要…
阅读更多...
面试场景题系列--（1）如果系统的 QPS 突然提升 10 倍该怎么设计？--xunznux

面试场景题系列--（1）如果系统的 QPS 突然提升 10 倍该怎么设计？--xunznux

1. 如果系统的 QPS 突然提升 10 倍该怎么设计&#xff1f; 1.1 硬件的扩展微服务的拆分 如果所有的业务包括交易系统、会员信息、库存、商品等等都夹杂在一起&#xff0c;当流量一旦起来之后&#xff0c;单体架构的问题就暴露出来了&#xff0c;机器挂了所有的业务就全部无法…
阅读更多...
kafka集群搭建-使用zookeeper

kafka集群搭建-使用zookeeper

1.环境准备&#xff1a; 使用如下3台主机搭建zookeeper集群&#xff0c;由于默认的9092客户端连接端口不在本次使用的云服务器开放端口范围内&#xff0c;故端口改为了8093。 172.2.1.69:8093 172.2.1.70:8093 172.2.1.71:8093 2.下载地址 去官网下载&#xff0c;或者使用如…
阅读更多...
达梦索引组织表和堆表

达梦索引组织表和堆表

达梦数据库默认创建的是索引组织表&#xff0c;‌而Oracle数据库默认创建的是堆表。‌这两种表类型的区别主要体现在数据存储和组织方式上&#xff1a; 索引组织表(‌Index Organized Table, IOT)&#xff1a;‌ 索引组织表‌有且仅有一个聚簇索引键。索引组织表也称“普通表”…
阅读更多...
UGUI优化篇--UGUI合批

UGUI优化篇--UGUI合批

UGUI合批 UGUI合批规则概述UGUI性能查看工具合批部分的特殊例子一个白色image、蓝色image覆盖了Text&#xff0c;白色image和Text哪个先渲染 Mask合批Mask为什么会产生两个drawcallMask为什么不能合批Mask注意要点 RectMask2D为什么RecMask2D比Mask性能更好主要代码RectMask2D注…
阅读更多...
【笔记】学习记录

【笔记】学习记录

2024年7月23日 1.图的5中存储方式 2.二叉树的先序&#xff0c;中序&#xff0c;后序遍历。 学了图的存储方式之后&#xff0c;二叉树好像就是小菜一碟一样。注意一下名词的顺序就可以了。 所谓先中后序&#xff0c;就是先根&#xff0c;中根&#xff0c;后根的差别。没有其…
阅读更多...
数据库练习-3

数据库练习-3

查询要求&#xff1a; 查询代码&#xff1a;
阅读更多...
Apache2服务介绍

Apache2服务介绍

apache2 安装使用配置web访问配置虚拟主机配置代理正向代理反向代理 官网 互联网上排名第一的 HTTP 服务器&#xff0c;Apache HTTP 服务器项目致力于开发和维护适用于现代操作系统&#xff08;包括 UNIX 和 Windows&#xff09;的开源 HTTP 服务器。该项目的目标是提供安全、…
阅读更多...
【系列教程之】1、点亮一个LED灯

【系列教程之】1、点亮一个LED灯

1、点亮一个LED灯 作者将狼才鲸创建日期2024-07-23 CSDN教程目录地址&#xff1a;【目录】8051汇编与C语言系列教程本Gitee仓库原始地址&#xff1a;才鲸嵌入式/8051_c51_单片机从汇编到C_从Boot到应用实践教程 本源码包含C语言和汇编工程&#xff0c;能直接在电脑中通过Keil…
阅读更多...
SQL性能优化秘籍：如何避免计算导致索引失效

SQL性能优化秘籍：如何避免计算导致索引失效

适用于MySQL、PostgreSQL、Oracle等各种数据库的优化技巧 问题剖析 设想我们为customer表的c_acctbal列创建了一个B树索引c_acctbal_idx&#xff0c;以加速相关查询。然而&#xff0c;一个看似无害的计算可能会阻碍索引的使用。比如这样的查询&#xff1a; SELECT * FROM cus…
阅读更多...
二次元手游《交错战线》游戏拆解

二次元手游《交错战线》游戏拆解

交错战线游戏拆解案 游戏亮点即核心趣味 一、关键词&#xff1a; 回合制游戏、二次元、机甲、横板、剧情、养成、异星探索。 二、游戏亮点&#xff1a; 符合目标群体审美的原画。 三、核心趣味&#xff1a; 抽卡、肝或者氪金解锁新皮肤。 核心玩法及系统规则 核心玩法&…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023