摘要

点云配准是三维视觉、结构光三维重建、激光雷达 SLAM、机器人定位、三维测量和多视角点云拼接中的核心技术。其目标是估计两个点云之间的刚体变换，使源点云能够对齐到目标点云。实际工程中，点云配准通常分为两个阶段：粗配准和精配准。粗配准用于解决初始位姿差异较大的问题，提供一个大致的初始变换；精配准则在初始位姿较好的基础上进一步优化对齐精度。本文系统整理 PCL 中常见的粗配准方法和 ICP 及其变种，包括 FPFH + SAC-IA、SampleConsensusPrerejective、FPCS、KFPCS、NDT、Point-to-Point ICP、Point-to-Plane ICP、GICP、Nonlinear ICP、Trimmed ICP、Colored ICP、Weighted ICP、Robust ICP 等，并结合工程场景给出选型建议。

---

1. 点云配准的基本概念

点云配准的目标是将两个或多个点云统一到同一个坐标系下。

设源点云为：

P={pi} P = \{p_i\} P={pi​}

目标点云为：

Q={qi} Q = \{q_i\} Q={qi​}

配准的目标是求一个刚体变换矩阵：

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

其中：

• $R$ 是 $3\times 3$ 旋转矩阵；
• $t$ 是 $3\times 1$ 平移向量；
• $T$ 是 $4\times 4$ 齐次变换矩阵。

源点云中的点经过变换后为：

$$p_i^{\prime }=R{p}_i+t$$

点云配准的常见优化目标可以写成：

$$\underset{R,t}{\min }\sum _i\Vert R{p}_i+t-q_i{\Vert }^2$$

也就是寻找一个旋转和平移，使源点云变换后尽可能接近目标点云。

---

2. 点云配准的一般流程

在实际工程中，点云配准通常不是直接使用 ICP，而是采用如下流程：

输入源点云 source 和目标点云 target

1. 点云预处理
   - 去除 NaN 点
   - 降采样
   - 离群点滤波
   - 法向量估计

2. 粗配准
   - 使用特征、先验位姿、NDT、RANSAC 等方法估计初始位姿

3. 精配准
   - 使用 ICP、Point-to-Plane ICP、GICP 等方法进一步优化

4. 得到最终变换矩阵

5. 将源点云变换到目标点云坐标系下

6. 可视化、保存或融合点云

常见工程组合如下：

FPFH + SAC-IA       → ICP / GICP
SHOT + RANSAC      → ICP
NDT                → GICP
转台角度 / 标定初值 → Point-to-Plane ICP
手动选点           → ICP

通常可以将点云配准分成两类：

粗配准：解决初始位姿问题，让两个点云大致对齐
精配准：在初始位姿较好的基础上，进一步提高对齐精度

---

3. 粗配准方法概述

3.1 什么是粗配准？

粗配准的目标不是得到最高精度的配准结果，而是先求出一个比较合理的初始变换矩阵：

$$T_{initial}$$

使源点云大致移动到目标点云附近。

之后再接 ICP、GICP、Point-to-Plane ICP 等精配准方法。

粗配准主要解决以下问题：

1. 两片点云初始距离较远
2. 存在较大旋转角度
3. ICP 直接配准容易失败
4. 需要为 ICP 提供初始位姿

粗配准常见方法包括：

方法	主要思想	是否需要特征	是否需要初值	适用场景
FPFH + SAC-IA	局部特征匹配 + 随机采样一致性	是	否	通用粗配准
SampleConsensusPrerejective	特征匹配 + 几何预剔除	是	否	模型到场景配准
FPCS	四点一致集	否/弱依赖	否	大位姿差、部分重叠
KFPCS	FPCS 改进版本	否/弱依赖	否	大位姿差、鲁棒粗配准
NDT	体素高斯分布匹配	否	最好有	大规模 LiDAR 点云
先验位姿	标定、转台、里程计、机械臂位姿	否	有	工业系统、机器人系统
手动选点 + SVD	人工选择对应点求变换	否	人工给对应点	自动配准失败时
关键点 + 特征 + RANSAC	关键点描述子匹配	是	否	大点云、目标识别

---

4. 基于先验位姿的粗配准

4.1 方法原理

基于先验位姿的粗配准是工程中非常稳定、非常常用的方法。

所谓先验位姿，就是通过外部信息提前知道两个点云之间的大致变换关系，例如：

1. 转台旋转角度
2. 相机外参
3. 机械臂末端位姿
4. 机器人里程计
5. IMU / GPS
6. 手眼标定结果
7. AprilTag / ArUco 标志物

例如在结构光转台扫描系统中，如果每次转台旋转角度为 $\theta$，则可以构造一个绕转台轴的旋转矩阵：

$$T_{initial}=\left[\begin{array}{cc}R(\theta )& t\\ 0& 1\end{array}\right]$$

然后将该矩阵作为初始变换，再进行 ICP 精配准。

4.2 优点

1. 稳定性高
2. 速度快
3. 不依赖点云局部特征
4. 不容易产生错误匹配
5. 非常适合工业测量系统

4.3 缺点

1. 需要额外硬件或标定信息
2. 标定误差会影响初始配准结果
3. 系统搭建成本较高

4.4 适用场景

1. 结构光多视角扫描
2. 转台点云拼接
3. 机械臂引导测量
4. 多相机系统
5. RGB-D 相机固定安装场景

对于结构光三维重建，优先推荐：

标定/转台初值 → Point-to-Plane ICP / GICP

---

5. 手动选点 + SVD 粗配准

5.1 方法原理

手动选点粗配准是最直观的方法。

假设人工在源点云和目标点云中分别选择若干组对应点：

$$p_i\leftrightarrow q_i$$

目标是求解刚体变换：

$$q_i\approx R{p}_i+t$$

一般至少需要选择 3 对不共线点。

基本流程如下：

1. 在源点云中手动选择若干特征点
2. 在目标点云中选择对应点
3. 计算两组点的质心
4. 去中心化
5. 构造协方差矩阵
6. 使用 SVD 分解求解旋转矩阵 R
7. 根据质心关系求解平移向量 t
8. 得到初始变换矩阵
9. 后续接 ICP 精配准

5.2 数学推导

源点集质心：

