上一篇聊了回环检测回环检测到之后需要做什么需要把两帧点云精确对齐算出它们之间的精确相对位姿。这个对齐过程就是点云配准而ICPIterative Closest Point迭代最近点是点云配准领域最经典、应用最广泛的算法。从1992年Besl和McKay提出以来ICP一直是3D数据处理的基础工具。ICP的目标很明确给定两组点云source和target找到一个刚体变换旋转平移使得source变换后和target尽可能重合。听起来是个简单的优化问题但实际操作中坑很多——初始位姿不好会收敛到局部最优、对应点匹配不准确会拉偏结果、点云有噪声和离群点会影响精度。ICP的核心迭代过程ICP的名字就说明了它的核心思想迭代地找最近点然后对齐。每一轮迭代做两件事找对应关系、求最优变换。第一步是找对应点。对source中的每个点在target中找距离最近的点作为对应点。这一步是最耗时的如果暴力搜索复杂度是O(N*M)N和M分别是两组点云的点数。第二步是用对应点求最优变换。有了对应关系后问题变成了找一个刚体变换T使得所有对应点对的距离之和最小。这是一个最小二乘问题可以用SVD分解求解。# ICP核心迭代 def icp(source, target, init_T, max_iter50): T init_T for i in range(max_iter): # 找最近点对应 correspondences find_nearest(T source, target) # SVD求解最优变换 delta_T solve_rigid_transform(source, target, correspondences) T delta_T T if converged(delta_T): break return T整个过程的终止条件通常是变换量小于阈值、误差变化小于阈值、或者达到最大迭代次数。ICP的收敛速度通常很快好的初始位姿下10-20次迭代就能收敛。对应关系搜索——KD树和法线约束找最近点是ICP最耗时的步骤。加速这一步最常用的数据结构是KD树。把target点云构建成KD树查询最近点的时间从O(N)降到O(logN)整体复杂度从O(NM)降到O(NlogM)。PCL库里的ICP实现默认就用KD树。构建KD树是一次性的开销O(M*logM)之后每次查询只要O(logM)。# 用KD树加速最近点搜索 from scipy.spatial import KDTree tree KDTree(target_points) distances, indices tree.query(source_points, k1)除了距离最近还可以加额外约束来改善对应关系的质量。法线约束是最常用的一种两个点的法线方向差异太大就不认为是好的对应。这在平面区域特别有用可以防止平面上的点匹配到边缘上的点。颜色约束也是一种选择。如果点云有颜色信息可以要求对应点的颜色差异小于阈值。这在纹理丰富的场景中可以显著减少错误对应。点到面ICP——精度更高的变体标准ICP用的是点到点距离最小化source中每个点到其对应点的距离。这种形式简单直接但有个问题当两组点云的表面近似平行时点到点距离在切向方向的约束很弱导致收敛慢、精度低。点到面ICPpoint-to-plane ICP改进了这个问题。它最小化的是source中每个点到对应点所在平面的距离而不是到对应点本身的距离。# 点到面ICP的误差函数 def point_to_plane_error(source, target, normals): errors [] for s, t, n in zip(source, target, normals): # 投影到法线方向的距离 errors.append(np.dot(s - t, n)) return np.array(errors)点到面ICP利用了表面的局部几何信息收敛速度比点到点快很多最终精度也更高。在平面区域多的场景比如室内墙壁、地板点到面ICP的优势特别明显。不过它需要计算法线而且法线要准确否则效果反而不如点到点ICP。广义ICP和鲁棒核函数广义ICPGeneralized ICPGICP是点到面ICP的进一步推广。它把每个点都表示成一个局部协方差矩阵通过协方差矩阵来融合点到点和点到面的信息。在平坦区域自动退化为点到面在边缘区域自动退化为点到点适应性更强。GICP在自动驾驶和机器人领域用得很多因为室外场景地形复杂纯粹的平面和纯粹的边缘都有GICP能自适应处理。特征ICP和多尺度策略标准ICP对所有点一视同仁但实际上点云中不同区域的点贡献差很多。LOAM系列的做法是只提取两类特征点边缘点曲率大的点和平面点曲率小的点。边缘点用点到线距离匹配平面点用点到面距离匹配。这种做法本质上是特征ICP把点数从几万降到几千速度大幅提升精度还不降。多尺度策略也是加速ICP的常用技巧。先在低分辨率下做粗配准得到一个粗略的变换然后在高分辨率下做精配准。低分辨率阶段用少量点快速收敛到大致正确的位置高分辨率阶段用全部点精调最终结果。这种先粗后精的策略在点云数量大的场景下效果很好。# 多尺度ICP策略 def multi_scale_icp(source, target, levels[4, 2, 1]): T np.eye(4) for voxel_size in levels: s downsample(source, voxel_size) t downsample(target, voxel_size) T icp(s, t, init_TT) return T鲁棒核函数也是提升ICP鲁棒性的重要手段。标准ICP用的是L2范数平方误差一个离群点就能把整个变换拉偏。加上Huber核或者Tukey核之后大的残差会被降权离群点的影响就被抑制了。# Huber核函数降权 def huber_weight(residual, delta0.1): abs_r np.abs(residual) weight np.where(abs_r delta, 1.0, delta / abs_r) return weight面试追问环节面试官ICP对初始位姿有什么要求ICP是一个局部优化算法只能收敛到最近的局部最优解。如果初始位姿偏差太大ICP会收敛到错误的解。经验上来说平移偏差在几十厘米以内、旋转偏差在十几度以内ICP通常能正确收敛。初始位姿可以通过里程计、IMU、或者其他全局配准方法比如FPFHRANSAC来提供。面试官ICP怎么处理部分重叠的点云部分重叠是ICP面临的一个实际难题。不重叠区域的点没有真正的对应关系强制匹配会产生错误的对应点拉偏结果。处理方法有几种一是用距离阈值过滤对应点距离太大的直接丢弃二是用重叠区域估计只保留重叠部分的点参与优化三是用Trimmed ICP每次迭代只保留距离最近的一部分对应点比如前70%。面试官ICP和NDT的区别是什么ICP基于点对应关系NDT基于概率分布。ICP需要逐点找最近邻计算量大对噪声敏感。NDT把目标点云体素化每个体素用一个高斯分布表示不需要找对应点对噪声更鲁棒。但NDT的精度受体素分辨率限制通常不如ICP精细。实际中经常先用NDT做粗配准再用ICP做精配准。面试官ICP的计算复杂度是多少怎么加速每轮迭代找最近点是O(NlogM)用KD树求解变换是O(N)。总复杂度是O(KN*logM)K是迭代次数。加速方法下采样减少点数、用多分辨率策略先粗后精、用GPU并行化最近点搜索、用点到面减少迭代次数。LOAM系列用的特征点ICP只匹配边缘点和平面点点数从几万降到几千速度很快。面试官怎么判断ICP是否收敛正确看几个指标。一是最终的平均误差RMSE如果明显大于传感器噪声水平可能收敛到了错误解。二是重叠率对应点中被保留的比例太低说明重叠不够。三是变换量的变化曲线如果迭代过程中变换量先大后小再大说明可能跳出了局部最优。四是对配准结果做可视化检查看两组点云是否真的对齐了。ICP虽然是一个30多年前的老算法但它在点云配准领域的地位至今无可替代。简单、直接、精度高这些优点让ICP成为几乎所有3D SLAM系统的标配。理解ICP的原理和局限性对做好SLAM项目很有帮助。ICP是局部配准算法需要一个不错的初始位姿。如果初始位姿完全未知就需要全局配准方法。上一篇第254篇 回环检测技术——词袋模型和位置识别下一篇我们聊NDT正态分布变换这是另一种点云配准方案在大规模场景下比ICP更高效。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力