当前位置： 首页 > news >正文

PCL SAC-IA 算法实现点云粗配准(永久免费版)

news 2024/10/23 10:21:06

目录

  • 一、概述
    • 1.1 原理
    • 1.2 实现步骤
    • 1.3应用场景
  • 二、关键函数
    • 2.1 头文件
    • 2.2 重定义
    • 2.3 计算法向量和FPFH
    • 2.4 关键点提取
    • 2.5 去除点云中的NAN点的函数
    • 2.6 SAC-IA 算法核心代码
    • 2.7 可视化
  • 三、完整代码
  • 四、结果展示

即日起，付费专栏所有内容将以永久免费形式陆续进行发表！！！

一、概述

1.1 原理

  SAC-IA (Sample Consensus Initial Alignment) 算法是一种常用的点云粗配准方法，它通常用于无人车、机器人视觉系统等领域的点云数据对之间的快速初始配准。

1.2 实现步骤

  这个算法基于特征匹配的思想，步骤大致包括：

  1. 采样关键点：从每个点云中选取一些关键点（通常是局部兴趣点或描述符），如SIFT、SURF或SHOT等。
  2. 特征匹配：计算关键点之间的对应关系，例如通过匹配模板描述符寻找最相似的关键点对。
  3. 一致性检验：使用RANSAC (Random Sample Consensus) 技术，随机选取一组候选对，在小样本集上尝试找到最佳的配准变换（比如刚体变换），同时估计误差模型。
  4. 迭代优化：如果RANSAC成功找到了一个好的解，就使用这个解初始化非线性优化器（如Levenberg-Marquardt或ICP），进一步优化配准结果，提高精度。
  5. 重复抽样：RANSAC会多次重复上述过程，每次从数据集中选择不同的样本，取所有成功的变换的平均值作为最终配准结果。
      SAC-IA算法的优点在于其鲁棒性和效率，适合大规模点云间的初步配准，但它可能会错过一些真实的对应关系。后续通常会结合更精确的全局优化方法来进行精细配准。

1.3应用场景

  SAC-IA (Simultaneous Localization and Mapping with Inertial Alignment) 算法是一种结合了视觉、惯性测量单元（IMU）数据的粗略定位与地图构建技术。它的主要应用场景包括：

  1. 室内导航与机器人定位：在没有GPS信号的室内环境中，如商场、办公室或地下车库，SAC-IA利用相机捕捉的特征点和IMU的数据进行实时位置估计。
  2. 自动驾驶汽车：车辆在起步阶段或者城市高架桥等GPS信号较弱的地方，SAC-IA可以辅助自动驾驶系统快速确定初始位置并建立环境模型。
  3. 大型无人机航拍：无人机在起飞或切换飞行模式时，需要对当前位置进行快速粗校准，SAC-IA能够提供准确的基础定位信息。
  4. 智能手机AR应用：增强现实应用程序在移动设备上运行时，可能依赖于这种算法来稳定设备姿态，并在用户视场中放置虚拟内容。

二、关键函数

2.1 头文件

#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>   // 使用多线程加速法向量估计
#include <pcl/features/fpfh_omp.h>        // FPFH加速计算
#include <pcl/registration/ia_ransac.h>   // SAC-IA初始配准算法
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>

2.2 重定义

为代码书写方便，对复杂代码进行重定义简化处理。

typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;            // 点云类型
typedef pcl::PointCloud<pcl::Normal> pointnormal;             // 法向量类型
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;    // FPFH特征类型

2.3 计算法向量和FPFH

SAC-IA 算法需要特征描述子

// 计算FPFH特征的函数
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{// 1. 估计法向量pointnormal::Ptr normals(new pointnormal);  // 用于存储计算得到的法向量pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;  // 使用OMP加速法向量计算ne.setInputCloud(input_cloud);              // 设置输入点云ne.setNumberOfThreads(8);                   // 设置并行计算的线程数ne.setSearchMethod(tree);                   // 使用KD树加速搜索ne.setKSearch(10);                          // 设置K近邻点数量为10ne.compute(*normals);                       // 计算法向量// 2. 计算FPFH特征fpfhFeature::Ptr fpfh(new fpfhFeature);     // 创建FPFH特征存储容器pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_estimator;  // FPFH特征估计器fpfh_estimator.setNumberOfThreads(8);       // 设置并行计算线程数fpfh_estimator.setInputCloud(input_cloud);  // 设置输入点云fpfh_estimator.setInputNormals(normals);    // 设置法向量fpfh_estimator.setSearchMethod(tree);       // 使用KD树加速搜索fpfh_estimator.setKSearch(10);              // 设置K近邻点数量为10fpfh_estimator.compute(*fpfh);              // 计算FPFH特征return fpfh;
}

