PCL 点云配准-ICP算法（精配准）

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 点云加载函数

2.1.2 ICP 配准函数

2.1.3 可视化函数

2.2完整代码

三、实现效果

---

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

---

一、概述

        ICP（Iterative Closest Point）算法是一种常用的点云精细配准算法。它通过迭代计算将一组点云变换到另一组点云的最佳位置，最终实现精确的对齐。ICP算法在每次迭代中选择与目标点云最接近的点作为对应点，通过计算最小化源点云和目标点云之间的均方误差（MSE）来进行刚性变换（包括平移和旋转）。

1.1原理

        ICP 算法的核心思想是不断迭代计算源点云和目标点云之间的最佳匹配变换。每次迭代分为以下几步：

1. 最近点对匹配：为每个源点找到其在目标点云中的最近邻点。
2. 变换矩阵计算：根据最近邻点对计算最小化误差的刚体变换（平移和旋转）。
3. 变换应用：将计算出的变换应用到源点云，更新源点云的位置。
4. 收敛判断：判断收敛条件是否满足，如果满足则停止迭代。

1.2实现步骤

1. 加载点云：从PCD文件中加载源点云和目标点云。
2. 初始化 ICP 对象：设置配准参数，如最大迭代次数、收敛条件、最大对应距离等。
3. 执行 ICP 算法：通过迭代计算源点云到目标点云的刚体变换，并应用该变换。
4. 结果可视化：通过 PCL 可视化工具显示源点云、目标点云和变换后的点云。

1.3应用场景

1. 物体识别与定位：在机器人和自动驾驶领域，使用ICP可以精确地将传感器采集的场景点云与已知模型点云进行匹配，来确定物体的姿态。
2. 3D扫描与重建：将不同视角下获取的点云数据进行拼接，形成完整的三维模型。
3. 医疗影像处理：在医学领域，用于多模态医学影像之间的配准。

二、代码实现

2.1关键函数

2.1.1 点云加载函数

该函数用于从 PCD 文件中加载点云数据。

pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {PCL_ERROR("无法读取点云文件\n");return nullptr;}std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点" << std::endl;return cloud;
}

2.1.2 ICP 配准函数

该函数实现了 ICP 算法的核心计算部分，完成点云的精细配准。

pcl::PointCloud<pcl::PointXYZ>::Ptr applyICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target) {pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(source);                    // 设置源点云icp.setInputTarget(target);                    // 设置目标点云icp.setTransformationEpsilon(1e-10);           // 为终止条件设置最小转换差异icp.setMaxCorrespondenceDistance(1);           // 设置对应点对之间的最大距离icp.setEuclideanFitnessEpsilon(0.001);         // 设置均方误差和小于阈值停止迭代icp.setMaximumIterations(35);                  // 最大迭代次数icp.setUseReciprocalCorrespondences(true);     // 使用相互对应关系pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>());icp.align(*aligned_cloud);                     // 进行配准std::cout << "ICP 收敛，得分：" << icp.getFitnessScore() << std::endl;std::cout << "变换矩阵：" << std::endl << icp.getFinalTransformation() << std::endl;return aligned_cloud;
}

2.1.3 可视化函数

该函数用于显示配准前后的点云，目标点云为红色，源点云为蓝色，配准后源点云为绿色

void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud) {boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP 配准结果"));viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色// 显示目标点云 (红色)pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);viewer->addPointCloud(target, target_color, "target cloud");// 显示源点云 (蓝色)pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 0, 255);viewer->addPointCloud(source, source_color, "source cloud");// 显示配准后的源点云 (绿色)pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned_cloud, 0, 255, 0);viewer->addPointCloud(aligned_cloud, aligned_color, "aligned cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned cloud");while (!viewer->wasStopped()) {viewer->spinOnce();}
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // 用于控制台计算时间// 加载点云函数
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& filename) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {PCL_ERROR("无法读取点云文件\n");return nullptr;}std::cout << "从文件 " << filename << " 读取点云，包含 " << cloud->size() << " 个点" << std::endl;return cloud;
}// ICP配准函数
pcl::PointCloud<pcl::PointXYZ>::Ptr applyICP(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target) {pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(source);icp.setInputTarget(target);icp.setTransformationEpsilon(1e-10);icp.setMaxCorrespondenceDistance(1);icp.setEuclideanFitnessEpsilon(0.001);icp.setMaximumIterations(35);icp.setUseReciprocalCorrespondences(true);pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>());icp.align(*aligned_cloud);std::cout << "ICP 收敛，得分：" << icp.getFitnessScore() << std::endl;std::cout << "变换矩阵：" << std::endl << icp.getFinalTransformation() << std::endl;return aligned_cloud;
}// 可视化函数
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud) {boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP 配准结果"));viewer->setBackgroundColor(0, 0, 0);/*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);viewer->addPointCloud(target, target_color, "target cloud");*/pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 0, 255);viewer->addPointCloud(source, source_color, "source cloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned_cloud, 0, 255, 0);viewer->addPointCloud(aligned_cloud, aligned_color, "aligned cloud");//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "aligned cloud");while (!viewer->wasStopped()) {viewer->spinOnce();}
}// 主函数
int main(int argc, char** argv) {// 加载点云auto source = loadPointCloud("1.pcd");auto target = loadPointCloud("2.pcd");if (!source || !target) return -1;// 执行ICP配准auto aligned_cloud = applyICP(source, target);// 可视化结果visualizePointClouds(source, target, aligned_cloud);return 0;
}

三、实现效果