点云学习笔记7——ICP实现点云精配准

#define BOOST_TYPEOF_EMULATION
#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>      // 点类型定义头文件
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h> //点云显示头文件
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/registration/gicp.h>
#include <pcl/registration/icp.h> // ICP配准类相关的头文件
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>

int main(int argc, char** argv)
{
	// 加载源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_raw(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\zx1.pcd", *cloud_source_raw);
	std::cout << "源点云加载完成！" << std::endl;

	// 加载目标点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_raw(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\zx2.pcd", *cloud_target_raw);
	std::cout << "目标点云加载完成！" << std::endl;

	// 创建统计滤波器对象
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> staticOR;
	staticOR.setInputCloud(cloud_source_raw);
	staticOR.setMeanK(20);   // 设置统计平均点数，增加MeanK的值会使每个点的邻域变大
	staticOR.setStddevMulThresh(1);   // 设置标准差倍数阈值，增加StddevMulThresh的值会减少被移除的点的数量

	// 进行滤波
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
	staticOR.filter(*cloud_filtered);//cloud_filtered 将用于存储经过离群点移除处理后的点云数据。
	cloud_source_raw = cloud_filtered;
	std::cout << "源点云滤波完成！" << std::endl;

	// 对源点云进行VoxelGrid降采样
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_source_raw);
	sor.setLeafSize(0.8f, 0.8f, 0.8f); // 设置体素大小
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
	sor.filter(*cloud_source_subsampled);

	// 对目标点云进行VoxelGrid降采样
	sor.setInputCloud(cloud_target_raw);
	sor.setLeafSize(0.8f, 0.8f, 0.8f); // 设置体素大小
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_subsampled(new pcl::PointCloud<pcl::PointXYZ>());//创建点云对象cloud_source_subsampled，存储经过降采样后点云数据
	sor.filter(*cloud_target_subsampled);//执行VoxelGrid滤波器的降采样操作

	std::cout << "源点云和目标点云降采样完成！" << std::endl;

	// 计算法向量
	pcl::PointCloud<pcl::Normal>::Ptr normals_source(new pcl::PointCloud<pcl::Normal>());
	pcl::PointCloud<pcl::Normal>::Ptr normals_target(new pcl::PointCloud<pcl::Normal>());
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;//创建 NormalEstimation类的实例ne，用于计算点云的法线
	ne.setInputCloud(cloud_source_subsampled);
	ne.setRadiusSearch(1);//定义义了在计算每个点的法线时考虑的邻域范围
	ne.compute(*normals_source);
	ne.setInputCloud(cloud_target_subsampled);
	ne.compute(*normals_target);

	std::cout << "源点云和目标点云法向量计算完成！" << std::endl;

	// ICP优化
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_source_subsampled);
	icp.setInputTarget(cloud_target_subsampled);
	std::cout << "加载完ICP配准的两个文件" << std::endl;
	icp.setMaximumIterations(500);//最大迭代次数为200
	icp.setTransformationEpsilon(1e-5);//设置变换矩阵的epsilon值，判断算法是否收敛
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZ>());
	icp.align(*cloud_result);

	std::cout << "ICP精调完成！" << std::endl;

	// 输出结果
	Eigen::Matrix4f icp_trans = icp.getFinalTransformation();
	std::cout << "icp变换矩阵：" << std::endl << icp_trans << std::endl;
	std::cout << "icp score:" << icp.getFitnessScore() << std::endl;//分数越接近1，表示匹配程度越高

	// 应用ICP变换到源点云上
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_transformed(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::transformPointCloud(*cloud_source_subsampled, *cloud_source_transformed, icp_trans);

	// 将变换后的源点云与目标点云相加
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sum(new pcl::PointCloud<pcl::PointXYZ>());
	*cloud_sum += *cloud_target_subsampled;
	*cloud_sum += *cloud_source_transformed;

	// 保存点云到文件
	pcl::io::savePCDFileBinary("D:\\PPCL\\zx121.pcd", *cloud_sum);
	std::cout << "Sum point cloud saved." << std::endl;

	// 可视化
	pcl::visualization::PCLVisualizer viewer("ICP - Results");
	viewer.setBackgroundColor(0, 0, 0);//三个参数，分别代表红、绿、蓝颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(cloud_target_subsampled->makeShared(), 128, 128, 0);//类的实例 target_color，它用于自定义点云的颜色
	viewer.addPointCloud(cloud_target_subsampled, target_color, "target_cloud");
	viewer.spinOnce(10);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(cloud_source_subsampled->makeShared(), 255, 0, 0);
	viewer.addPointCloud(cloud_source_subsampled, source_color, "source_cloud");//用于为源点云cloud_source_subsampled设置颜色

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> result_color(cloud_result->makeShared(), 0, 255, 0);
	viewer.addPointCloud(cloud_result, result_color, "result_cloud");//用于为配准后的点云cloud_result设置颜色

	while (!viewer.wasStopped()) 
	{
		viewer.spinOnce(100);
	}
	return 0;
}