一、ICP 的优化目标目标函数ICP 的目标是求一个刚体变换 R,t使得两组点云误差最小其中pi​ —— source 点qi​ —— target 最近点ICP流程1 找最近点2 求最优 R,t3 变换点云4 迭代这实际上是一个非凸优化问题。这里就首先要说一下什么是非凸 问题什么是凸函数例子f(x)x^2图像* * * * * * *性质只有一个全局最小值不会有局部最小值所以优化很简单。非凸函数是什么只要不是凸函数就是非凸函数。图像特点有多个山谷有多个局部最小值例如常见的非凸函数的例子非凸函数特点无限多个极值无限局部最优Rosenbrock函数优化经典测试函数非凸优化方法多初值优化最常见工程方法。步骤随机初始化多个点↓分别优化↓选择最小结果随机梯度二、为什么会出现局部最优数学原理理解假设有两条直线点云 源点云 * * * * * * * 目标点云 * * * * * * * 真实位姿应该是 平移右移。 如果初始位姿错误 * * * * * * * * * * * * * * ICP 最近邻匹配可能变成 * - 最近 * 但对应点 错位 p1 → q2 p2 → q3 p3 → q4 ICP 会认为 当前已经很接近最优 于是停止。三、解决方法工程中常见方法1 好的初始值来自里程计IMUGPS2 RANSAC 初始匹配使用Random Sample Consensus先求粗配准。3 特征匹配例如FPFHSHOT先做 feature matching。4 使用 Point-to-Plane ICP优化5 多分辨率 ICP粗点云 → 细点云逐层优化。四、为何point to Plane 比point to Point 收敛更快Point-to-Plane 收敛更快、稳定性更好但并不能彻底解决局部最优问题只是降低了发生概率。Point-to-Plane ICP点到平面几何意义只计算点到平面的距离n ↑ | p_i → 平面 ---------误差是沿法向方向距离而不是点到点距离。为什么 Point-to-Plane 收敛更快几何解释方法收敛速度稳定性局部最优Point-to-Point慢一般容易Point-to-Plane快更稳定仍存在核心原因① 误差函数更接近真实几何约束真实目标是点应该落在目标表面上而不是点必须匹配到某个具体点。Point-to-Plane 正好表达distance to surfacedistance\ to\ surfacedistance to surface因此误差模型更合理。② 优化函数更接近线性③ 沿切线方向允许滑动这是最重要的一点。 假设两个平面 源点云 ************ 目标点云 ---------------- Point-to-Point 必须匹配到最近点 Point-to-Plane 允许沿平面滑动因为误差只看法向切向误差 几乎为0所以优化空间更平滑数学解释为什么仍然会有局部最优Point-to-Plane 仍然依赖 最近邻匹配 如果最近邻错误 p_i → 错误平面 优化仍会收敛到错误位置。 例如 重复柱子 | | | | | 匹配错柱子 柱1 → 柱2 误差仍然很小。 因此 Point-to-Plane 不能解决局部最优只是缓解。