点云配准：使用质心差异进行粗配准的ICP算法

#include\x3c vtkAutoInit.h\x3e\nVTK_MODULE_INIT(vtkRenderingOpenGL);\nVTK_MODULE_INIT(vtkInteractionStyle);\n#define BOOST_TYPEOF_EMULATION\n#include\x3c iostream\x3e //标准输入/输出\n#include\x3c pcl/io/pcd_io.h\x3e //pcd文件输入/输出\n#include\x3c pcl/point_types.h\x3e //各种点类型\n#include\x3c pcl/registration/icp.h\x3e //ICP(iterative closest point)配准\n#include\x3c pcl/visualization/pcl_visualizer.h\x3e //PCL可视化\n\nint main(int argc, char** argv)\n{\n //创建点云指针\n pcl::PointCloud\x3c pcl::PointXYZ\x3e::Ptr cloud_in(new pcl::PointCloud\x3c pcl::PointXYZ\x3e); //创建输入点云（指针）\n pcl::PointCloud\x3c pcl::PointXYZ\x3e::Ptr cloud_out(new pcl::PointCloud\x3c pcl::PointXYZ\x3e); //创建输出/目标点云（指针）\n\n //生成并填充点云cloud_in\n pcl::io::loadPCDFile\x3c pcl::PointXYZ\x3e("D:\点云文件\雕像1.pcd", *cloud_in);\n pcl::io::loadPCDFile\x3c pcl::PointXYZ\x3e("D:\点云文件\雕像2.pcd", *cloud_out);\n\n // 初始粗略配准\n pcl::registration::TransformationEstimationSVD\x3c pcl::PointXYZ, pcl::PointXYZ\x3e svd;\n Eigen::Matrix4f initial_transform = Eigen::Matrix4f::Identity();\n pcl::PointCloud\x3c pcl::PointXYZ\x3e::Ptr cloud_in_aligned(new pcl::PointCloud\x3c pcl::PointXYZ\x3e);\n pcl::transformPointCloud(*cloud_in, *cloud_in_aligned, initial_transform);\n\n // 计算目标点云和配准后的输入点云的质心差异\n Eigen::Vector4f centroid_diff = cloud_out->sensor_origin_ - cloud_in_aligned->sensor_origin_;\n\n // 应用质心差异来更新初始变换矩阵\n initial_transform(0, 3) = centroid_diff(0);\n initial_transform(1, 3) = centroid_diff(1);\n initial_transform(2, 3) = centroid_diff(2);\n\n pcl::IterativeClosestPoint\x3c pcl::PointXYZ, pcl::PointXYZ\x3e icp; //创建ICP对象，用于ICP配准\n icp.setInputCloud(cloud_in_aligned); //设置输入点云\n icp.setInputTarget(cloud_out); //设置目标点云\n pcl::PointCloud\x3c pcl::PointXYZ\x3e Final; //存储结果\n //进行配准，结果存储在Final中\n icp.align(Final, initial_transform);\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, pcl::visualization::PointCloudColorHandlerCustom\x3c pcl::PointXYZ\x3e(cloud_in, 255, 0, 0), "cloud_in"); // 使用红色显示输入点云\n viewer.addPointCloud(cloud_out, pcl::visualization::PointCloudColorHandlerCustom\x3c pcl::PointXYZ\x3e(cloud_out, 0, 255, 0), "cloud_out"); // 使用绿色显示目标点云\n viewer.addPointCloud\x3c pcl::PointXYZ\x3e(Final.makeShared(), pcl::visualization::PointCloudColorHandlerCustom\x3c pcl::PointXYZ\x3e(Final.makeShared(), 0, 0, 255), "cloud_aligned"); // 使用蓝色显示配准后的点云\n viewer.spin();\n\n return (0);\n}