点云配准算法对比：ICP vs NDT vs TEASER++ vs CPD

两帧点云，一帧来自时刻 t，另一帧来自 t+1。配准的目标是找到那个刚体变换（旋转矩阵 R + 平移向量 t），让两帧点云尽可能重合。

问题说起来简单。但实际工程中，你不知道哪对点是对应关系，两帧点云密度不同，15% 的点可能是噪声或遮挡伪影，而且实时 SLAM 要求 10 ms 内出结果。

本文对比四种工程师真正在用的配准算法：ICP、NDT、TEASER++ 和 CPD。每个算法都说清楚原理、典型代码、适用场景和坑。不讲纯理论推导。

点云配准算法的分类

先把全局视角捋清楚。点云配准算法大致分四大流派：

基于点（ICP 家族）：把每个点当成独立观测，找最近邻，计算变换。ICP、GICP、Point-to-Plane ICP 都属于这一类。原始工作追溯到 1992 年 Besl & McKay。

基于分布（NDT 家族）：把空间划分为网格，每格用概率分布拟合点集，优化变换使源点云落在分布上的似然最大。Biber 等人 2003 年提出。

基于特征（SAC-IA、FPCS、Super4PCS）：提取几何描述子（FPFH、SHOT）或用四点全等集（4-point congruent set）寻找粗对齐，不依赖逐点匹配。通常作为 ICP 精配准的前端。

概率方法 / 鲁棒估计（CPD、TEASER++、GMMReg）：把配准建模成概率推断或截断损失的鲁棒估计。处理异常点，部分支持非刚性变形。

ICP、NDT、TEASER++ 和 CPD 不覆盖整个领域，但是产线代码里最常出现的四种。基于特征的方法下面会简单提到。

ICP：迭代最近点算法

Besl 和 McKay 在 1992 年提出，至今仍是 PCL、Open3D 等点云库的默认方法。名字就是算法本身：迭代→找最近点→计算变换→重复。

每次迭代做四件事：匹配（把源点云每个点找到目标点云最近的点）、剔除（丢弃距离超阈值的点对）、求解刚体变换、应用变换到源点云。循环直到收敛（平移变化小于 0.001 m 或达到预设迭代上限）。

Point-to-Point vs Point-to-Plane vs GICP

实际工程中重点关注三种变体。

Point-to-Point ICP 是原始版本，最小化点对点的欧氏距离。两帧点云接近对齐时收敛慢——梯度趋于平坦。

Point-to-Plane ICP（Chen & Medioni, 1991）把误差投影到目标点的法向量方向，收敛速度提升 5-10 倍。代价是需要表面法向量。

Generalized ICP（GICP, Segal et al. 2009）把每个点当作小平面片，计算局部协方差矩阵，兼顾收敛速度和曲面适应性。实测比 point-to-point ICP 最终配准误差低 30-50%，计算量增加 20-40%。

其他值得知道的变体：

• Symmetric ICP — 同时最小化源到目标和目标到源的误差，减少点云密度差异带来的偏差
• Point-to-Distribution ICP — 给每个局部邻域拟合高斯分布，对部分重叠场景更友好
• LM-ICP — 用 Levenberg-Marquardt 替代线性化 SVD 求解内层最小化，残差大时更稳定但单次迭代更慢

ICP 的两个致命弱点

局部最优。 初始位姿给得好（误差在几度和几厘米内），它收敛快；初始差 30 度以上，直接卡在局部最优。配准结果看起来合理，但完全错误。这也是大多数 ICP 管线要先做粗配准的原因。

异常点敏感。 一个误匹配的远距离点对就能把整个变换拉偏。距离阈值剔除有用，但这是个调参旋钮：太紧会丢有效匹配，太松会让噪声污染估计。

Open3D 调用示例

import open3d as o3d

source = o3d.io.read_point_cloud("scan_001.ply")
target = o3d.io.read_point_cloud("scan_002.pcd")

# 初始变换矩阵（如来自里程计）
init = np.eye(4)