$$\bar{p}=\frac{1}{N}\sum _{i=1}^N{p}_i$$

目标点集质心：

$$\bar{q}=\frac{1}{N}\sum _{i=1}^N{q}_i$$

去中心化：

$$p_i^{\prime }=p_i-\bar{p}$$

$$q_i^{\prime }=q_i-\bar{q}$$

构造协方差矩阵：

$$H=\sum _{i=1}^N{p}_i^{\prime }{q}_i^{\prime T}$$

对 $H$ 做 SVD 分解：

$$H=U\Sigma V^T$$

旋转矩阵为：

$$R=V{U}^T$$

平移向量为：

$$t=\bar{q}-R\bar{p}$$

5.3 优点

1. 原理简单
2. 不依赖点云特征
3. 可以作为自动配准失败时的兜底方案
4. 对小规模点云非常方便

5.4 缺点

1. 需要人工交互
2. 对选点精度敏感
3. 不适合大规模自动化系统

---

6. 基于局部特征的粗配准

基于特征的粗配准是 PCL 中非常常用的一类方法。

基本思想是：

1. 对源点云和目标点云提取局部几何特征
2. 根据特征相似性建立候选对应关系
3. 使用 RANSAC 或采样一致性方法剔除错误匹配
4. 估计初始变换矩阵

常见流程如下：

输入 source 和 target

1. 降采样
2. 法向量估计
3. 提取关键点或直接对采样点提取特征
4. 计算 FPFH / SHOT / PFH 等局部描述子
5. 根据特征匹配建立对应关系
6. 使用 RANSAC / SAC-IA 估计初始变换
7. 输出 T_initial
8. 接 ICP / GICP 精配准

---

7. PFH 特征

7.1 PFH 的基本思想

PFH，全称 Point Feature Histogram，即点特征直方图。

PFH 用于描述某个点邻域内的局部几何形状。它通过分析邻域中点与点之间的法向量夹角关系，构建一个统计直方图，用来表示局部表面的几何特征。

对于某个查询点 $p_q$，在其邻域中有若干点：

{p1,p2,...,pk} \{p_1, p_2, ..., p_k\} {p1​,p2​,...,pk​}

PFH 会计算邻域中点对之间的位置关系和法向量关系。

其核心是构建 Darboux 坐标系，并统计点对之间的角度特征。

7.2 特点

优点：
1. 描述能力强
2. 能较好表达局部几何结构
3. 对局部曲面、边缘、角点有较好区分能力

缺点：
1. 计算复杂度高
2. 对大规模点云不够高效
3. 工程中更多使用 FPFH 替代

PFH 的复杂度通常较高，因此实际 PCL 工程中更常用 FPFH。

---

8. FPFH 特征

8.1 FPFH 的基本思想

FPFH，全称 Fast Point Feature Histogram，即快速点特征直方图。

它是 PFH 的加速版本。

PFH 会计算邻域内所有点对之间的关系，计算量较大。FPFH 则先计算每个点与其邻域点之间的简化点特征，然后再加权融合邻居点的特征。

FPFH 的计算大致分为两步：

1. 计算 SPFH，即 Simplified Point Feature Histogram
2. 根据邻域关系对 SPFH 进行加权累加，得到 FPFH

FPFH 的表达形式可以理解为：

$$FPFH(p_q)=SPFH(p_q)+\frac{1}{k}\sum _{i=1}^k\frac{1}{{\omega }_i}SPFH(p_i)$$

其中：

• $p_q$ 是查询点；
• $p_i$ 是邻域点；
• $k$ 是邻域点数量；
• ${\omega }_i$ 是权重，通常与距离有关。

8.2 FPFH 的作用

FPFH 可以描述点云局部几何形状，例如：

1. 平面
2. 边缘
3. 角点
4. 曲面
5. 凹凸变化区域

在粗配准中，FPFH 通常用于寻找源点云和目标点云之间的特征匹配关系。

8.3 优点

1. 比 PFH 速度快
2. PCL 支持完善
3. 工程中使用非常广泛
4. 适合与 SAC-IA、RANSAC 配合进行粗配准

8.4 缺点

1. 对法向量质量敏感
2. 对噪声和点云分辨率敏感
3. 对重复结构容易产生误匹配
4. 对平滑、缺少特征的表面效果一般

---

9. SHOT 特征

9.1 方法原理

SHOT，全称 Signature of Histograms of Orientations。

SHOT 也是一种局部三维特征描述子。它的基本思想是：在关键点周围建立局部参考坐标系，然后将邻域空间划分成多个区域，在每个区域内统计法向量方向分布，最终形成一个高维描述子。

其核心步骤如下：

1. 对关键点建立局部参考坐标系
2. 以关键点为中心划分球形邻域
3. 将邻域分成多个空间网格
4. 在每个网格内统计法向量方向
5. 将所有统计结果拼接成描述子

9.2 优点

1. 描述能力强
2. 对局部几何结构表达较丰富
3. 适合物体识别和模型匹配
4. 可扩展到 SHOTColor，融合颜色信息

9.3 缺点

1. 计算量比 FPFH 大
2. 依赖稳定的局部参考坐标系
3. 对噪声和点云密度变化敏感

---

10. FPFH + SAC-IA 粗配准

10.1 方法概述

FPFH + SAC-IA 是 PCL 中最常用的粗配准方法之一。

PCL 对应类为：

pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>

常见写法：

pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;

10.2 算法原理

SAC-IA，全称 Sample Consensus Initial Alignment，即采样一致性初始配准。

其核心思想是：

1. 对源点云和目标点云分别计算 FPFH 特征
2. 从源点云中随机选择若干采样点
3. 根据 FPFH 特征在目标点云中寻找候选匹配点
4. 使用这些候选点对估计一个刚体变换矩阵
5. 将源点云变换到目标点云坐标系
6. 计算配准误差
7. 重复随机采样过程
8. 选择误差最小的变换作为粗配准结果

它的关键不是空间最近邻，而是特征最近邻。

也就是说，即使两个点云初始位置相差很远，只要局部几何特征相似，仍然有机会找到正确对应关系。

10.3 常用参数

