PCL 中 ICP 算法原理与实现详解

前言

在三维视觉和机器人领域，经常需要将不同视角扫描得到的点云拼接在一起。ICP（Iterative Closest Point，迭代最近点）算法就是解决这个问题的经典方法。

本文将深入讲解 ICP 算法的数学原理，并对照 PCL（Point Cloud Library）的源码实现，让你真正理解每一行代码背后的数学含义。

一、ICP 算法要解决什么问题？

假设有两块点云： - 源点云 —— 需要移动的点云 - 目标点云 —— 固定不动的参考点云

ICP 的目标是找到一个刚性变换（旋转 + 平移），使得源点云经过变换后，与目标点云尽可能重合。

二、ICP 算法的核心思想

ICP 算法的思路非常直观，可以用四句话概括：

找对应：对于源点云中的每个点，在目标点云中找最近的点作为对应点
算变换：根据这些对应点对，计算最优的旋转和平移
做变换：将计算出的变换应用到源点云
重复：重复上述步骤，直到收敛

这个过程的数学本质是最小化以下目标函数：

其中是点对数量，表示欧氏距离。

三、数学推导：如何求解最优变换？

3.1 问题简化

假设我们已经有了对对应点，如何求解最优的和？

首先计算两个点云的质心：

然后计算去质心坐标（将每个点减去质心）：

关键洞察：平移只与质心有关，旋转只与去质心坐标有关。因此可以分开求解。

3.2 求解旋转矩阵 R

去质心后，问题简化为最小化：

展开并化简（省略常数项），等价于最大化：

其中是协方差矩阵：

3.3 SVD 分解求 R

对进行奇异值分解（SVD）：

则最优旋转矩阵为：

注意：需要检查行列式，确保是纯旋转（det(R)=1）而非反射（det(R)=-1）。若 det(R)=-1，需修正矩阵的最后一列符号。

3.4 求解平移向量 t

得到后，最优平移为：

这个公式的直观含义：将源点云的质心旋转后，平移到目标点云的质心位置。

四、PCL 中的 ICP 实现

4.1 PCL ICP 类结构

PCL 中 ICP 算法的主要类位于 pcl/registration/icp.h，核心类继承关系如下：

1
2
3

pcl::Registration (基类)
    └── pcl::IterativeClosestPoint (ICP 主类)
            └── pcl::IterativeClosestPointNonLinear (非线性优化版本)

4.2 核心参数配置

下面是 PCL 中使用 ICP 的典型代码，我们逐行分析：

pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;

// 1. 收敛阈值：变换矩阵变化小于此值时停止
reg.setTransformationEpsilon(1e-6);

// 2. 最大对应距离：两点超过此距离不视为对应点（单位：米）
reg.setMaxCorrespondenceDistance(0.1);

// 3. 最大迭代次数
reg.setMaximumIterations(100);

// 4. 设置点表示（用于法向特征）
reg.setPointRepresentation(
    boost::make_shared<const MyPointRepresentation>(point_representation)
);

// 5. 设置源点云和目标点云
reg.setInputCloud(points_with_normals_src);
reg.setInputTarget(points_with_normals_tgt);

参数物理意义：

参数	作用	调参建议
`TransformationEpsilon`	判断收敛的阈值	越小精度越高，但可能不收敛
`MaxCorrespondenceDistance`	对应点搜索半径	设为点云平均间距的 10-20 倍
`MaximumIterations`	最大迭代次数	防止无限循环，通常 50-100

4.3 执行配准

pcl::PointCloud<PointNormalT> reg_result;
reg.align(reg_result);

// 检查是否收敛
if (reg.hasConverged()) {
    std::cout << "ICP 收敛，得分：" << reg.getFitnessScore() << std::endl;
    // 获取最终变换矩阵
    Eigen::Matrix4f final_transform = reg.getFinalTransformation();
}

getFitnessScore() 返回所有对应点对的平均误差，越小表示配准效果越好。

五、手动迭代：理解 ICP 内部流程