result = o3d.pipelines.registration.registration_icp(
    source, target,
    max_correspondence_distance=0.05,  # 5cm 阈值
    init,
    o3d.pipelines.registration.TransformationEstimationPointToPlane(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
)

print(result.fitness_)  # 内点比例
print(result.inlier_rmse_)

要换成 GICP，把 TransformationEstimationPointToPlane() 替换成 TransformationEstimationForGeneralizedICP()。

实测数据：在两帧各 5 万点的 LiDAR 点云、初始位姿误差小于 2° 和 0.5 m 的条件下，标准 ICP 在现代 CPU 上 15-30 次迭代、50-120 ms 收敛。

NDT：正态分布变换

NDT 换了个思路。不逐点匹配，而是把目标点云划分为三维网格，每个格子里用正态分布拟合点集。配准就是优化一个刚体变换，让源点云落在这些分布上的概率最大。

Biber 等人在 2003 年提出，最初用于 2D 激光扫描的移动机器人定位，后来扩展到三维。

为什么 NDT 快

速度优势来自网格的固定结构。评估一个候选变换只需要判断源点落在哪些格子里，然后求 PDF 值之和。不需要最近邻搜索，不需要对应关系计算。多分辨率优化（粗网格→细网格）进一步加速。

单线程 CPU 上，NDT 通常比 ICP 快 3-10 倍。GPU 实现差距更大。

分辨率敏感性问题

NDT 最大的坑是网格尺寸参数。格子太大，每个高斯拟合一大块几何，丢细节；格子太小，点数不够，分布估计噪声大。

实际做法是多分辨率搜索：先用 1-2 m 网格粗对齐，再切到 0.5 m、0.2 m 细化。但参数在不同环境间不通用——户外 50 m 量程和室内 3 m 走廊需要的参数差别很大。

PCL 调用示例

#include <pcl/registration/ndt.h>

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setStepSize(0.1);
ndt.setResolution(1.0);  // 1m 网格
ndt.setMaximumIterations(35);

ndt.setInputSource(source_cloud);
ndt.setInputTarget(target_cloud);

pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output, initial_guess_matrix);

std::cout << "Score: " << ndt.getFitnessScore() << std::endl;
std::cout << "Transform:\n" << ndt.getFinalTransformation() << std::endl;

setResolution 是最关键的调参点。从 1.0 开始，需要更精细就降到 0.5。

NDT 在产线中的位置

大规模室外 SLAM 是 NDT 的主场。自动驾驶、农业机器人常使用 NDT 变体（NDT-OM、多层 NDT）做扫描匹配，因为高点数下的速度优势很明显。

360° 激光雷达在宽垂直视场角内点云分布均匀，NDT 网格每个格子里的点数比较一致——不会出现某些格子爆满、某些格子稀疏的情况。如果在评估 NDT pipeline 的激光雷达选型，可以参考 M360 与 MID-360 的参数对比。

室内结构化环境（薄墙、锐利拐角）是 NDT 的弱点。高斯假设在平面边界处失效，配准误差系统性偏大。

TEASER++：基于半正定松弛的可信估计

Yang 等人在 2020 年提出，思路跟前三者完全不同。它把配准建模为截断最小二乘问题，再通过半正定规划（SDP）松弛来求解。

截断最小二乘

标准最小二乘让所有点对的残差平方和最小。一个 5 m 残差的异常点就能主导整个代价函数。截断最小二乘设一个硬上限——残差超过阈值的点对贡献为零。类似 Huber 损失的硬截断版本。

截断最小二乘本身是非凸的，不好直接解。TEASER++ 松弛为 SDP 问题（凸的），这个松弛通常是”紧”的——松弛解和真实最优解几乎一致。

为什么抗异常点能力强

真实 LiDAR 数据中异常点无处不在：运动车辆、玻璃反射、雨雾回波、多径效应。一台 200 kHz 点云输出频率的 360° 激光雷达，必然包含动态物体的回波。

TEASER++ 用内点/外点二分法处理：每个点对要么被接受（贡献代价），要么被丢弃（完全忽略）。没有软权重让边缘异常点部分污染估计。算法还会输出每个点的内点标记，方便下游过滤。

性能代价

TEASER++ 在干净数据上比 ICP 和 NDT 慢——SDP 求解器有固定开销。在 1 万点云上大约 30-80 ms，而 NDT 同等数据只需 2-5 ms。

但当异常点比例达到 20-40% 时，ICP 和 NDT 基本都会失败，TEASER++ 仍然能给出合理结果。这是它的核心价值：粗配准、SLAM 回环检测、无法保证点云质量的应用。

TEASER++ Python 调用

import teaserpp_python

solver_params = teaserpp_python.RobustRegistrationSolver.Params()
solver_params.noise_bound = 0.05  # 5cm 噪声容忍度
solver_params.estimate_scaling = False
solver_params.cbar2 = 1.0
solver_params.reg_name = "TEASER"

solver = teaserpp_python.RobustRegistrationSolver(solver_params)
solver.solve(src, tgt, correspondences)  # correspondences: Nx2 索引对

solution = solver.getSolution()
print(solution.translation)
print(solution.rotation)
print(solution.inliers)  # 布尔掩码

correspondences 通常由单独的步骤产生（如 FPFH 特征匹配），因为 TEASER++ 本身不包含对应关系搜索。

CPD：相干点漂移

Myronenko 和 Song 在 2010 年提出，从统计学角度处理配准。把目标点云看作高斯混合模型（GMM）的质心集，用 EM 算法找源点云到这个 GMM 的对齐变换。

GMM 框架

每个目标点变成一个高斯分量。源点是这个混合模型生成的数据。EM 算法交替进行：E 步计算每个源点属于每个目标分量的概率（软对应关系），M 步更新变换参数使期望对数似然最大。

这自然处理了多对一的对应关系，而且提供的是软对应而非硬二值匹配。

非刚性配准

CPD 的独特之处在于支持非刚性变换。在刚体变换之上加入位移场（用高斯径向基函数建模），可以配准发生了变形的点云。

