ICP 为啥会陷入到局部？为何point to Plane 比point to Point 收敛更快？

一、ICP 的优化目标

目标函数

ICP 的目标是求一个刚体变换： R,t

使得两组点云误差最小：

其中：

• pi​ ------ source 点

• qi​ ------ target 最近点

ICP流程：

1 找最近点

2 求最优 R,t

3 变换点云

4 迭代

这实际上是一个 非凸优化问题。

这里就首先要说一下什么是非凸 问题

什么是凸函数

例子：

f(x)=x^2

图像：

      *
    *   *
  *       *
 *         *

性质：

• 只有 一个全局最小值

• 不会有局部最小值

所以优化很简单。

非凸函数是什么

只要不是凸函数，就是非凸函数。

图像特点：

• 有多个山谷

• 有多个局部最小值

例如：

常见的非凸函数的例子

非凸函数

特点：

• 无限多个极值

• 无限局部最优

Rosenbrock函数（优化经典测试函数）

非凸优化方法

多初值优化

最常见工程方法。

步骤：

随机初始化多个点

↓

分别优化

↓

选择最小结果

随机梯度

二、为什么会出现局部最优

数学原理

理解

假设有两条直线点云：

源点云：

* * * * * * *

目标点云：

      * * * * * * *

真实位姿应该是 平移右移。

如果初始位姿错误：
* * * * * * *
      * * * * * * *

ICP 最近邻匹配可能变成：

* -> 最近 *

但对应点 错位：

p1 → q2
p2 → q3
p3 → q4

ICP 会认为：

当前已经很接近最优

于是停止。

三、解决方法

工程中常见方法：

1 好的初始值

来自：

• 里程计

• IMU

• GPS

---

2 RANSAC 初始匹配

使用

Random Sample Consensus

先求粗配准。

---

3 特征匹配

例如：

• FPFH

• SHOT

先做 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 不能解决局部最优，只是缓解。