为了更好地理解 ICP，我们可以手动控制迭代过程：

Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity();  // 累积变换
pcl::PointCloud<PointNormalT>::Ptr src(new pcl::PointCloud<PointNormalT>());
pcl::PointCloud<PointNormalT>::Ptr tgt(new pcl::PointCloud<PointNormalT>());

*src = *points_with_normals_src;
*tgt = *points_with_normals_tgt;

for (int i = 0; i < 30; ++i) {
    pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
    reg.setTransformationEpsilon(1e-6);
    reg.setMaxCorrespondenceDistance(0.1);
    reg.setInputCloud(src);
    reg.setInputTarget(tgt);
    
    pcl::PointCloud<PointNormalT> reg_result;
    reg.align(reg_result);
    
    // 累积变换矩阵
    Ti = reg.getFinalTransformation() * Ti;
    
    // 将本次结果作为下一次的输入
    *src = reg_result;
    
    std::cout << "迭代 " << i+1 
              << ", 误差：" << reg.getFitnessScore() << std::endl;
}

为什么需要累积变换？ 因为每次迭代都是相对于当前位姿的小调整，需要将所有小变换连乘得到全局变换。

六、ICP 算法的局限性与改进

6.1 原始 ICP 的问题

需要初始位姿接近：如果初始位置相差太大，容易陷入局部最优
对噪声敏感：异常点会严重影响配准精度
计算量大：每次迭代都要搜索最近邻
无法处理部分重叠：重叠区域太少时效果差

6.2 PCL 中的改进方案

PCL 提供了多种改进策略：

// 1. 使用统计离群点剔除
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud(cloud);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*filtered_cloud);

// 2. 使用对应点拒绝（剔除坏点）
pcl::registration::CorrespondenceRejectorDistance::Ptr rej(
    new pcl::registration::CorrespondenceRejectorDistance()
);
rej->setMaximumDistance(0.1);
reg.addCorrespondenceRejector(rej);

// 3. 使用点对特征（PPF）进行全局配准
pcl::PPFRegistration<PointNormalT, PointNormalT> ppf_reg;
ppf_reg.setInputCloud(source);
ppf_reg.setInputTarget(target);
ppf_reg.align(result);

七、完整示例：两帧点云配准

#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>

int main() {
    // 1. 读取点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr src(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr tgt(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("source.pcd", *src);
    pcl::io::loadPCDFile("target.pcd", *tgt);
    
    // 2. 创建 ICP 对象
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputCloud(src);
    icp.setInputTarget(tgt);
    
    // 3. 设置参数
    icp.setMaxCorrespondenceDistance(0.1);  // 10cm
    icp.setMaximumIterations(100);
    icp.setTransformationEpsilon(1e-6);
    
    // 4. 执行配准
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);
    
    // 5. 输出结果
    if (icp.hasConverged()) {
        std::cout << "ICP 收敛，得分：" << icp.getFitnessScore() << std::endl;
        std::cout << "变换矩阵：" << std::endl 
                  << icp.getFinalTransformation() << std::endl;
        
        // 6. 保存结果
        pcl::io::savePCDFile("aligned.pcd", Final);
    } else {
        std::cout << "ICP 未收敛" << std::endl;
    }
    
    return 0;
}

八、CMake 配置

在 CMakeLists.txt 中添加：

find_package(PCL REQUIRED COMPONENTS registration io common)

add_executable(icp_demo icp.cpp)
target_link_libraries(icp_demo 
    ${PCL_LIBRARIES}
)

总结

ICP 算法的核心可以概括为：

数学本质：最小化点对间距离的平方和
求解方法：SVD 分解求最优旋转 + 质心计算求平移
迭代流程：找对应点 → 算变换 → 应用变换 → 重复
关键参数：对应距离阈值、迭代次数、收敛阈值

理解 ICP 的数学原理后，再看 PCL 的 API 设计就会清晰很多。每一行代码背后，都有明确的数学含义。

参考资源： - PCL 官方文档 - Registration - Besl & McKay (1992) - ICP 原论文 - PCL 源码