admin管理员组文章数量:1649938
目录
- 一、概述
- 二、代码
- 三、结果
内容抄自CSDN点云侠：【2024最新版】PCL点云处理算法汇总（C++长期更新版）。质量无忧，永久免费，可放心复制粘贴。
一、概述
SAC-IA算法是一种基于特征匹配的初始配准方法，算法通过以下步骤完成：
特征提取：首先对输入点云进行下采样和特征提取，通常使用FPFH（Fast Point Feature Histograms）特征进行描述。
特征匹配：通过FPFH特征在目标点云与源点云之间进行匹配。
随机采样一致性：通过RANSAC方法随机采样若干个点，计算假设变换矩阵，并基于误差和内点数量选择最优变换矩阵。
输出配准结果：通过最优变换矩阵将源点云配准到目标点云。
二、代码
#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近邻点数量为10
ne.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近邻点数量为10
fpfh_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配准
// 使用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;
}
三、结果
版权声明：本文标题：PCL 点云配准-SAC-IA 初始配准算法（粗配准） 内容由热心网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：https://www.elefans.com/xitong/1729509386a1203861.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。
发表评论