点云配准算法对比ICP vs NDT vs TEASER vs CPD两帧点云一帧来自时刻 t另一帧来自 t1。配准的目标是找到那个刚体变换旋转矩阵 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 ICPChen Medioni, 1991把误差投影到目标点的法向量方向收敛速度提升 5-10 倍。代价是需要表面法向量。Generalized ICPGICP, 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 调用示例importopen3daso3dsourceo3d.io.read_point_cloud(scan_001.ply)targeto3d.io.read_point_cloud(scan_002.pcd)# 初始变换矩阵如来自里程计initnp.eye(4)resulto3d.pipelines.registration.registration_icp(source, target,max_correspondence_distance0.05,# 5cm 阈值init,o3d.pipelines.registration.TransformationEstimationPointToPlane(),o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration50))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 调用示例#includepcl/registration/ndt.hpcl::NormalDistributionsTransformpcl::PointXYZ,pcl::PointXYZndt;ndt.setStepSize(0.1);ndt.setResolution(1.0);// 1m 网格ndt.setMaximumIterations(35);ndt.setInputSource(source_cloud);ndt.setInputTarget(target_cloud);pcl::PointCloudpcl::PointXYZ::Ptr output(newpcl::PointCloudpcl::PointXYZ);ndt.align(*output,initial_guess_matrix);std::coutScore: ndt.getFitnessScore()std::endl;std::coutTransform:\nndt.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 调用importteaserpp_pythonsolver_paramsteaserpp_python.RobustRegistrationSolver.Params()solver_params.noise_bound0.05# 5cm 噪声容忍度solver_params.estimate_scalingFalsesolver_params.cbar21.0solver_params.reg_nameTEASERsolverteaserpp_python.RobustRegistrationSolver(solver_params)solver.solve(src, tgt, correspondences)# correspondences: Nx2 索引对solutionsolver.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 主要留在医学影像和形状分析领域。横向对比总览维度ICPNDTTEASERCPD精度干净数据高中高高高精度20% 异常点低低高中速度1万点5-15 ms2-5 ms30-80 ms50-150 ms速度10万点100-500 ms30-100 ms200-800 ms1 s异常点容忍度差需预过滤差需预过滤优内置良软分配初始对齐依赖强局部最优中多分辨率缓解弱近似全局中内存开销O(n) kd-treeO(网格³)O(n) SDP矩阵O(n×m) 对应矩阵非刚性支持无无无有最佳场景精细对齐、实时大规模室外SLAM粗配准、回环检测医学影像、形状分析几个要点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 (multi-scale)高重叠率 小帧间运动地面站扫描FPFH ICP 或 TEASER ICP帧间变换大需要粗配准移动机器人 SLAMNDT (frame-to-frame) TEASER (loop closure)速度 鲁棒性静态扫描多视角Super4PCS 或 TEASER ICP无初始位姿需要全局对齐选型决策清单没有单一最优解。决策取决于数据质量、算力预算和下游需求。用 ICP 的场景有较好的初始对齐来自 IMU、GPS、里程计或物体检测点云相对干净需要快速精修。机器人领域 90% 的情况都靠它。用 NDT 的场景速度是第一约束大规模室外点云能针对环境调网格分辨率。自动驾驶 SLAM 前端是典型应用。360° 激光雷达输出点云分布均匀时NDT 网格填充更均匀——这也是 NDT 变体主导室外 SLAM 的原因之一。用 TEASER 的场景数据噪声大、异常点多或者没有好的初始位姿。它慢但在其他算法都失败的时候它能行。常见模式TEASER 做粗配准再 ICP 精修。用 CPD 的场景处理可形变物体——医学影像、形状分析、角色动画。不是机器人算法但如果工作跨到生物医学领域值得知道。组合使用产线系统很少依赖单一算法。典型 SLAM 管线NDT 做帧间匹配 TEASER 做回环候选 ICP 做最终子图精修。特征法做粗配准SAC-IA FPFH 或 Super4PCS 在完全没有先验位姿时是替代选项。速度比 TEASER 慢但没有 SDP 求解器开销。输入数据质量比算法选择更重要这个容易被忽视输入点云的质量对配准精度的影响比算法选择更大。测距精度决定了每个点对真实表面的偏离程度。≤2 cm10m 精度的 LiDAR 给配准算法的噪声空间比 5 cm 精度的传感器小得多。角分辨率影响边缘和拐角的捕捉质量这些几何特征是大多数配准算法寻找对应关系的基础。在调点云降噪管线之前先看看传感器实际输出什么。一台 360° 激光雷达≤0.18° 角度精度、≤2 cm10m 测距精度、200 kHz 点云输出频率给出的输入点云质量高所有配准算法表现都会更好。如果你在为重配准任务评估硬件M360 的完整规格值得一看。如果配准结果不稳定先检查传感器数据再调算法参数。好的输入点云会让任何算法都表现更好。正在为配准管线选型查看 M360 vs MID-360 详细参数对比或联系我们的工程团队讨论你的具体需求。本文首发于 SmartBotParts了解更多工业级 3D LiDAR 方案https://smartbotparts.com/zh/articles/point-cloud-registration-algorithms-compared-zh.html?utm_sourcecsdnutm_mediumarticleutm_campaignpoint-cloud-registration-algorithms-compared