PCL 点云配准：NDT 粗配准 + ICP 精配准

#include <vtkAutoInit.h> VTK_MODULE_INIT(vtkRenderingOpenGL); VTK_MODULE_INIT(vtkInteractionStyle); #define BOOST_TYPEOF_EMULATION #include #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/registration/icp.h> #include <pcl/registration/ndt.h> #include <pcl/filters/voxel_grid.h> #include <pcl/visualization/pcl_visualizer.h>

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);

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);

Eigen::Matrix4f init_transform = Eigen::Matrix4f::Identity();
init_transform(0, 3) = 0.1;

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputCloud(cloud_in_downsampled);
ndt.setInputTarget(cloud_out_downsampled);
ndt.setTransformationEpsilon(0.01);
ndt.setStepSize(0.1);
ndt.setResolution(1.0);
ndt.setMaximumIterations(100);
ndt.setTransformationEpsilon(1e-8);
ndt.setEuclideanFitnessEpsilon(0.01);

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*cloud_aligned, init_transform);

std::cout << "NDT has converged: " << ndt.hasConverged() << " score: " << ndt.getFitnessScore() << std::endl;
std::cout << ndt.getFinalTransformation() << std::endl;

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in_downsampled);
icp.setInputTarget(cloud_out_downsampled);
icp.setMaxCorrespondenceDistance(0.5); // Increase the maximum correspondence distance
icp.setTransformationEpsilon(1e-8);
icp.setMaximumIterations(100);
icp.setRANSACOutlierRejectionThreshold(0.05);
icp.align(*cloud_in_downsampled, ndt.getFinalTransformation());

std::cout << "ICP has converged: " << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
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>(cloud_in, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_in, 255, 0, 0), "cloud_in_original");
viewer.addPointCloud<pcl::PointXYZ>(cloud_in_downsampled, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_in_downsampled, 0, 0, 255), "cloud_aligned");
viewer.spin();

pcl::io::savePCDFile("D:\\DIANYUNWENJIANJIA\\icp_pcd.pcd", *cloud_in_downsampled);

return 0;

}