#include <vtkAutoInit.h>\nVTK_MODULE_INIT(vtkRenderingOpenGL);\nVTK_MODULE_INIT(vtkInteractionStyle);\n#define BOOST_TYPEOF_EMULATION\n#include //标准输入/输出\n#include <pcl/io/pcd_io.h> //pcd文件输入/输出\n#include <pcl/point_types.h> //各种点类型\n#include <pcl/registration/icp.h> //ICP(iterative closest point)配准\n#include <pcl/registration/transformation_estimation_svd.h> //SVD估计初始变换矩阵\n#include <pcl/features/normal_3d.h> // 法线估计\n#include <pcl/features/shot.h> // SHOT特征描述子\n#include <pcl/registration/correspondence_estimation.h> // 对应点估计\n\n#include <pcl/visualization/pcl_visualizer.h> //PCL可视化\n\nint main(int argc, char** argv)\n{\n //创建点云指针\n pcl::PointCloudpcl::PointXYZ::Ptr cloud_in(new pcl::PointCloudpcl::PointXYZ); //创建输入点云（指针）\n pcl::PointCloudpcl::PointXYZ::Ptr cloud_out(new pcl::PointCloudpcl::PointXYZ); //创建输出/目标点云（指针）\n\n //生成并填充点云cloud_in\n pcl::io::loadPCDFilepcl::PointXYZ("D:\点云文件\雕像1.pcd", *cloud_in);\n pcl::io::loadPCDFilepcl::PointXYZ("D:\点云文件\雕像2.pcd", *cloud_out);\n\n // 创建下采样滤波器\n pcl::VoxelGridpcl::PointXYZ voxel_grid;\n voxel_grid.setInputCloud(cloud_in);\n voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // 设置下采样的体素大小\n pcl::PointCloudpcl::PointXYZ::Ptr cloud_in_downsampled(new pcl::PointCloudpcl::PointXYZ);\n voxel_grid.filter(*cloud_in_downsampled);\n\n voxel_grid.setInputCloud(cloud_out);\n pcl::PointCloudpcl::PointXYZ::Ptr cloud_out_downsampled(new pcl::PointCloudpcl::PointXYZ);\n voxel_grid.filter(*cloud_out_downsampled);\n\n // 计算法线\n pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;\n normal_estimation.setInputCloud(cloud_in_downsampled);\n pcl::PointCloudpcl::Normal::Ptr normals_in(new pcl::PointCloudpcl::Normal);\n normal_estimation.setRadiusSearch(0.03);\n normal_estimation.compute(*normals_in);\n\n normal_estimation.setInputCloud(cloud_out_downsampled);\n pcl::PointCloudpcl::Normal::Ptr normals_out(new pcl::PointCloudpcl::Normal);\n normal_estimation.compute(*normals_out);\n\n // 计算SHOT特征描述子\n pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> shot_estimation;\n shot_estimation.setInputCloud(cloud_in_downsampled);\n shot_estimation.setInputNormals(normals_in);\n pcl::PointCloudpcl::SHOT352::Ptr descriptors_in(new pcl::PointCloudpcl::SHOT352);\n shot_estimation.setRadiusSearch(0.04);\n shot_estimation.compute(*descriptors_in);\n\n shot_estimation.setInputCloud(cloud_out_downsampled);\n shot_estimation.setInputNormals(normals_out);\n pcl::PointCloudpcl::SHOT352::Ptr descriptors_out(new pcl::PointCloudpcl::SHOT352);\n shot_estimation.compute(*descriptors_out);\n\n // 对应点估计\n pcl::registration::CorrespondenceEstimation<pcl::SHOT352, pcl::SHOT352> correspondence_estimation;\n correspondence_estimation.setInputSource(descriptors_in);\n correspondence_estimation.setInputTarget(descriptors_out);\n pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);\n correspondence_estimation.determineCorrespondences(*correspondences);\n\n // 提取对应点的坐标\n pcl::PointCloudpcl::PointXYZ::Ptr correspondences_cloud_in(new pcl::PointCloudpcl::PointXYZ);\n pcl::PointCloudpcl::PointXYZ::Ptr correspondences_cloud_out(new pcl::PointCloudpcl::PointXYZ);\n for (int i = 0; i < correspondences->size(); ++i) {\n correspondences_cloud_in->push_back(cloud_in_downsampled->at(correspondences->at(i).index_query));\n correspondences_cloud_out->push_back(cloud_out_downsampled->at(correspondences->at(i).index_match));\n }\n\n // 初始粗略配准\n pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;\n Eigen::Matrix4f initial_transform;\n svd.estimateRigidTransformation(*correspondences_cloud_in, *correspondences_cloud_out, initial_transform);\n\n // 应用初始变换矩阵进行粗略配准\n pcl::PointCloudpcl::PointXYZ::Ptr cloud_in_aligned(new pcl::PointCloudpcl::PointXYZ);\n pcl::transformPointCloud(*cloud_in_downsampled, *cloud_in_aligned, initial_transform);\n\n pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //创建ICP对象，用于ICP配准\n icp.setInputCloud(cloud_in_aligned); //设置输入点云\n icp.setInputTarget(cloud_out_downsampled); //设置目标点云（输入点云进行仿射变换，得到目标点云）\n pcl::PointCloudpcl::PointXYZ Final; //存储结果\n //进行配准，结果存储在Final中\n icp.align(Final);\n //输出ICP配准的信息（是否收敛，拟合度）\n std::cout << "has converged:" << icp.hasConverged() << " score: " <<\n icp.getFitnessScore() << std::endl;\n //输出最终的变换矩阵（4x4）\n std::cout << icp.getFinalTransformation() << std::endl;\n\n // 可视化\n pcl::visualization::PCLVisualizer viewer("ICP Registration");\n viewer.addPointCloud(cloud_in_downsampled, pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ(cloud_in_downsampled, 255, 0, 0), "cloud_in"); // 使用红色显示输入点云\n viewer.addPointCloud(cloud_out_downsampled, pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ(cloud_out_downsampled, 0, 255, 0), "cloud_out"); // 使用绿色显示目标点云\n viewer.addPointCloudpcl::PointXYZ(Final.makeShared(), pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ(Final.makeShared(), 0, 0, 255), "cloud_aligned"); // 使用蓝色显示配准后的点云\n viewer.spin();\n\n return (0);\n