参数	含义	建议
setMinSampleDistance()	随机采样点之间的最小距离	与点云尺度有关
setNumberOfSamples()	每次随机采样的点数	通常为 3
setCorrespondenceRandomness()	每个源特征点在目标中随机选择的候选匹配数量	5~20
setMaxCorrespondenceDistance()	最大对应点距离	粗配准可设置较大
setMaximumIterations()	最大迭代次数	500~5000

示例参数：

sac_ia.setMinSampleDistance(0.05);
sac_ia.setNumberOfSamples(3);
sac_ia.setCorrespondenceRandomness(5);
sac_ia.setMaxCorrespondenceDistance(0.1);
sac_ia.setMaximumIterations(1000);

10.4 优点

1. 不需要初始位姿
2. 适合大角度、大平移的初始配准
3. PCL 使用方便
4. 后续容易接 ICP / GICP

10.5 缺点

1. 依赖 FPFH 特征质量
2. 法向量估计不稳定时效果会变差
3. 重复结构容易误匹配
4. 平滑表面特征区分度低
5. 计算量较大

10.6 适用场景

1. 初始位姿未知
2. 两片点云有较大旋转和平移
3. 点云具有明显局部几何特征
4. 点云重叠率较高
5. 需要为 ICP 提供初始变换

---

11. SampleConsensusPrerejective 粗配准

11.1 方法概述

SampleConsensusPrerejective 是 PCL 中另一种基于采样一致性的粗配准方法，常用于模型点云到场景点云的位姿估计。

PCL 对应类为：

pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>

它相比普通 RANSAC 的一个重要特点是：在验证候选位姿之前，先通过几何约束进行预剔除。

11.2 算法原理

其流程如下：

1. 提取源点云和目标点云的局部特征
2. 根据特征建立候选对应关系
3. 从候选对应中随机采样若干点对
4. 根据采样点对计算候选变换矩阵
5. 在正式验证前进行几何一致性检查
6. 如果明显不合理，则提前剔除该假设
7. 对通过预剔除的候选变换进行内点评估
8. 选择内点最多或误差最小的变换

预剔除的意义在于：

减少明显错误的变换假设，提高 RANSAC 搜索效率。

11.3 优点

1. 比普通 RANSAC 更高效
2. 对杂乱背景有较好鲁棒性
3. 适合目标识别和位姿估计
4. 可用于模型点云到场景点云配准

11.4 缺点

1. 依赖特征描述子
2. 参数较多
3. 对低特征点云效果一般
4. 对完整场景到完整场景拼接不一定最优

11.5 适用场景

1. CAD 模型点云匹配到实测场景
2. 机器人抓取中的物体定位
3. 模板点云位姿估计
4. 场景中存在遮挡和杂乱背景

---

12. FPCS 粗配准

12.1 方法概述

FPCS，全称 Four-Point Congruent Sets，即四点一致集算法。

PCL 中对应类为：

pcl::registration::FPCSInitialAlignment<PointSource, PointTarget>

FPCS 不完全依赖局部特征描述子，而是利用点集之间的几何不变量进行粗配准。

12.2 算法原理

FPCS 的核心思想是：

如果两个点云之间存在重叠区域，那么在源点云中选择的一组四点，
在目标点云中也应该能找到一组几何关系一致的四点。

算法流程如下：

1. 在源点云重叠区域中选择一组共面或近似共面的四个点
2. 计算这四个点之间的几何不变量
3. 在目标点云中搜索具有相同几何关系的四点集合
4. 根据四点对应关系计算候选变换
5. 将源点云变换到目标点云坐标系
6. 计算重叠区域内点数量或配准误差
7. 选择最优变换作为粗配准结果

12.3 几何不变量

刚体变换不会改变以下几何关系：

1. 点与点之间的距离
2. 共面关系
3. 线段比例
4. 交点比例

FPCS 正是利用这些刚体不变量来寻找候选对应关系。

12.4 优点

1. 不强依赖局部描述子
2. 可以处理较大的初始位姿差异
3. 对部分重叠点云有一定鲁棒性
4. 适合粗配准

12.5 缺点

1. 计算量较大
2. 对重叠率有要求
3. 点云太大时速度可能较慢
4. 参数不好调时容易失败

12.6 适用场景

1. 初始位姿差异很大
2. 点云有一定重叠区域
3. 局部特征描述子不稳定
4. 需要不依赖法向量特征的粗配准

---

13. KFPCS 粗配准

13.1 方法概述

KFPCS 是 FPCS 的改进版本。

PCL 中对应类为：

pcl::registration::KFPCSInitialAlignment<PointSource, PointTarget>

KFPCS 在 FPCS 的基础上增加了更多候选解保存和评分机制，通常比普通 FPCS 更鲁棒。

13.2 算法原理

KFPCS 的基本框架仍然是四点一致集：

1. 从源点云中选择四点基准
2. 在目标点云中寻找几何一致的四点组合
3. 生成多个候选变换
4. 对候选变换进行评分
5. 保留多个较优候选解
6. 从候选解中选择最终粗配准结果

相比 FPCS，KFPCS 更关注：

1. 多候选解管理
2. 更鲁棒的评分方式
3. 对误匹配的抑制

13.3 优点

1. 鲁棒性通常优于普通 FPCS
2. 适合存在噪声和部分重叠的点云
3. 对初始位姿差异较大场景有一定优势

13.4 缺点

1. 参数较多
2. 计算量仍然较大
3. 工程中使用频率低于 FPFH + SAC-IA

---

14. NDT 粗配准

14.1 方法概述

NDT，全称 Normal Distributions Transform，即正态分布变换。

PCL 中对应类为：

pcl::NormalDistributionsTransform<PointSource, PointTarget>

NDT 既可以作为粗配准方法，也可以作为中等精度的精配准方法。

它常用于大规模激光雷达点云配准，例如自动驾驶、移动机器人、SLAM 等场景。

14.2 算法原理

NDT 与 ICP 最大的不同在于：

ICP 是点到点或点到面的匹配；
NDT 是点到概率分布的匹配。

NDT 的基本流程如下：

1. 将目标点云划分为多个体素网格
2. 对每个体素中的点拟合一个三维高斯分布
3. 源点云经过某个变换后，应该尽量落在目标点云的高概率区域
4. 通过优化算法不断调整变换矩阵
5. 使整体概率得分最大
6. 输出变换矩阵