这在医学影像中很重要——对齐呼吸周期不同阶段的肺部 CT、术前术后的脑部 MRI、牙齿扫描等。人体器官会变形，纯刚体变换捕捉不到真实对应关系。

为什么 LiDAR 领域很少用 CPD

机器人和自动驾驶场景中，非刚性能力基本用不上。机器人底盘不会弯曲，建筑不会变形。LiDAR 点云的差异来自噪声，刚体变换就够处理。

CPD 在刚体配准上也比 ICP 和 NDT 慢——EM 迭代更贵，GMM 似然计算随源和目标点数双线性增长。10 万点以上的点云，不降采样很难实时运行。

所以 CPD 主要留在医学影像和形状分析领域。

横向对比总览

几个要点：

• ICP 和 NDT 在干净数据上表现接近，但 NDT 速度优势明显。
• TEASER++ 是表中唯一真正抗高比例异常点的算法。
• CPD 占据独立生态——需要非刚性配准时，列表里其他算法都做不到。

算法在实际系统中的角色

机械臂抓取

抓取场景需要亚毫米级精度，且已知目标物体。传感器给出物体部分点云，CAD 模型提供初始位姿（来自物体检测），GICP 做精细对齐，达到亚毫米精度。

移动机器人 SLAM

每帧 10-30 万点，10-20 Hz 刷新。速度优先。Autoware、Apollo 等量产 SLAM 系统的扫描匹配前端普遍使用 NDT 或其变体。

回环检测（经过长轨迹回到已访问位置）则更依赖 TEASER++，因为回环处的点云差异大——视角不同、漂移累积、物体移动。

3D 扫描与重建

多视角扫描的重叠区域需要配准。典型流程是两阶段：粗配准（基于特征的方法如 SAC-IA + FPFH 或 Super4PCS 把重叠区域拉进 ICP 收敛域），然后 ICP 多尺度精细对齐。

室内建图机器人

室内环境有走廊、门洞、玻璃反射产生的鬼点。NDT 在走廊段表现尚可（平面结构符合高斯假设），但在拐角和玻璃处失效。ICP 加上激进的异常点剔除更可靠。

手持扫描

手持 3D 扫描仪产生大量重叠帧，重叠率 60-80%，帧间运动小。ICP 在这里效果很好，因为相邻帧姿态天然接近（手持运动平滑）。多尺度 ICP（粗到细的对应距离）是标准做法。

地面站 vs 手持 vs 移动机器人 vs 静态扫描

四种典型场景的算法选择：

选型决策清单

没有单一最优解。决策取决于数据质量、算力预算和下游需求。

用 ICP 的场景：有较好的初始对齐（来自 IMU、GPS、里程计或物体检测），点云相对干净，需要快速精修。机器人领域 90% 的情况都靠它。

用 NDT 的场景：速度是第一约束，大规模室外点云，能针对环境调网格分辨率。自动驾驶 SLAM 前端是典型应用。360° 激光雷达输出点云分布均匀时，NDT 网格填充更均匀——这也是 NDT 变体主导室外 SLAM 的原因之一。

用 TEASER++ 的场景：数据噪声大、异常点多，或者没有好的初始位姿。它慢，但在其他算法都失败的时候它能行。常见模式：TEASER++ 做粗配准，再 ICP 精修。

用 CPD 的场景：处理可形变物体——医学影像、形状分析、角色动画。不是机器人算法，但如果工作跨到生物医学领域，值得知道。

组合使用：产线系统很少依赖单一算法。典型 SLAM 管线：NDT 做帧间匹配 + TEASER++ 做回环候选 + ICP 做最终子图精修。

配准完成后，许多 LiDAR 管线的下一步是标注。关于标注平台的选型，参见点云标注工具对比。关于检测场景的完整工作流，参见隧道 LiDAR 检测指南。

特征法做粗配准：SAC-IA + FPFH 或 Super4PCS 在完全没有先验位姿时是替代选项。速度比 TEASER++ 慢，但没有 SDP 求解器开销。

输入数据质量比算法选择更重要

这个容易被忽视：输入点云的质量对配准精度的影响比算法选择更大。

测距精度决定了每个点对真实表面的偏离程度。≤2 cm@10m 精度的 LiDAR 给配准算法的噪声空间比 5 cm 精度的传感器小得多。角分辨率影响边缘和拐角的捕捉质量，这些几何特征是大多数配准算法寻找对应关系的基础。

在调点云降噪管线之前，先看看传感器实际输出什么。一台 360° 激光雷达，≤0.18° 角度精度、≤2 cm@10m 测距精度、200 kHz 点云输出频率，给出的输入点云质量高，所有配准算法表现都会更好。如果你在为重配准任务评估硬件，M360 的完整规格值得一看。

如果配准结果不稳定，先检查传感器数据，再调算法参数。好的输入点云会让任何算法都表现更好。

正在为配准管线选型？查看 M360 vs MID-360 详细参数对比，或联系我们的工程团队讨论你的具体需求。