相机运动估计 三、 3D - 3D: ICP

 

1 SVD 方法

 

2 非线性优化方法

于是，在非线性优化中只需不断迭代，我们就能找到极小值。而且，可以证明 [6] ， ICP 问题存在唯一解或无穷多解的情况。

在唯一解的情况下，只要我们能找到极小值解，那么 这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。

这也意味着 ICP 求解可以任意选定初始值。这是已经匹配点时求解 ICP 的一大好处。 需要说明的是，我们这里讲的 ICP ，

是指已经由图像特征给定了匹配的情况下，进行 位姿估计的问题。在匹配已知的情况下，这个最小二乘问题实际上具有解析解 [52, 53, 54] ，

所以并没有必要进行迭代优化。 ICP 的研究者们往往更加关心匹配未知的情况。不过，在 RGB-D SLAM 中，由于一个像素的深度数据可能测量不到，

所以我们可以混合着使用 PnP 和 ICP 优化：对于深度已知的特征点，用建模它们的 3D-3D 误差；对于深度未知的特征

点，则建模 3D-2D 的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得 求解更加方便。