对于某个体素内的点，可以计算均值：

$$\mu =\frac{1}{N}\sum _{i=1}^N{x}_i$$

协方差为：

$$\Sigma =\frac{1}{N-1}\sum _{i=1}^N(x_i-\mu )(x_i-\mu {)}^T$$

源点云中的点 $x$ 经过变换后，如果落入某个体素，其概率密度可以表示为：

$$p(x)\propto \exp \left(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\right)$$

NDT 的目标是让变换后的源点云尽可能落在目标点云的高概率区域。

14.3 常用参数

参数	含义	建议
setResolution()	体素分辨率	根据点云尺度设置
setStepSize()	优化步长	0.05~0.5
setTransformationEpsilon()	变换收敛阈值	0.001~0.01
setMaximumIterations()	最大迭代次数	30~100

示例：

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

ndt.setResolution(1.0);
ndt.setStepSize(0.1);
ndt.setTransformationEpsilon(0.01);
ndt.setMaximumIterations(50);

14.4 优点

1. 不需要逐点寻找最近邻
2. 适合大规模点云
3. 对激光雷达点云效果较好
4. 对初始位姿的要求通常比 ICP 略低

14.5 缺点

1. 体素分辨率对结果影响很大
2. 点云过稀疏时高斯分布不稳定
3. 对小型高精度零件未必适合
4. 细节配准精度通常不如 ICP / GICP

14.6 适用场景

1. 车载 LiDAR 点云配准
2. 大规模室外场景
3. 移动机器人定位
4. 点云地图匹配
5. 有大致初值的场景级点云配准

---

15. 关键点 + 特征 + RANSAC 粗配准

15.1 方法概述

对于大规模点云，直接对所有点计算特征会很慢，因此常常先提取关键点，再对关键点计算描述子。

常见关键点包括：

关键点	特点
ISS	PCL 中常用的三维关键点
Harris3D	三维角点检测
SIFT3D	三维尺度空间关键点
Uniform Sampling	均匀采样，简单稳定

15.2 算法流程

1. 对源点云和目标点云进行降采样
2. 提取关键点
3. 对关键点邻域估计法向量
4. 计算 FPFH / SHOT 等描述子
5. 根据描述子建立特征匹配
6. 使用 RANSAC 剔除错误匹配
7. 根据正确匹配估计初始变换
8. 接 ICP / GICP 精配准

15.3 优点

1. 减少计算量
2. 适合大规模点云
3. 对目标识别和模型匹配有优势
4. 可以避免所有点都参与特征计算

15.4 缺点

1. 关键点检测结果不稳定时会影响配准
2. 平滑表面关键点少
3. 参数依赖点云尺度
4. 仍然依赖描述子质量

---

16. ICP 精配准概述

16.1 什么是 ICP？

ICP，全称 Iterative Closest Point，即迭代最近点算法。

它是点云精配准中最经典的方法。

ICP 的基本思想是：

1. 给定源点云和目标点云
2. 在目标点云中为源点云寻找最近邻对应点
3. 根据对应点估计刚体变换
4. 使用该变换更新源点云
5. 重复迭代直到收敛

标准 ICP 的优化目标为：

$$E(R,t)=\sum _i\Vert R{p}_i+t-q_i{\Vert }^2$$

其中：

• $p_i$ 是源点；
• $q_i$ 是目标点中的对应点；
• $R$ 是旋转矩阵；
• $t$ 是平移向量。

16.2 ICP 的基本流程

输入：source、target、初始变换 T_initial

1. 使用初始变换对 source 进行变换
2. 对每个源点，在 target 中寻找最近点
3. 根据对应点计算刚体变换
4. 更新 source
5. 计算误差
6. 判断是否收敛
7. 如果未收敛，继续迭代
8. 输出最终变换矩阵

16.3 ICP 的优点

1. 原理简单
2. 实现方便
3. PCL 支持完善
4. 在初始位姿较好时精度较高
5. 适合局部精配准

16.4 ICP 的缺点

1. 依赖初始位姿
2. 容易陷入局部最优
3. 对离群点敏感
4. 对低重叠率点云容易失败
5. 大尺度点云直接计算较慢

---

17. Point-to-Point ICP

17.1 方法原理

Point-to-Point ICP 是最基础的 ICP。

它最小化源点变换后与目标对应点之间的欧氏距离：

$$E(R,t)=\sum _i\Vert R{p}_i+t-q_i{\Vert }^2$$

其中 $q_i$ 是目标点云中与 $p_i$ 最近的点。

PCL 对应类为：

pcl::IterativeClosestPoint<PointSource, PointTarget>

17.2 优点

1. 不需要法向量
2. 实现简单
3. 速度相对较快
4. 适合初始位姿较好的点云

17.3 缺点

1. 收敛速度可能较慢
2. 对初始位姿敏感
3. 对噪声和离群点敏感
4. 对曲面点云不如 Point-to-Plane ICP

17.4 适用场景

1. 初始位姿较好
2. 点云质量较好
3. 不方便估计法向量
4. 对精度要求不是极端苛刻

---

18. Point-to-Plane ICP

18.1 方法原理

Point-to-Plane ICP，即点到面 ICP。

它不是最小化点到点距离，而是最小化源点到目标点切平面的距离。

优化目标为：

$$E(R,t)=\sum _i((R{p}_i+t-q_i)\cdot n_i{)}^2$$

其中：

• $p_i$ 是源点；
• $q_i$ 是目标点；
• $n_i$ 是目标点处的法向量。

几何意义是：

源点变换后不一定要严格靠近目标点本身，
而是应该靠近目标点所在的局部切平面。

18.2 为什么 Point-to-Plane ICP 收敛更快？

对于连续曲面来说，点到点距离会受到采样点位置影响，而点到面距离更符合表面配准目标。

例如两个平面轻微错位时，点到面误差能更直接地反映平面之间的距离，因此通常收敛更快。

18.3 PCL 实现

PCL 中可使用：

pcl::IterativeClosestPointWithNormals<PointNormalT, PointNormalT>

或者通过设置点到面变换估计器实现。

18.4 优点

1. 收敛速度通常快于点到点 ICP
2. 适合连续曲面
3. 适合结构光扫描点云
4. 工业零件配准中非常常用

