PCL点云配准：使用SampleConsensusInitialAlignment和ICP进行点云配准

#include <vtkAutoInit.h> VTK_MODULE_INIT(vtkRenderingOpenGL); VTK_MODULE_INIT(vtkInteractionStyle); #define BOOST_TYPEOF_EMULATION #include //标准输入/输出 #include <pcl/io/pcd_io.h> //pcd文件输入/输出 #include <pcl/point_types.h> //各种点类型 #include <pcl/registration/icp.h> //ICP(iterative closest point)配准 #include <pcl/filters/voxel_grid.h> // 下采样滤波器 #include <pcl/registration/transformation_estimation_svd.h> //SVD估计初始变换矩阵 #include <pcl/registration/sample_consensus_initial_alignment.h> //SampleConsensusInitialAlignment配准

#include <pcl/visualization/pcl_visualizer.h> //PCL可视化

int main(int argc, char** argv) { //创建点云指针 pcl::PointCloudpcl::PointXYZ::Ptr cloud_in(new pcl::PointCloudpcl::PointXYZ); //创建输入点云（指针） pcl::PointCloudpcl::PointXYZ::Ptr cloud_out(new pcl::PointCloudpcl::PointXYZ); //创建输出/目标点云（指针）

//生成并填充点云cloud_in
pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\点云文件\\雕像1.pcd", *cloud_in);
pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\点云文件\\雕像2.pcd", *cloud_out);

// 创建下采样滤波器
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(cloud_in);
voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // 设置下采样的体素大小
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
voxel_grid.filter(*cloud_in_downsampled);

voxel_grid.setInputCloud(cloud_out);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
voxel_grid.filter(*cloud_out_downsampled);

// 使用SampleConsensusInitialAlignment进行粗略配准
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::PointXYZ> sac_ia;
sac_ia.setInputCloud(cloud_in_downsampled);
sac_ia.setSourceFeatures(cloud_in_downsampled);
sac_ia.setInputTarget(cloud_out_downsampled);
sac_ia.setTargetFeatures(cloud_out_downsampled);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_aligned(new pcl::PointCloud<pcl::PointXYZ>);
sac_ia.align(*cloud_in_aligned);

// 输出SampleConsensusInitialAlignment配准的信息（是否收敛，拟合度）
std::cout << "has converged:" << sac_ia.hasConverged() << " score: " <<
    sac_ia.getFitnessScore() << std::endl;
// 输出最终的变换矩阵（4x4）
std::cout << sac_ia.getFinalTransformation() << std::endl;

// 创建ICP对象，用于进一步细化配准
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in_aligned);
icp.setInputTarget(cloud_out_downsampled);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);

// 输出ICP配准的信息（是否收敛，拟合度）
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    icp.getFitnessScore() << std::endl;
// 输出最终的变换矩阵（4x4）
std::cout << icp.getFinalTransformation() << std::endl;

// 可视化
pcl::visualization::PCLVisualizer viewer("ICP Registration");
viewer.addPointCloud(cloud_in_downsampled, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_in_downsampled, 255, 0, 0), "cloud_in"); // 使用红色显示输入点云
viewer.addPointCloud(cloud_out_downsampled, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_out_downsampled, 0, 255, 0), "cloud_out"); // 使用绿色显示目标点云
viewer.addPointCloud<pcl::PointXYZ>(Final.makeShared(), pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(Final.makeShared(), 0, 0, 255), "cloud_aligned"); // 使用蓝色显示配准后的点云
viewer.spin();

return (0);

}