PCL点云库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/features/normal_3d.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::PointCloudpcl::Normal::Ptr sourceNormals(new pcl::PointCloudpcl::Normal);

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

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud_in_downsampled);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_in_normals(new pcl::PointCloud<pcl::Normal>);
ne.setKSearch(20);
ne.compute(*cloud_in_normals);

ne.setInputCloud(cloud_out_downsampled);
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_out_normals(new pcl::PointCloud<pcl::Normal>);
ne.setKSearch(10);
ne.compute(*cloud_out_normals);

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in_downsampled);
icp.setInputTarget(cloud_out_downsampled);
icp.setMaximumIterations(100);
icp.setMaxCorrespondenceDistance(0.5);
icp.setRANSACOutlierRejectionThreshold(0.05);
icp.align(*cloud_in_downsampled);

std::cout << "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;

} 这段代码是如何进行粗配准的内容：这段代码使用了点云库（PCL）中的ICP（Iterative Closest Point）算法进行粗配准。

首先，代码加载了两个点云文件，分别命名为cloud_in和cloud_out。

然后，通过VoxelGrid滤波对两个点云进行下采样，生成了cloud_in_downsampled和cloud_out_downsampled。

接下来，使用NormalEstimation计算了两个下采样点云的法线信息，分别保存在cloud_in_normals和cloud_out_normals中。

然后，创建了ICP对象icp，并设置了一些参数，如最大迭代次数、最大对应点距离、RANSAC离群点剔除阈值。

然后，将下采样后的cloud_in_downsampled设置为icp的输入点云，将cloud_out_downsampled设置为目标点云，然后调用icp.align()函数进行配准。

最后，输出配准结果，包括是否收敛和得分，以及最终的变换矩阵。并使用PCLVisualizer可视化结果。

代码最后保存了配准后的点云结果到文件中。

总结来说，这段代码通过ICP算法实现了粗配准，即将两个点云对齐到一个相同的坐标系中。