18.5 缺点

1. 需要估计法向量
2. 法向量错误会直接影响结果
3. 对边缘、尖角、噪声区域较敏感

18.6 适用场景

1. 结构光点云
2. 工业零件表面
3. 曲面点云
4. 高密度扫描点云

---

19. Nonlinear ICP

19.1 方法原理

Nonlinear ICP 是使用非线性优化方法求解位姿的 ICP 变种。

普通 ICP 中，每次迭代通常使用 SVD 或线性最小二乘估计刚体变换。而 Nonlinear ICP 会使用非线性优化方法，例如 Levenberg-Marquardt，对位姿参数进行优化。

PCL 中对应类为：

pcl::IterativeClosestPointNonLinear<PointSource, PointTarget>

19.2 核心思想

刚体变换可以用旋转和平移参数表示：

$$\xi =[{\omega }_x,{\omega }_y,{\omega }_z,t_x,t_y,t_z{]}^T$$

其中：

• ${\omega }_x,{\omega }_y,{\omega }_z$ 表示旋转参数；
• $t_x,t_y,t_z$ 表示平移参数。

Nonlinear ICP 通过不断调整 $\xi$，使误差函数最小。

19.3 优点

1. 可以处理更复杂的误差模型
2. 优化过程更灵活
3. 适合精细局部配准

19.4 缺点

1. 计算量较大
2. 参数更复杂
3. 仍然依赖较好的初始位姿

---

20. GICP：广义 ICP

20.1 方法概述

GICP，全称 Generalized Iterative Closest Point，即广义 ICP。

PCL 中对应类为：

pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>

GICP 可以理解为 Point-to-Point ICP 和 Point-to-Plane ICP 的统一形式。

20.2 算法原理

普通 ICP 假设每个点的误差是各向同性的，即每个方向上的误差权重相同。

而实际点云中的局部结构往往不同：

1. 平面区域：沿平面方向误差可以较大，法向方向误差应该较小
2. 边缘区域：不同方向上的误差约束不同
3. 曲面区域：局部几何结构更加复杂

GICP 会为每个点估计局部协方差矩阵，用协方差描述该点附近的局部几何形态。

误差模型可以写成：

$$E(T)=\sum _i{d}_i^T(C_i^B+T{C}_i^A{T}^T{)}^{-1}{d}_i$$

其中：

$$d_i=q_i-T{p}_i$$

• $C_i^A$ 是源点局部协方差；
• $C_i^B$ 是目标点局部协方差；
• $T$ 是待估计变换；
• $d_i$ 是对应点残差。

20.3 优点

1. 比普通 ICP 更稳健
2. 能利用局部表面结构
3. 对激光雷达点云效果较好
4. 对复杂曲面比普通 ICP 更可靠

20.4 缺点

1. 计算量比 ICP 大
2. 参数调节更复杂
3. 仍然需要较好的初始位姿

20.5 适用场景

1. LiDAR 点云配准
2. 移动机器人 SLAM
3. 复杂曲面点云
4. 点云局部结构明显的场景

---

21. Colored ICP / Colored GICP

21.1 方法概述

Colored ICP 或 Colored GICP 是在几何约束基础上引入颜色信息的 ICP 变种。

PCL 中有：

pcl::GeneralizedIterativeClosestPoint6D

它通常用于带有颜色信息的点云，例如：

pcl::PointXYZRGBA
pcl::PointXYZRGB

21.2 算法原理

普通 ICP 只使用 XYZ 几何坐标。

Colored ICP 会同时考虑：

1. 几何距离
2. 颜色相似性

综合误差可以理解为：

$$E=E_{geometry}+\lambda E_{color}$$

其中：

• $E_{geometry}$ 是几何误差；
• $E_{color}$ 是颜色误差；
• $\lambda$ 是颜色权重。

对于几何结构重复但颜色不同的区域，颜色信息可以帮助区分正确匹配。

21.3 优点

1. 能利用 RGB 信息辅助配准
2. 对纹理明显的 RGB-D 点云有帮助
3. 可改善几何结构重复区域的误匹配问题

21.4 缺点

1. 对光照变化敏感
2. 颜色信息不稳定时会干扰配准
3. 不适合彩色信息缺失的 LiDAR 点云
4. 对结构光单色点云未必有意义

21.5 适用场景

1. RGB-D 点云
2. 彩色三维扫描
3. 纹理丰富的物体
4. 几何重复但颜色区分明显的场景

---

22. Trimmed ICP

22.1 方法概述

Trimmed ICP 是用于处理部分重叠点云的 ICP 变种。

普通 ICP 通常默认大部分点都能找到正确对应点。但实际场景中，两片点云可能只有一部分重叠，例如：

1. 多视角扫描时存在遮挡
2. 目标点云只覆盖模型的一部分
3. 场景中存在背景杂点
4. 两帧点云重叠率较低

Trimmed ICP 的思想是：只保留误差较小的一部分对应点参与优化。

22.2 算法原理

流程如下：

1. 为源点云寻找目标点云中的对应点
2. 计算每组对应点的距离误差
3. 将对应点按照误差从小到大排序
4. 只保留前一部分误差较小的对应点
5. 使用保留下来的点对估计刚体变换
6. 重复迭代直到收敛

如果保留比例为 $\rho$，则优化目标可以写成：

$$E(R,t)=\sum _{i=1}^{\rho N}\Vert R{p}_i+t-q_i{\Vert }^2$$

其中只使用误差较小的 $\rho N$ 个点对。

22.3 优点

1. 适合部分重叠点云
2. 对遮挡和背景杂点更鲁棒
3. 可以减少错误对应点的影响

22.4 缺点

1. 需要估计重叠比例
2. 保留比例过小会导致配准不稳定
3. 保留比例过大则无法有效剔除离群点

22.5 适用场景

1. 模型点云与局部实测点云配准
2. 多视角点云部分重叠
3. 存在遮挡的扫描点云
4. 点云中有较多背景干扰点

---

23. Robust ICP

23.1 方法概述

Robust ICP 不是某一个固定算法，而是一类思想。

它的核心目标是：

降低离群点和错误对应点对配准结果的影响。

实现 Robust ICP 的常见方式有：