2.4 关键点提取

点数太多计算效率会很慢，这里使用下采样的方法获取关键点。

// 点云下采样函数
pointcloud::Ptr downsample_cloud(pointcloud::Ptr input_cloud, float leaf_size)
{// 使用体素滤波器对点云进行下采样pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);  // 设置体素的大小pointcloud::Ptr filtered_cloud(new pointcloud);           // 创建存储下采样后的点云voxel_grid.setInputCloud(input_cloud);                    // 设置输入点云voxel_grid.filter(*filtered_cloud);                       // 执行下采样return filtered_cloud;                                    // 返回下采样后的点云
}

2.5 去除点云中的NAN点的函数

一些点云中存在无效点，使用该函数进行无效点的删除。

// 去除点云中的NAN点的函数
void remove_nan_points(pointcloud::Ptr input_cloud)
{// 移除输入点云中的NAN点vector<int> indices;pcl::removeNaNFromPointCloud(*input_cloud, *input_cloud, indices);  // 去除NAN并保存有效索引
}

2.6 SAC-IA 算法核心代码

// 使用SAC-IA进行点云配准pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;sac_ia.setInputSource(source);                     // 设置源点云sac_ia.setSourceFeatures(source_fpfh);             // 设置源点云的FPFH特征sac_ia.setInputTarget(target);                     // 设置目标点云sac_ia.setTargetFeatures(target_fpfh);             // 设置目标点云的FPFH特征sac_ia.setMinSampleDistance(0.1);                  // 设置样本点之间的最小距离sac_ia.setCorrespondenceRandomness(6);             // 设置邻居数量，用于随机特征对应选择pointcloud::Ptr aligned_cloud(new pointcloud);     // 用于存储配准后的点云sac_ia.align(*aligned_cloud);                      // 执行配准

2.7 可视化

该函数可视化去噪前和去噪后的点云数据。

// 点云可视化函数
void visualizePointCloudsRegistration(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud)
{boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Registration Result"));int v1 = 0, v2 = 1;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);// 设置点云的颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);  // 源点云颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);  // 目标点云颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp_cloud, 255, 0, 0); // 配准后的点云颜色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(icp_cloud, transe, "pcs cloud", v2);  // 添加配准后的点云// 启动可视化窗口while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(10000));}
}

三、完整代码

