多尺度ICP点云配准

一、概述

1、ICP配准方法

ICP（Iterative Closest Point，迭代最近点算法）是一种将两片点云进行精确对齐的经典算法。所谓点云配准，就是找到从一个点云到另一个点云的刚性变换（旋转和平移），使得两者在空间位置上尽可能重合。ICP通过反复迭代来逼近最优解：首先在源点云中取点，在目标点云中寻找最近点作为对应关系；然后根据这些对应点对计算出最优的刚性变换；接着将源点云按该变换移动，再重新找最近点，如此循环，直到满足收敛条件（如均方根误差足够小或迭代次数达到上限）。由于整个过程只依赖点坐标而不需要特征提取，因此实现简单且通用，但它对初始位姿敏感，容易陷入局部最优，通常需要先进行粗配准或提供较好的初值。ICP广泛应用于三维重建、机器人定位、医学图像融合、工业测量等领域。

2、改进特性：

本代码实现了一种 鲁棒的多尺度 ICP（迭代最近点）点云配准算法 ，用于将源点云精确对齐到目标点云，广泛应用于三维重建、SLAM、逆向工程等领域。

1） 法向量估计（PCA）：

对每个点的邻域点集进行主成分分析：

1. 计算协方差矩阵
2. 特征分解得到三个特征值
3. 最小特征值对应的特征向量即为法向量

2） Point-to-Plane 度量

传统 ICP 最小化