1. 最大对应距离限制
2. 对应点剔除器
3. RANSAC 剔除错误对应点
4. 鲁棒核函数
5. 动态权重调整

23.2 鲁棒核函数

普通 ICP 使用 L2 误差：

$$E=\sum _i\Vert e_i{\Vert }^2$$

L2 误差对大残差点非常敏感。

Robust ICP 会使用鲁棒核函数，例如：

核函数	特点
Huber	小误差二次惩罚，大误差线性惩罚
Cauchy	对大误差点降权明显
Tukey	超过阈值的大误差点几乎不参与优化

鲁棒目标可以写成：

$$E=\sum _i\rho (\Vert e_i\Vert )$$

其中 $\rho$ 是鲁棒核函数。

23.3 PCL 中常用工程实现

最简单的方式是设置最大对应距离：

icp.setMaxCorrespondenceDistance(0.05);

也可以使用对应关系剔除器，例如：

pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> rejector;

23.4 优点

1. 对噪声和离群点更鲁棒
2. 对动态物体干扰更稳定
3. 能减少错误对应关系影响

23.5 缺点

1. 参数需要根据点云尺度调整
2. 鲁棒核函数选择会影响结果
3. 剔除过强可能导致有效点不足

---

24. Weighted ICP

24.1 方法概述

Weighted ICP 即加权 ICP。

它为不同点对分配不同权重，使高可信度点对对结果贡献更大，低可信度点对贡献更小。

优化目标为：

$$E(R,t)=\sum _i{w}_i\Vert R{p}_i+t-q_i{\Vert }^2$$

其中：

• $w_i$ 是第 $i$ 个点对的权重；
• 权重越大，该点对对最终结果影响越大。

24.2 权重来源

权重可以根据以下信息确定：

1. 点的测量置信度
2. 深度误差模型
3. 法向量夹角
4. 曲率大小
5. 点到相机距离
6. 颜色相似性
7. 特征匹配分数
8. 点云边缘区域可靠性

例如 RGB-D 相机中，深度测量误差通常随距离增加而增大，因此可以给远距离点较小权重。

24.3 优点

1. 可以利用点的置信度信息
2. 对精度不均匀的点云更合理
3. 适合传感器误差模型已知的情况

24.4 缺点

1. 需要设计合理的权重
2. 权重错误会导致配准偏差
3. PCL 中通常需要结合具体变换估计器实现

24.5 适用场景

1. RGB-D 点云
2. 结构光点云中不同区域精度不同
3. 多传感器融合点云
4. 有置信度或测量误差模型的场景

---

25. Symmetric ICP

25.1 方法概述

普通 ICP 通常是单向的：

source → target

也就是为源点云中的点在目标点云中寻找对应点。

Symmetric ICP 则同时考虑两个方向：

source → target
target → source

25.2 算法原理

单向 ICP 的误差为：

$$E_{s\to t}=\sum _i\Vert T{p}_i-q_i{\Vert }^2$$

对称 ICP 同时加入反向误差：

$$E=E_{s\to t}+E_{t\to s}$$

即：

$$E=\sum _i\Vert T{p}_i-q_i{\Vert }^2+\sum _j\Vert T^{-1}{q}_j-p_j{\Vert }^2$$

这样可以减少由于单向最近邻造成的偏差。

25.3 优点

1. 减少单向匹配偏差
2. 对两片点云密度差异较大时更合理
3. 对曲面配准有一定优势

25.4 缺点

1. 计算量更大
2. 实现比普通 ICP 复杂
3. PCL 中通常需要自定义扩展

---

26. Multi-resolution ICP

26.1 方法概述

Multi-resolution ICP，即多分辨率 ICP。

它不是改变 ICP 的误差模型，而是改变配准策略。

基本思想是：

先用低分辨率点云进行大范围粗对齐，
再逐渐提高点云分辨率进行精细配准。

26.2 算法流程

原始点云
   ↓
大体素降采样
   ↓
ICP 粗对齐
   ↓
中等体素降采样
   ↓
ICP 进一步优化
   ↓
小体素降采样或原始点云
   ↓
ICP 精细优化

示例：

VoxelGrid leaf size = 0.05
        ↓
VoxelGrid leaf size = 0.02
        ↓
VoxelGrid leaf size = 0.005

26.3 优点

1. 提高收敛范围
2. 降低陷入局部最优的概率
3. 加快初始阶段配准速度
4. 适合大规模点云

26.4 缺点

1. 流程比单次 ICP 复杂
2. 每层参数需要合理设置
3. 降采样过大可能丢失关键结构

26.5 适用场景

1. 大规模点云配准
2. 多视角点云拼接
3. 初始位姿较好但点云量很大
4. 工业扫描点云精配准

---

27. Projective ICP

27.1 方法概述

Projective ICP 常用于 RGB-D 相机、深度图和有组织点云。

普通 ICP 使用 KD-Tree 在三维空间寻找最近邻，而 Projective ICP 使用相机投影关系寻找对应点。

27.2 算法原理

流程如下：

1. 将源点云根据当前位姿投影到目标深度图像平面
2. 在目标深度图的相同或邻近像素位置寻找对应点
3. 使用点到面误差优化相机位姿
4. 重复迭代直到收敛

它通常用于实时三维重建系统，例如 KinectFusion。

27.3 优点

1. 对有组织点云速度很快
2. 避免大量 KD-Tree 最近邻搜索
3. 适合实时 RGB-D 重建

27.4 缺点

1. 依赖相机内参
2. 视角变化太大时容易失败
3. 对遮挡和深度缺失敏感
4. 不适合普通无组织点云

27.5 适用场景

1. RGB-D SLAM
2. KinectFusion
3. 深度相机连续帧配准
4. 实时三维重建

---

28. Plane-to-Plane ICP

28.1 方法概述

Plane-to-Plane ICP 是点到面 ICP 的进一步扩展。

Point-to-Plane ICP 只考虑目标点处的局部平面，而 Plane-to-Plane ICP 会同时考虑源点和目标点的局部平面结构。

28.2 算法原理

每个点的邻域可以拟合出一个局部平面：

源点局部平面：由源点及其法向量描述
目标点局部平面：由目标点及其法向量描述

Plane-to-Plane ICP 不仅要求点的位置靠近，还要求两个局部平面的方向一致。

