目录
一、概述
1.1原理
1.2实现步骤
1.3应用场景
二、代码实现
2.1关键函数
2.1.1 计算FPFH特征函数
2.1.2执行SAC-IA配准
2.1.3可视化函数
2.2完整代码
三、实现效果
PCL点云算法汇总及实战案例汇总的目录地址链接：
PCL点云算法与项目实战案例汇总（长期更新）
一、概述
点云配准是3D计算机视觉中的重要技术，广泛应用于物体识别、机器人导航等领域。SAC-IA（Sample Consensus Initial Alignment）是基于RANSAC（随机采样一致性）算法的初始配准方法，可用于粗配准。在这篇博客中，我们将探讨如何使用SAC-IA算法进行点云粗配准，并详细介绍相关步骤和代码实现。
1.1原理
SAC-IA算法是一种基于特征匹配的初始配准方法，算法通过以下步骤完成：
- 特征提取：首先对输入点云进行下采样和特征提取，通常使用FPFH（Fast Point Feature Histograms）特征进行描述。
- 特征匹配：通过FPFH特征在目标点云与源点云之间进行匹配。
- 随机采样一致性：通过RANSAC方法随机采样若干个点，计算假设变换矩阵，并基于误差和内点数量选择最优变换矩阵。
- 输出配准结果：通过最优变换矩阵将源点云配准到目标点云。
1.2实现步骤
1.3应用场景
SAC-IA算法可广泛应用于以下场景：
- 机器人导航：在多次扫描过程中对不同时间的点云数据进行配准，帮助机器人建立环境地图。
- 3D重建：将多个视角下获取的物体点云进行拼接，构建完整的3D模型。
- 目标识别：通过配准将某个已知物体的模型与场景中的物体进行对齐，完成目标识别。
二、代码实现
2.1关键函数
2.1.1 计算FPFH特征函数
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{// -------------------------法向量估计-----------------------pointnormal::Ptr normals(new pointnormal);pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;n.setInputCloud(input_cloud);n.setNumberOfThreads(8); // 使用OpenMP加速n.setSearchMethod(tree); // KDTree搜索n.setKSearch(10); // 计算法向量时使用10个近邻点n.compute(*normals); // 计算法向量// -------------------------FPFH特征计算-------------------------fpfhFeature::Ptr fpfh(new fpfhFeature);pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_estimator;fpfh_estimator.setNumberOfThreads(8); // 使用OpenMP加速fpfh_estimator.setInputCloud(input_cloud);fpfh_estimator.setInputNormals(normals); // 使用计算的法向量fpfh_estimator.setSearchMethod(tree); // KDTree搜索fpfh_estimator.setKSearch(10); // 使用10个近邻点fpfh_estimator.compute(*fpfh); // 计算FPFH特征return fpfh; // 返回FPFH特征
}
2.1.2执行SAC-IA配准
// 执行SAC-IA配准的函数
pointcloud::Ptr perform_sac_ia(pointcloud::Ptr source, fpfhFeature::Ptr source_fpfh, pointcloud::Ptr target, fpfhFeature::Ptr target_fpfh)
{// 使用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;return aligned_cloud; // 返回配准后的点云
}2.1.3可视化函数
// 可视化点云的函数
void visualize_pcd(pointcloud::Ptr pcd_src, pointcloud::Ptr pcd_tgt, pointcloud::Ptr pcd_final)
{// 创建可视化窗口pcl::visualization::PCLVisualizer viewer("点云配准可视化");int v1, v2;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_color(pcd_src, 0, 255, 0); // 源点云绿色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_color(pcd_tgt, 255, 0, 0); // 目标点云红色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_color(pcd_final, 0, 255, 0); // 配准结果绿色// 在两个视口中分别添加点云viewer.addPointCloud(pcd_src, src_color, "source cloud", v1); // 左视口添加源点云viewer.addPointCloud(pcd_tgt, tgt_color, "target cloud", v1); // 左视口添加目标点云viewer.addPointCloud(pcd_tgt, tgt_color, "target cloud v2", v2); // 右视口添加目标点云viewer.addPointCloud(pcd_final, final_color, "aligned cloud", v2); // 右视口添加配准后的点云// 启动可视化窗口，直到用户关闭窗口while (!viewer.wasStopped()) {viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}2.2完整代码
#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并保存有效索引
}// 执行SAC-IA配准的函数
pointcloud::Ptr perform_sac_ia(pointcloud::Ptr source, fpfhFeature::Ptr source_fpfh, pointcloud::Ptr target, fpfhFeature::Ptr target_fpfh)
{// 使用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;return aligned_cloud; // 返回配准后的点云
}// 可视化点云的函数
void visualize_pcd(pointcloud::Ptr pcd_src, pointcloud::Ptr pcd_tgt, pointcloud::Ptr pcd_final)
{// 创建可视化窗口pcl::visualization::PCLVisualizer viewer("点云配准可视化");int v1, v2;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_color(pcd_src, 0, 255, 0); // 源点云绿色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_color(pcd_tgt, 255, 0, 0); // 目标点云红色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_color(pcd_final, 0, 255, 0); // 配准结果绿色// 在两个视口中分别添加点云viewer.addPointCloud(pcd_src, src_color, "source cloud", v1); // 左视口添加源点云viewer.addPointCloud(pcd_tgt, tgt_color, "target cloud", v1); // 左视口添加目标点云viewer.addPointCloud(pcd_tgt, tgt_color, "target cloud v2", v2); // 右视口添加目标点云viewer.addPointCloud(pcd_final, final_color, "aligned cloud", v2); // 右视口添加配准后的点云// 启动可视化窗口，直到用户关闭窗口while (!viewer.wasStopped()) {viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}// 主函数
int main(int argc, char** argv)
{pointcloud::Ptr source_cloud(new pointcloud); // 创建源点云对象pointcloud::Ptr target_cloud(new pointcloud); // 创建目标点云对象// 加载点云数据pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source_cloud); // 加载源点云pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target_cloud); // 加载目标点云// 去除源点云和目标点云中的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配准pointcloud::Ptr aligned_cloud = perform_sac_ia(source, source_fpfh, target, target_fpfh);// 可视化源点云、目标点云和配准后的点云visualize_pcd(source_cloud, target_cloud, aligned_cloud);return 0;
}
三、实现效果
