PCL库简单的icp配准

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>int main(int argc, char** argv)
{// 确保提供了两个PCD文件作为输入if (argc != 3) {PCL_ERROR("请提供两个PCD文件作为输入。\n");return (-1);}// 读取源点云和目标点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_reg(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud_in) == -1) {PCL_ERROR("无法读取文件 %s\n", argv[1]);return (-1);}if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *cloud_out) == -1) {PCL_ERROR("无法读取文件 %s\n", argv[2]);return (-1);}// 创建ICP配准对象pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(cloud_in);icp.setInputTarget(cloud_out);// 进行配准icp.align(*cloud_reg);if (icp.hasConverged()){std::cout << "配准成功！" << std::endl;std::cout << "最终变换矩阵:" << icp.getFinalTransformation() << std::endl;std::cout << "配准后的点云:" << std::endl;for (size_t i = 0; i < cloud_reg->size(); ++i)std::cout << "    " << cloud_reg->points[i].x<< " " << cloud_reg->points[i].y<< " " << cloud_reg->points[i].z << std::endl;// 保存配准后的点云为PCD文件pcl::io::savePCDFile("aligned_cloud.pcd", *cloud_reg);std::cout << "已保存配准后的点云为 'aligned_cloud.pcd'" << std::endl;}else{std::cout << "配准失败！" << std::endl;}return 0;
}

测试了一下PCL库的icp效果，对于40度左右的旋转的配准效果还是不好。

joey@joey-Legion-Y7000P-IRX9:~/code/pclregistration/build$ ./simple_icp '/home/joey/code/pclregistration/data/1564024971.235044000.pcd' '/home/joey/code/pclregistration/data/1564025000.934826000.pcd'

在命令行调用生成的可执行文件。