因此误差项可能同时包含：

1. 点到面的距离误差
2. 法向量方向误差
3. 局部平面协方差误差

28.3 优点

1. 更充分利用局部平面结构
2. 适合建筑、室内、道路等平面丰富场景
3. 对 LiDAR 点云有一定优势

28.4 缺点

1. 依赖稳定法向量
2. 对曲率变化复杂的物体不一定适合
3. 实现复杂度较高

---

29. Sparse ICP

29.1 方法概述

Sparse ICP 使用稀疏范数代替普通 L2 范数，以提高对离群点的鲁棒性。

普通 ICP 优化：

$$E=\sum _i\Vert e_i{\Vert }^2$$

Sparse ICP 使用类似 $L_p$ 范数：

$$E=\sum _i\Vert e_i{\Vert }^p,0<p<1$$

当 $p<1$ 时，大误差点对整体目标函数的影响会被抑制，因此对离群点更加鲁棒。

29.2 优点

1. 对离群点鲁棒
2. 适合部分重叠点云
3. 对错误对应点不太敏感

29.3 缺点

1. 优化问题更复杂
2. 可能存在非凸问题
3. PCL 中通常没有直接标准封装
4. 工程实现成本较高

---

30. Non-rigid ICP

30.1 方法概述

前面介绍的大多数 ICP 都是刚体配准，只求一个全局旋转和平移：

$$p_i^{\prime }=R{p}_i+t$$

Non-rigid ICP 即非刚性 ICP，它允许点云发生局部形变。

其变换不再是一个全局刚体矩阵，而是每个点或局部区域都有自己的变换：

$$p_i^{\prime }=T_i{p}_i$$

30.2 适用场景

1. 人脸模型配准
2. 人体模型配准
3. 医学图像中的器官表面配准
4. 布料、软组织、形变物体

30.3 优点

1. 可以处理非刚性形变
2. 适合人体、医学、动画等领域
3. 表达能力远强于刚体 ICP

30.4 缺点

1. 算法复杂
2. 需要正则化约束
3. 计算量大
4. 不适合普通工业刚体零件配准

---

31. Joint ICP

31.1 方法概述

Joint ICP 是多帧联合配准方法。

普通 ICP 通常是两两点云配准：

source → target

Joint ICP 则可以同时考虑多帧点云之间的公共变换或联合约束。

31.2 方法原理

假设有多帧点云：

$$P_1,P_2,...,P_n$$

普通方式可能是依次两两配准：

P1 → P2 → P3 → ... → Pn

这种方式容易产生累积误差。

Joint ICP 希望在一个联合优化框架下同时优化多个点云之间的位姿关系，从而减少误差传播。

31.3 优点

1. 适合多帧点云联合优化
2. 可以减少两两配准的累积误差
3. 适合多视角点云融合

31.4 缺点

1. 实现复杂度高
2. 计算量较大
3. 参数和约束设计更复杂

---

32. 常见 ICP 变种对比

ICP 变种	核心思想	是否需要法向量	鲁棒性	适用场景
Point-to-Point ICP	点到点距离最小	否	一般	初始位姿较好
Point-to-Plane ICP	点到目标切平面距离最小	是	较好	曲面、工业零件
Nonlinear ICP	非线性优化求解	可选	较好	精细局部配准
GICP	融合局部协方差信息	通常需要局部邻域	好	LiDAR、复杂曲面
Colored ICP	几何 + 颜色约束	可选	较好	RGB-D 彩色点云
Trimmed ICP	只保留误差较小的点对	否	好	部分重叠、遮挡
Robust ICP	鲁棒核 / 剔除错误对应点	可选	好	噪声、离群点
Weighted ICP	给点对加权	可选	较好	点云精度不均匀
Symmetric ICP	双向误差约束	通常需要	较好	密度差异点云
Multi-resolution ICP	从粗到细逐级配准	可选	较好	大规模点云
Projective ICP	基于投影寻找对应点	通常需要	一般	RGB-D 实时重建
Plane-to-Plane ICP	平面到平面误差	是	较好	室内、建筑、LiDAR
Sparse ICP	稀疏范数抑制离群点	可选	很好	部分重叠、离群点多
Non-rigid ICP	局部非刚性变形	可选	视模型而定	人体、医学、形变物体
Joint ICP	多帧联合优化	可选	较好	多视角联合配准

---

33. PCL 中常用配准类总结

功能	PCL 类
普通 ICP	pcl::IterativeClosestPoint
点到面 ICP	pcl::IterativeClosestPointWithNormals
非线性 ICP	pcl::IterativeClosestPointNonLinear
GICP	pcl::GeneralizedIterativeClosestPoint
彩色 GICP	pcl::GeneralizedIterativeClosestPoint6D
NDT	pcl::NormalDistributionsTransform
FPFH + SAC-IA	pcl::SampleConsensusInitialAlignment
预剔除 RANSAC	pcl::SampleConsensusPrerejective
FPCS	pcl::registration::FPCSInitialAlignment
KFPCS	pcl::registration::KFPCSInitialAlignment
Trimmed ICP	pcl::recognition::TrimmedICP
关键点 ISS	pcl::ISSKeypoint3D
FPFH 特征	pcl::FPFHEstimation / pcl::FPFHEstimationOMP
法向量估计	pcl::NormalEstimation / pcl::NormalEstimationOMP

---

34. 工程中常用配准组合

34.1 初始位姿较好

如果两个点云已经比较接近，例如来自连续帧、转台小角度扫描、机械臂粗定位，可以直接使用精配准。

推荐流程：

去除 NaN
    ↓
VoxelGrid 降采样
    ↓
离群点滤波
    ↓
法向量估计
    ↓
Point-to-Plane ICP / GICP

推荐方法：

Point-to-Plane ICP
GICP
普通 ICP

---

34.2 初始位姿未知

如果两个点云之间存在较大旋转和平移，不能直接使用 ICP。

推荐流程：

VoxelGrid 降采样
    ↓
法向量估计
    ↓
FPFH 特征提取
    ↓
SAC-IA 粗配准
    ↓
ICP / GICP 精配准

推荐组合：

FPFH + SAC-IA + ICP
FPFH + SAC-IA + GICP

