SLAM十四讲学习笔记5—— 前端：两图之间关系求解——ICP

分类

1. 2D-2D 对极几何：只有两个单目图像
2. 3D-2D PnP：帧和地图
3. 3D-3D ICP（Iterative Closest Point，ICP）

改进版的ICP

视觉相机可以通过上述特征匹配的方法得到两副图上的点与点一一对应的关系好了。根据已配对好的两组3D点，使用ICP法可以解出两幅图之间的关系。
设：、运动关系：
定义误差项：
最小二乘：
质心：
改写目标函数：

目标函数简化为：

最小化第一项，然后取t，使得后一项为零
旋转的求取：
定义去质心坐标：
最小化：
推导：
最右一项：
SVD解法解得R值：
R计算出后通过质心之间连线可以将t值计算出来。

ICP变体

ICP算法存在许多的变体，其中point-to-point和point-to-plane是最受欢迎的。 point-to-plane通常在结构化环境中表现更好。

3D-3D ICP问题

1. 线性解法即为最优解。但是使用BA解法（因为PnP也可以使用BA解法）时可以ICP与PnP结合在一起求解（当有些点可以得到3D信息，有些不可以时才会考虑两者同时使用）。
2. ICP也可以从非线性优化角度求解，但是.已知匹配时，ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值。当存在退化（即极端）情况时（eg：点都在一条线上，同时照片以此线为轴旋转）会有无穷解。