#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>   // 使用多线程加速法向量估计
#include <pcl/features/fpfh_omp.h>        // FPFH加速计算
#include <pcl/registration/ia_ransac.h>   // SAC-IA初始配准算法
#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;    // FPFH特征类型// 计算FPFH特征的函数
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{// 1. 估计法向量pointnormal::Ptr normals(new pointnormal);  // 用于存储计算得到的法向量pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;  // 使用OMP加速法向量计算ne.setInputCloud(input_cloud);              // 设置输入点云ne.setNumberOfThreads(8);                   // 设置并行计算的线程数ne.setSearchMethod(tree);                   // 使用KD树加速搜索ne.setKSearch(10);                          // 设置K近邻点数量为10ne.compute(*normals);                       // 计算法向量// 2. 计算FPFH特征fpfhFeature::Ptr fpfh(new fpfhFeature);     // 创建FPFH特征存储容器pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_estimator;  // FPFH特征估计器fpfh_estimator.setNumberOfThreads(8);       // 设置并行计算线程数fpfh_estimator.setInputCloud(input_cloud);  // 设置输入点云fpfh_estimator.setInputNormals(normals);    // 设置法向量fpfh_estimator.setSearchMethod(tree);       // 使用KD树加速搜索fpfh_estimator.setKSearch(10);              // 设置K近邻点数量为10fpfh_estimator.compute(*fpfh);              // 计算FPFH特征return fpfh;
}// 点云下采样函数
pointcloud::Ptr downsample_cloud(pointcloud::Ptr input_cloud, float leaf_size)
{// 使用体素滤波器对点云进行下采样pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);  // 设置体素的大小pointcloud::Ptr filtered_cloud(new pointcloud);           // 创建存储下采样后的点云voxel_grid.setInputCloud(input_cloud);                    // 设置输入点云voxel_grid.filter(*filtered_cloud);                       // 执行下采样return filtered_cloud;                                    // 返回下采样后的点云
}// 去除点云中的NAN点的函数
void remove_nan_points(pointcloud::Ptr input_cloud)
{// 移除输入点云中的NAN点vector<int> indices;pcl::removeNaNFromPointCloud(*input_cloud, *input_cloud, indices);  // 去除NAN并保存有效索引
}// 点云可视化函数
void visualizePointCloudsRegistration(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud)
{boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Registration Result"));int v1 = 0, v2 = 1;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);// 设置点云的颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);  // 源点云颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);  // 目标点云颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp_cloud, 255, 0, 0); // 配准后的点云颜色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(icp_cloud, transe, "pcs 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>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source_cloud) < 0){PCL_ERROR("Couldn't read file \n");return -1;}// 加载目标点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target_cloud) < 0){PCL_ERROR("Couldn't read file \n");return -1;}// 去除源点云和目标点云中的NAN点remove_nan_points(source_cloud);remove_nan_points(target_cloud);// 对点云进行下采样pointcloud::Ptr source = downsample_cloud(source_cloud, 0.005f);  // 源点云下采样pointcloud::Ptr target = downsample_cloud(target_cloud, 0.005f);  // 目标点云下采样// 创建KD树用于加速搜索pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());// 计算源点云和目标点云的FPFH特征fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source, tree);fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target, tree);// 使用SAC-IA进行点云配准pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;sac_ia.setInputSource(source);                     // 设置源点云sac_ia.setSourceFeatures(source_fpfh);             // 设置源点云的FPFH特征sac_ia.setInputTarget(target);                     // 设置目标点云sac_ia.setTargetFeatures(target_fpfh);             // 设置目标点云的FPFH特征sac_ia.setMinSampleDistance(0.1);                  // 设置样本点之间的最小距离sac_ia.setCorrespondenceRandomness(6);             // 设置邻居数量，用于随机特征对应选择pointcloud::Ptr aligned_cloud(new pointcloud);     // 用于存储配准后的点云sac_ia.align(*aligned_cloud);                      // 执行配准// 输出配准结果cout << "配准得分: " << sac_ia.getFitnessScore() << endl;cout << "变换矩阵：\n" << sac_ia.getFinalTransformation() << endl;// 可视化源点云、目标点云和配准后的点云visualizePointCloudsRegistration(source_cloud, target_cloud, aligned_cloud);return 0;
}

四、结果展示

从源点云中读取 40256 个点
从目标点云中读取 40097 个点ICP has converged, score is 1.09437e-05
变换矩阵：0.827048   0.0104631   -0.562035   0.0358102-0.0186147    0.999788 -0.00877939 5.45643e-050.561824   0.0177231    0.827067   0.03758170           0           0           1

在这里插入图片描述


http://www.mrgr.cn/news/56666.html

相关文章：

  • 多功能纤维上线，大脑肠道 “无线畅聊” 不是梦
  • 2024 四川省大学生信息安全技术大赛 安恒杯 部分 WP
  • API接口开发系列文章：构建高效、安全、可扩展的服务
  • leetcode day1 910+16
  • C/C++中const用法
  • FineReport 控件的实际值与显示值
  • 【卡尔曼滤波】观测模型包含输入的线性卡尔曼滤波
  • 输出时间序列中的时区是什么Series.dt.tz_convert(tz)
  • 酒店智能轻触开关的类型及其应用
  • 过零检测比较器电路设计
  • 【数据结构与算法】Java中的基本数据结构：数组、链表、树、图、散列表等。
  • Java | Leetcode Java题解之第502题IPO
  • Android Audio基础——音频混合器介绍（十二）
  • 深入解析 FarmHash 算法C++ 实现与性能优化
  • 【源码+文档】基于SpringBoot+Vue城市智慧社区综合服务平台【提供源码+答辩PPT+参考文档+项目部署】
  • 什么是感知与计算融合？
  • java中double强制转换int引发的OOM问题
  • 大厂物联网(IoT)高频面试题及参考答案
  • AIGC文本生成视频
  • Python中的isinstance和hasattr
  • 【使用Flask构建RESTful API】从零开始开发简单的Web服务！
  • 追寻数组的轨迹，解开算法的情愫
  • Python语法基础：复数
  • 【在Linux世界中追寻伟大的One Piece】Socket编程UDP
  • 道可云人工智能元宇宙每日资讯｜上海市互联网业联合会人工智能专业委员会成立
  • 上海亚商投顾：沪指缩量震荡 风电、传媒股集体走强