点云配准这个方向绕不开的第一个算法就是ICP——Iterative Closest Point迭代最近点。我最早接触三维扫描数据处理时几十站扫描数据堆在面前每一站都有自己的局部坐标靠的就是ICP这类算法把多视角点云“咬合”到同一个坐标系下。后来做激光雷达定位、地形点云回访比对底层用的还是它。可以说只要你在做点云相关工作ICP就是那把最基本的钥匙。这篇文章我会把ICP的数学原理、代码实现、参数调优、常见坑和与NDT的对比一次讲透适合正在学点云处理的学生、刚入行SLAM/三维重建的工程师以及想在项目里落地配准方案但还没完全吃透这套机制的开发者。1. 配准问题到底是什么ICP出现的背景与定位1.1 从多帧扫描到统一坐标系配准的物理意义三维激光扫描仪、机械式LiDAR、RGB-D相机这类传感器的共同特点是一次只能采集某个视角下的点云。你想获得一个物体的完整模型或者一片区域的完整地形就得绕着扫、多帧拼接。但每一帧点云都有自己的局部坐标系拼接的本质就是求解一个刚体变换——旋转矩阵R和平移向量t把相邻帧、多帧点云统一到同一坐标系下。这里必须强调“刚体变换”这个前提。ICP从设计上就假设两个视角之间只存在旋转和平移没有尺度变化也没有物体形变所以它敢在目标函数里直接用欧氏距离作为误差度量。如果场景里存在拉着横距的尺子式偏移、又或者物体本身在扫描过程中发生了形变那ICP的假设就失效了结果往往惨不忍睹。另一个容易被新手忽略的点是配准并不只是“把两帧数据拼起来”这个孤立动作。SLAM里的帧间位姿估计、多站扫描的全局一致化、三维重建中的深度图融合、测绘里的地形点云重访比对底层全部依赖“两两配准”这个原子操作。ICP就是原子操作里最经典、最通用的一种解法理解了它后面再看NDT、GICP、特征配准都会轻松很多。1.2 ICP在配准算法家族中的位置配准算法大概分成几类基于迭代优化的ICP、NDT、GICP、基于特征匹配的FPFH、SHOT、ISS关键点提取后用RANSAC寻找对应关系、以及近年基于深度学习的配准方法。ICP属于“假设对应关系 → 估计变换 → 重新寻找对应”这样一套迭代优化框架。ICP最大的优势是不需要设计人工特征直接在点坐标层面干活。一个普通工程师用几行代码就能调用数学内核又非常清晰训练成本低在初值较好的情况下精度极高。它很“皮实”不管点云是机械零件、人体模型、室内房间还是地形只要能给出重叠区域和大致初始位姿它都能跑出不错的结果。但它不是万能的。遇到大视角差异、低重叠率、大量离群点的时候ICP很容易收敛到局部最优。很多项目里大家发现直接跑ICP“不行”其实不是算法本身有多脆弱而是没先解决粗对齐和噪点过滤的问题。这篇文章后面会重点聊这些坑这里先在心里有个谱ICP是精配准工具不是万能对齐器。1.3 ICP适用场景与不适用场景拿我自己的项目经历来说适用场景非常明确连续帧LiDAR里程计帧间位移小、初值好三维扫描多站拼接各站之间已有粗略坐标或人工初值机械臂抓取场景中的目标识别与定位模型和实测点云初始姿态接近地形点云回访比对两期数据大致对齐后需要精配准。而不适用场景也非常典型两帧点云初始姿态相差巨大比如旋转90度以上ICP大概率卡在局部最优点云稀疏到K近邻搜索无法建立稳定对应重叠率太低比如低于20%最近邻匹配里大量是无效对应动态场景中有移动物体破坏了刚体变换假设。在这些情况下正确姿势是先上特征配准或NDT做粗配准把位姿拉到ICP的收敛域内再交给ICP精配准。别拿ICP硬刚它扛不住。2. ICP的数学内核目标函数、最近邻匹配与位姿求法的完整推导2.1 目标函数迭代最小化两点集距离ICP的目标函数非常直白。假设源点云 P {p1, p2, ..., pn}目标点云 Q {q1, q2, ..., qm}我们想求一个刚体变换 T由旋转矩阵 R 和平移向量 t 组成把源点云变换到目标点云的坐标系下。如果知道 pi 对应的目标点是 qi这个问题就变成了一个标准的最小二乘问题min_{R,t} (1/n) * Σ || R*pi t - qi ||²这个形式让人联想起最小二乘拟合直线、拟合平面本质是同一个套路最小化误差平方和。但难点在于工程里我们并不知道 pi 对应哪个 qi——两个点云来自不同视角同一个物理点在不同帧里索引编号完全无关。所以ICP采用“交替优化”的思路先假设一个变换用最近邻搜索找到每个源点在目标点云中的对应点然后基于这些对应点重新估计一个更好的变换然后用新变换再更新对应点如此反复迭代。这就是名字Iterative Closest Point的含义——每次迭代都找最近点再求最优变换。2.2 最近邻匹配与对应点假设假设第 k 次迭代得到变换 T^k把它作用到源点云上得到变换后的点云 P^k T^k(P)。对每一个变换后的点 pi在目标点云 Q 中找欧氏距离最近的点 qj把它们当作一对临时对应点。所有对应点合成一个集合(pi, qi)。这里必须说透“最近点就是对应点”是一个非常强的假设。它默认两帧点云中最近的两个点大概率是同一个物理表面的两次测量。当点云密度均匀、重叠率足够、初始位姿贴近时这个假设基本成立ICP迭代能快速收敛。但当点云密度差异大比如目标点云疏密不均最近邻会系统性地偏向稠密区域导致对应关系偏移当噪声重时最近邻很容易匹配到噪声点当重叠率低时大量源点根本没有真实对应点却仍会被强行匹配到最近的目标点上造成误差污染。这也是为什么后续出现了一堆ICP变体比如Point-to-Plane点到平面、Trimmed ICP裁剪ICP丢弃距离最大的部分对应点、GICP把平面到平面信息融入协方差。它们的核心都是想修复最近邻假设在某些场景下的脆弱性。2.3 用SVD求解旋转和平移一步步推导有了对应点集合之后怎么估计最优的 R 和 t这里最经典的解法是SVD分解。整个过程不复杂但每一步都有明确的几何意义值得彻底搞懂。第一步计算两组点的质心。对变换后的源点集 P 和目标点集 Q 的对应点分别计算平均位置u_p (1/n) Σ pi u_q (1/n) Σ qi第二步去质心化。把两组点各自减去质心得到 x_i pi - u_py_i qi - u_q。这一步的作用是分离平移和旋转先让两组点“重心重合”后续只求最优旋转。第三步构造协方差矩阵 H Σ x_i * y_i^T。这里 H 是一个 3×3 矩阵它编码了两组点在去质心后的协方差关系。第四步对 H 做SVD分解H U Σ V^T。其中 U 和 V 是正交矩阵Σ 是对角矩阵。第五步旋转矩阵 R V * U^T。这个公式的来源是正交Procrustes问题目标是让旋转后的 x_i 与 y_i 尽可能地重合。第六步处理反射情况。如果 det(R) 0说明求出来的是一个反射而不是旋转需要把 V 的最后一列乘以 -1再重新计算 R。这一步很细节但忘了会得到镜像结果点云形态完全面向反了。第七步平移 t u_q - R * u_p。质心对齐之后平移量直接由质心差和旋转决定。值得记住的一个性质是无论点云是几千个点还是几百万个点最优变换都是“先对齐质心再估旋转最后求平移”。在实际调试中如果发现配准结果“平移总差一点”多半是R求错了而不是t公式的问题。2.4 迭代过程与收敛条件把上面的模块串起来ICP的完整流程是初始化变换 T0。可以是单位矩阵也可以来自IMU、轮式里程计或特征匹配的粗略估计。对源点云应用当前变换得到变换后点云。在目标点云中搜索最近邻建立对应点对。用SVD求解最优的 R 和 t更新总变换。计算平均距离误差或误差变化量判断是否收敛。如果没有收敛回到步骤2继续迭代。收敛条件的选择很关键常见有四类误差变化量小于阈值比如平均距离误差变化小于 1e-6变换增量小于阈值比如旋转增量小于0.001弧度、平移增量小于1e-4米RMS误差小于某个阈值达到最大迭代次数比如50次。工程上我建议“最大迭代次数”和“误差变化阈值”同时开启。单独用误差变化阈值很可能会因为局部最小值处误差变化极小而提前停止单独用最大迭代次数又会在不收敛时白白浪费算力。3. 跑通一个最简ICPOpen3D/PCL的最小实现3.1 环境准备与数据选择在Python里推荐用Open3D安装足够简单pip install open3d在C里推荐用PCLUbuntu下可以直接apt安装sudo apt install libpcl-dev数据集方面可以下载经典的斯坦福兔子、Bunny或Armadillo点云也可以自己用激光雷达或者RGB-D相机扫描两帧有重叠的场景。新手阶段最重要的是保证两帧数据真的有重叠区域并且初始位姿别差太远否则实验容易反复失败白白消耗信心。3.2 Open3D最小代码直接抄作业以下代码是Open3D里Point-to-Point ICP的最小实现我平时做验证也常用这份底子import open3d as o3d import numpy as np import copy def draw_registration_result(source, target, transformation): source_temp copy.deepcopy(source) target_temp copy.deepcopy(target) source_temp.transform(transformation) source_temp.paint_uniform_color([1, 0.706, 0]) target_temp.paint_uniform_color([0, 0.651, 0.929]) o3d.visualization.draw_geometries([source_temp, target_temp]) # 读取两帧点云 source o3d.io.read_point_cloud(cloud_a.pcd) target o3d.io.read_point_cloud(cloud_b.pcd) # 最大对应距离阈值单位与点云坐标一致 threshold 0.02 # 初始变换矩阵4x4齐次矩阵模拟粗对齐结果 trans_init np.array([ [0.999, -0.025, 0.010, 0.100], [0.025, 0.999, 0.005, 0.020], [-0.010, -0.005, 0.999, 0.010], [0, 0, 0, 1] ]) # 执行ICPPoint-to-Point reg_p2p o3d.pipelines.registration.registration_icp( source, target, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPoint() ) print(变换矩阵\n, reg_p2p.transformation) print(内点比例fitness, reg_p2p.fitness) print(内点RMSE, reg_p2p.inlier_rmse) # 执行ICPPoint-to-Plane需要源和目标点云都有法线 source.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius0.01, max_nn30)) target.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius0.01, max_nn30)) reg_p2plane o3d.pipelines.registration.registration_icp( source, target, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPlane() ) print(Point-to-Plane变换矩阵\n, reg_p2plane.transformation)这份代码里fitness和inlier_rmse是判断配准质量的关键指标。fitness表示符合距离阈值的对应点占比inlier_rmse表示这些内点的均方根误差。一个高质量配准通常表现为fitness较高具体阈值取决于重叠率且inlier_rmse较低与点云噪声水平同量级。只看变换矩阵很难判断好坏有了这两个指标排查问题时会方便很多。3.3 如何验证配准是否真的成功算法返回“成功”不代表配准正确。我的建议是至少三重验证第一层是可视化验证。把源点云变换后与目标点云用不同颜色叠加显示人眼扫一遍看轮廓、边缘、平面是否贴合。虽然听起来原始但这一步永远无法被数值指标完全替代。第二层是重叠度验证。可以用Open3D的compute_point_cloud_distance计算源点云每个点到目标点云最近距离统计距离小于阈值的点占比。这个比例直接反映了配准后重叠区域的重合度。第三层是法线一致性验证。在重叠区域内源点云变换后的法线与目标点云对应点的法线夹角应当接近0度。如果变换矩阵把表面方向搞反了法线夹角会明显偏大。我见过不少“算法报告收敛、肉眼看上去没大问题、可微调后就是差一点”的案例最后都是靠法线一致性找出的问题。3.4 从Python换到CPCL写法如果你在C工程里使用PCLICP的用法也很固定。这里给出一个最小可运行的参考#include pcl/registration/icp.h #include pcl/point_types.h #include pcl/point_cloud.h #include pcl/io/pcd_io.h #include Eigen/Dense pcl::PointCloudpcl::PointXYZ::Ptr source_cloud(new pcl::PointCloudpcl::PointXYZ()); pcl::PointCloudpcl::PointXYZ::Ptr target_cloud(new pcl::PointCloudpcl::PointXYZ()); pcl::io::loadPCDFile(cloud_a.pcd, *source_cloud); pcl::io::loadPCDFile(cloud_b.pcd, *target_cloud); pcl::IterativeClosestPointpcl::PointXYZ, pcl::PointXYZ icp; icp.setInputSource(source_cloud); icp.setInputTarget(target_cloud); icp.setMaxCorrespondenceDistance(0.02); icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1e-6); icp.setMaximumIterations(50); pcl::PointCloudpcl::PointXYZ final_cloud; Eigen::Matrix4f initial_guess Eigen::Matrix4f::Identity(); initial_guess(0, 3) 0.1; initial_guess(1, 3) 0.02; initial_guess(2, 3) 0.01; icp.align(final_cloud, initial_guess); Eigen::Matrix4f T icp.getFinalTransformation(); std::cout has converged: icp.hasConverged() std::endl; std::cout fitness score: icp.getFitnessScore() std::endl;这里有个经常踩的坑PCL默认的ICP是Point-to-Point且transformation_epsilon与euclidean_fitness_epsilon分别控制不同收敛条件。很多人在C里用PCL发现结果不如Python先不要怀疑算法实现而是检查三点是否估计了法线如果用Point-to-Plane、max_correspondence_distance是否和点云密度匹配、initial_guess是否合理。4. 实测中绕不开的坑初值敏感、离群点、局部最优与收敛判断4.1 初值问题ICP不是万能对齐算法ICP的迭代过程本质上是在一个高维能量面上做局部优化。如果初始位姿落进了错误的“盆地”算法就会收敛到局部最优而不是全局最优。这个特性在你做实验时会感受得非常明显给定一个偏差很小的初值ICP几轮迭代就收敛到毫米级精度初值偏差稍大它就可能卡在一个明显错位的“解”上RMSE看着也在下降但点云就是没对上。那初值误差多大算“稍大”根据我的实测经验粗略的经验范围是旋转变换超过20到30度或者平移偏差超过点云整体尺度的10%左右ICP就开始不可靠了。面对这种情况工程上通常采用三种方式之一用里程计或IMU提供初始位姿ICP只做精配准。这是SLAM系统里最普遍的做法连续帧之间位移小初值天然满足收敛条件先用FPFHRANSAC或NDT做粗配准把位姿拉到ICP的收敛域内再用ICP精配准在多假设下运行多次ICP取fitness最高、rmse最低的结果作为稳健兜底。很多资料说“ICP只需要给一个初始变换”但没强调初始变换质量对结果的影响有多大。尤其是地形点云配准两站激光雷达数据之间如果没有GPS/里程计约束初始变换基本靠猜直接跑ICP几乎必挂。4.2 离群点对目标函数的污染必须先滤再配为什么离群点对ICP的破坏力这么大原因藏在目标函数里所有最近邻距离都会参与求平均。如果场景里有一辆停着的车、一个走过的人或者传感器本身产生的飞点这些点没有真实对应点最近邻距离必然很大直接拉高了整体均方误差。更麻烦的是优化过程会为了“讨好”这些离群点扭曲整个变换矩阵最终得到一个两边都顾不上、整体错位的位姿。处理离群点的常用手段分几个层面基于统计滤波StatisticalOutlierRemoval对每个点统计邻域距离分布剔除距离均值过大的点。Open3D和PCL都自带实现。基于半径滤波删除邻域内点数少于阈值的孤立点。这类点在扫描边缘和遮挡处很常见。ICP内部拒绝对应Open3D的threshold和PCL的setMaxCorrespondenceDistance本质上就是对距离过大的对应点直接丢弃不参与变换估计。加权策略对距离大的对应点赋予更低权重限制它们的“话语权”。实操中我的习惯是先做一轮统计滤波或半径滤波再做体素降采样最后在ICP里设置一个和点云分辨率匹配的threshold。不要指望单靠ICP内部的对应距离阈值就能解决所有噪点预处理永远比后处理省事。4.3 收敛判定里的陷阱ICP的“收敛”和“正确”是两回事。算法停了不代表配准到位。常见的陷阱有三个第一个陷阱是误差下降变慢但位置差很远。这种情况下算法虽然还在迭代但更新量逐次变小误差变化低于阈值后提前退出实际上卡在了局部最优。判断办法是用fitness加持如果fitness低说明大量对应点落在阈值之外即使rmse看起来还行这个配准也不可用。第二个陷阱是fitness很低但算法报告成功。这种情况发生在重叠率不高的时候。算法会因为极少数距离很近的对应点得到低rmse但整体对齐完全错误。所以只看rmse不看fitness是一件很危险的事。第三个陷阱是threshold设置不合理。threshold设太大离群点全被当成内点污染目标函数threshold设太小正常点被误杀精度被锁死在阈值附近。比如点云分辨率是1cm你设threshold为1m那么配准误差在几十厘米级别都算“内点”算法自然没有动力继续优化。我的做法是设置多级判据最大迭代次数40到60次误差变化低于1e-6停止结束后检查fitness是否大于0.7按重叠率调整inlier_rmse是否小于点云分辨率的5到10倍。只有同时满足才算配准通过任何一项不满足都回到预处理和初值阶段去排查。4.4 常见失败模式与完整排查链路如果你跑完ICP后发现点云明显错位不用焦虑按照下面的链路逐级排查大多数情况都能定位到根因检查初值。把初始变换直接作用到源点云上可视化和源点云一起显示确认两帧初始状态下已经大致重叠。如果这一步就没对上问题出在外部初始估计不关ICP的事。检查对应距离阈值。threshold一般设置为点云分辨率的5到10倍。点云分辨率约1cmthreshold可以在0.05m到0.1m之间。检查法线。Point-to-Plane严重依赖法线质量法线方向混乱或估计半径不合适会导致迭代振荡。法线方向必须一致朝外或一致朝向传感器不能忽正忽负。检查点云密度差异。源和目标点云密度差异过大时最近邻匹配会偏向密集区域。解决方法是两帧统一做体素降采样让密度处于同一水平。降采样后重试。体素降采样不仅加快计算还能让最近邻匹配更稳定。很多“配不齐”的问题在降采样后直接消失。换变体。Point-to-Point在噪声环境下效果有限尝试Point-to-Plane或GICP往往在误差下降速度上有明显提升。5. ICP与NDT的取舍什么时候换算法怎么换5.1 NDT的基本思想NDTNormal Distributions Transform正态分布变换与ICP核心思路不同。ICP直接在原始点上找最近邻对应NDT则把目标点云划分成固定大小的栅格每个栅格内的点用均值和协方差建模成一个正态分布。配准目标变为让源点云变换后的每个点在目标点云对应栅格的概率密度函数下的总得分最大。这个思路带来了两个直接好处第一栅格化相当于对目标点云做了平滑压缩对密度不均匀不敏感第二目标函数变成了连续概率密度可以用牛顿法等梯度优化方法收敛域明显大于ICP。在实际LiDAR场景中NDT对初值的要求宽松很多在点云稀疏、运动畸变、大范围场景里都比ICP更稳。5.2 核心对比ICP、Point-to-Plane、NDT维度ICPPoint-to-PointICPPoint-to-PlaneNDT对应关系获取点-点最近邻点-目标平面距离点-栅格概率分布初值敏感性高中高相对低计算效率依赖KD-Tree中等需要法线略重栅格化后常更快配准精度高初值好时很高中等受栅格分辨率限制对点云密度差异的鲁棒性较差中等较好适合场景室内扫描、两两精配平面特征主导的场景LiDAR SLAM、地形点云主要难点局部最优、离群点法线估计质量栅格分辨率的选择在初值足够好的前提下Point-to-Plane的精度通常优于Point-to-Point原因是传感器噪声在表面法线方向大、沿表面切向小而Point-to-Point要求最近点在三维空间完全重合切向误差也会被计入Point-to-Plane允许点在切向滑动只在法线方向约束点云距离更符合传感器测量特性。这也是为什么在室内结构化环境和地形平面丰富的场景里Point-to-Plane几乎是默认选择。5.3 工程中的组合策略单纯比较ICP和NDT不如讨论如何组合使用。我推荐一套经过多个项目验证的通用策略粗配准如果旋转差异较大优先用FPFHRANSAC或NDT。NDT对点云类型不敏感、实现简单常用于大场景粗对齐。精配准粗配准之后用Point-to-Plane ICP或GICP做精配。此时初值已经在收敛域内ICP的高精度优势能发挥出来。验证用fitness、inlier_rmse和可视化交叉确认不达标则尝试多个初始假设。如果是连续帧SLAM由于帧间位移很小直接用上一帧位姿作为初值ICP就够了一般不需要NDT参与。如果在做地形点云回访比对哪怕两期数据大致重叠我也建议NDT粗配ICP精配的组合稳定性好很多。6. 工程化调优建议从数据预处理到参数设置的完整清单6.1 点云预处理降采样、法线估计、去噪点云预处理是ICP成功的一半但也是最容易被跳过的一步。我的固定流程是先做直通滤波或统计滤波去掉明显离群点。很多传感器会产生飞点分布在场景边缘和物体边界处这些点不处理的话会在ICP最近邻匹配阶段制造大量错误对应。然后是体素降采样。这个操作在3D网格里均匀采样每格保留一个代表点。它的好处是双重的计算量大幅下降同时局部网格内的平均相当于平滑降噪。对百万级点云体素降采样到几万点速度提升一个数量级精度损失很小。最后是法线估计。Point-to-Plane ICP和可视化都依赖法线。法线估计用KD-Tree找邻域PCA最小特征值对应的方向就是法线。参数里最核心的是邻域半径半径太小法线容易被噪声干扰半径太大法线过度平滑把边缘拐角也磨没了。地形点云尺度大邻域半径可以设得大一些机械零件尺度小邻域半径要收紧。6.2 KD-Tree与最近邻搜索效率ICP每一轮迭代都要做最近邻搜索这是整个算法最耗时的部分。暴力搜索复杂度O(n*m)几千个点还能忍几百万个点直接卡死。用KD-Tree可以把复杂度降到O(n log m)实际速度提升上百倍。PCL和Open3D内部都封装了KD-Tree一般不需要自己实现但有两个注意点第一点云坐标的量纲和数值范围要合理。如果某轴数值范围特别大会拉偏KD-Tree的划分结构影响搜索效率。大尺度地形点云建议先转成米制或局部ENU坐标系不要直接在经纬度或毫米级尺度上搜索。第二如果源点云特别大且每轮迭代都全量搜索建议在迭代前先降采样。比如源点云从500万点降到5万点每轮搜索成本大幅下降而配准结果不会差多少。6.3 参数设置的经验参考值不同传感器、不同尺度ICP参数差异很大。下面这组数值是我常用的经验起点可根据实际情况微调参数建议起点说明体素降采样尺寸平均点距的2~5倍太小提速不明显太大丢失细节MaxCorrespondenceDistance平均点距的5~10倍筛掉远距离错误对应最大迭代次数50防止不收敛死循环TransformationEpsilon1e-8增量判断越小要求越严EuclideanFitnessEpsilon1e-6误差变化判断法线估计邻域半径平均点距的5~10倍受噪声和表面曲率影响调参顺序比参数绝对值更重要。我通常先把初值问题解决——确保可视化下两点云已经大致重叠再做降采样然后跑ICP最后根据fitness和rmse去微调threshold和迭代次数。一上来就盯着参数调很容易忽略真正的根因。6.4 地形点云配准中的特殊注意事项最后结合“地形点云配准”这个热点方向说几句。地形点云和室内扫描有几个明显差异平面特征强、垂直结构少、点数量巨大、植被和地表反射噪声多。因为平面特征强Point-to-Plane ICP效果通常比Point-to-Point好得多法线方向一致的情况下精度提升显著。因为垂直结构少纯地形在Z轴方向上约束弱容易出现“上下滑动”。解决思路是如果有GPS或已知高程约束可以把Z轴固定如果只有点云可以选一些垂直结构明显的子区域参与配准增强Z方向约束。植被噪声多这是一个非常实际的问题。地形点云里树木、草丛会产生大量不均匀噪点直接进入ICP会严重干扰最近邻匹配。我的做法是先做分类或直通滤波把地面以上的植被层大致滤掉如果数据分类成本太高至少做一个统计滤波把离散的植被点剔除。在整地形配准流程上我的固定套路是直通滤波限高→统计滤波去噪→体素降采样到5到10cm→NDT粗配栅格1m左右→Point-to-Plane ICP精配threshold取0.2m。这套组合我在多个LiDAR地形项目中验证过比直接跑ICP稳妥得多。最后分享一个小经验不要迷信“换更高级的算法”。我见过大量ICP失败案例最终都出在初值和预处理上而不是算法本身。只要你把初始变换拉到收敛域内、把噪声点滤掉、再把threshold设置到和点云分辨率同量级经典ICP的精度完全够用。先把这个基本功练扎实再考虑上GICP、NDT还是深度学习配准你会发现自己对各种算法的理解都通透了很多。