pcl点云配准代码

### PCL 点云配准示例代码 以下是基于PCL库的一个完整的点云配准示例代码，展示了如何利用NDT（Normal Distributions Transform）算法完成两个点云的对齐过程。此代码片段涵盖了读取点云数据、执行配准操作以及保存最终结果的过程。 ```cpp #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/voxel_grid.h> #include <pcl/features/normal_3d.h> #include <pcl/surface/gp3.h> #include <pcl/registration/icp.h> #include <pcl/registration/ndt.h> int main(int argc, char **argv) { // 加载目标点云和输入点云 pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr input(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ>("target_cloud.pcd", *target) == -1 || pcl::io::loadPCDFile<pcl::PointXYZ>("input_cloud.pcd", *input) == -1) { std::cerr << "Error loading point clouds." << std::endl; return (-1); } // 创建NDT对象并设置参数 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; ndt.setTransformationEpsilon(0.01); // 设置收敛阈值 ndt.setStepSize(0.1); // 设置步长大小 ndt.setResolution(1.0); // 设置栅格分辨率 ndt.setMaximumIterations(35); // 设置最大迭代次数 // 将目标点云传递给NDT算法 ndt.setInputTarget(target); // 对输入点云进行下采样处理 pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter; voxel_grid_filter.setLeafSize(0.08f, 0.08f, 0.08f); voxel_grid_filter.setInputCloud(input); voxel_grid_filter.filter(*input); // 执行NDT配准 Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity(); ndt.align(*input, initial_guess); // 输出配准结果 if (ndt.hasConverged()) { std::cout << "ICP has converged, score is: " << ndt.getFitnessScore() << std::endl; std::cout << "Final transformation matrix:" << std::endl; std::cout << ndt.getFinalTransformation() << std::endl; // 转换后的输入点云存储到 output 中 pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>()); pcl::transformPointCloud(*input, *output, ndt.getFinalTransformation()); // 保存变换后的点云文件 pcl::io::savePCDFileASCII("aligned_input_cloud.pcd", *output); } else { std::cerr << "NDT failed to converge!" << std::endl; } return 0; } ``` 上述代码实现了以下功能： - 首先加载目标点云 `target` 和待配准点云 `input`[^2]。 - 利用 NDT 方法配置初始猜测矩阵，并调用 `align()` 函数完成配准。 - 如果成功，则输出欧几里得适合度得分 Fitness Score 并打印最终变换矩阵[^3]。 - 变换后的点云被保存至指定路径下的 `.pcd` 文件中。 ### 关键概念说明 - **NDT（Normal Distribution Transforms）** 是一种用于高精度点云匹配的技术，在复杂场景中有较好的表现效果。 - **Fitness Score** 表征了两组点云间的距离误差程度，数值越低表示两者贴合越好。