---

34.3 大规模激光雷达点云

LiDAR 点云通常点数多、范围大、局部稀疏，推荐使用 NDT 或 GICP。

推荐流程：

里程计 / IMU / GPS 初值
    ↓
NDT 粗配准
    ↓
GICP 精配准

推荐组合：

NDT + GICP
NDT + ICP

---

34.4 结构光三维重建点云

结构光点云通常具有以下特点：

1. 点云密度高
2. 表面连续
3. 局部细节丰富
4. 边缘和反光区域可能有异常点
5. 多视角扫描通常有转台或标定初值

推荐流程：

去除无效点
    ↓
统计滤波 / 半径滤波
    ↓
VoxelGrid 降采样
    ↓
使用转台角度或外参作为初值
    ↓
Point-to-Plane ICP / GICP
    ↓
多视角融合

如果没有硬件初值：

VoxelGrid
    ↓
FPFH
    ↓
SAC-IA
    ↓
Point-to-Plane ICP / GICP

---

34.5 模型点云到场景点云

例如机器人抓取、目标识别、CAD 模型定位等场景。

推荐流程：

模型点云与场景点云预处理
    ↓
关键点提取
    ↓
SHOT / FPFH 特征计算
    ↓
SampleConsensusPrerejective
    ↓
ICP 精配准

推荐组合：

SHOT + SampleConsensusPrerejective + ICP
FPFH + SampleConsensusPrerejective + ICP

---

35. 常用参数设置建议

35.1 ICP 参数

参数	含义	建议
setMaximumIterations()	最大迭代次数	30~100
setMaxCorrespondenceDistance()	最大对应点距离	点云尺度的 1%~5%
setTransformationEpsilon()	变换收敛阈值	1e-8 左右
setEuclideanFitnessEpsilon()	误差收敛阈值	1e-6 左右

注意：

setMaxCorrespondenceDistance() 必须根据点云单位调整。
如果点云单位是米，0.05 表示 5 cm；
如果点云单位是毫米，0.05 只表示 0.05 mm，可能过小。

---

35.2 NDT 参数

参数	含义	建议
setResolution()	体素分辨率	根据点云尺度设置
setStepSize()	优化步长	0.05~0.5
setTransformationEpsilon()	收敛阈值	0.001~0.01
setMaximumIterations()	最大迭代次数	30~100

---

35.3 SAC-IA 参数

参数	含义	建议
setMinSampleDistance()	采样点之间的最小距离	与点云分辨率相关
setNumberOfSamples()	每次采样点数	通常为 3
setCorrespondenceRandomness()	特征候选匹配数量	5~20
setMaxCorrespondenceDistance()	最大对应距离	粗配准可适当大
setMaximumIterations()	最大迭代次数	500~5000

---

36. 工程注意事项

36.1 一定要处理 NaN 点

PCL 中很多算法不能直接处理 NaN 点，否则可能导致程序崩溃或结果异常。

常用处理方式：

std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);

---

36.2 配准前建议降采样

点云过密会导致：

1. 最近邻搜索变慢
2. 特征计算耗时增加
3. ICP 迭代速度下降
4. 局部噪声影响增大

常用 VoxelGrid：

pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setLeafSize(0.01f, 0.01f, 0.01f);
voxel.setInputCloud(cloud);
voxel.filter(*cloud_down);

---

36.3 粗配准和精配准不要混用目标

粗配准的目标是：

找到大致正确的初始位姿

精配准的目标是：

在局部范围内提高精度

不要期望粗配准达到最终精度，也不要期望 ICP 解决大范围初始位姿问题。

---

36.4 法向量半径要合理

Point-to-Plane ICP、FPFH、SHOT 等方法都依赖法向量。

法向量搜索半径过小：

容易受噪声影响

法向量搜索半径过大：

会平滑掉局部细节

一般应根据点云分辨率设置。

---

36.5 点云重叠率很重要

ICP、SAC-IA、FPCS 等方法都需要一定重叠区域。

如果两片点云重叠率过低，例如低于 20%~30%，配准会明显变得困难。

此时可以考虑：

1. 使用硬件先验初值
2. 使用 Trimmed ICP
3. 增加扫描重叠区域
4. 使用标志点或人工选点

---

36.6 注意单位尺度

PCL 不知道点云单位是米、毫米还是其他单位。

例如：

icp.setMaxCorrespondenceDistance(0.05);

如果点云单位是米，则表示 5 cm。

如果点云单位是毫米，则表示 0.05 mm。

因此所有距离参数都必须根据点云单位设置。

---

37. 总结

点云配准一般分为：

粗配准 + 精配准

粗配准负责解决初始位姿问题，常用方法包括：

1. 先验位姿
2. 手动选点 + SVD
3. FPFH + SAC-IA
4. SampleConsensusPrerejective
5. FPCS / KFPCS
6. NDT
7. 关键点 + 特征 + RANSAC

精配准负责进一步提高配准精度，常用 ICP 变种包括：

1. Point-to-Point ICP
2. Point-to-Plane ICP
3. Nonlinear ICP
4. GICP
5. Colored ICP
6. Trimmed ICP
7. Robust ICP
8. Weighted ICP
9. Symmetric ICP
10. Multi-resolution ICP
11. Projective ICP
12. Plane-to-Plane ICP
13. Sparse ICP
14. Non-rigid ICP
15. Joint ICP

实际工程中最常用的组合为：

FPFH + SAC-IA + ICP
FPFH + SAC-IA + GICP
NDT + GICP
先验位姿 + Point-to-Plane ICP
转台初值 + Point-to-Plane ICP

对于结构光三维重建点云，优先推荐：

硬件/标定初值 + Point-to-Plane ICP

如果没有可靠初值，则推荐：

FPFH + SAC-IA + Point-to-Plane ICP / GICP

点云配准没有一种方法适合所有场景。实际选型时，需要综合考虑：

1. 点云尺度
2. 点云密度
3. 噪声水平
4. 初始位姿
5. 重叠率
6. 是否有颜色
7. 是否有法向量
8. 是否有硬件先验
9. 是否要求实时性
10. 是否要求高精度

一个稳定的工程流程通常是：

预处理 → 粗配准 → 精配准 → 误差评估 → 点云融合