对级几何、PnP和ICP估计三维位姿

视觉SLAM中估计三维位姿的三种方式：
首先摆出结论：

对级约束

用在单目初始化中估计第一帧和第二帧之间的位姿变化R,t。再进行三角化求出特征点的相对深度，后续的帧间位姿都是使用PnP来估计的。
对级几何中首先根据匹配的特征点计算E、F或H，八点法等，最后根据SVD分解求得R，t。

PnP

已知：第一帧中特征点对应的空间点的相机坐标系下的位姿、第二帧中匹配好的二维特征点。

首先需要搞清楚几个坐标系以及他们之间的关系，否则多视图几何就是一锅粥
世界坐标系：与相机坐标系相差外参R，t，在多视图几何中我们把参考帧的相机坐标系作为世界坐标系，对于当前帧看到的空间中一点P，它的世界坐标就是它在参考帧中相机坐标系下的点。
相机坐标系：与像素坐标相差内参K，原点在相机光心，，一般用的是相机归一化坐标
像素坐标系：一般以图像的左上角为原点，特征点的二维坐标。
参考博客：点击 here

要么是单目三角化、要么是双目或深度相机直接得到第一帧中空间点P的3D位置（第一帧中的相机坐标系下），又根据特征匹配得到第二帧中与第一帧中匹配的特征点。
对于第二帧来说，点P的世界坐标系下的3D位置是已知的，图像I1和I2之间的位姿变换为R，t，所以第一帧（参考帧）的相机坐标系下（就是当前帧的世界坐标系）的空间点P，对于第二帧来说，P点的世界坐标系下的3D位置是已知的。
说的这么多，其实是想解释下网上说的这句话：参考博客

“PnP问题解决了已知世界参考系下地图点以及相机参考系下投影点位置时3D-2D相机位姿估计问题”

所以说PNP核心解决的问题是根据世界坐标系下的点P的3D位置坐标求出在第二帧相机坐标系下的位置坐标，说好的“3D-2D”呢？就是空间中3D点和匹配好的2D点已知。
得到了第二帧的相机坐标系下的P点的3D位置坐标，第一帧的P的相机坐标系下的3D位置坐标已知，这就转换成了"3D-3D"估计位姿的问题。
注意：PnP最后也是通过ICP来估计位姿的
解法：线性解法（P3P和EPnP等）和非线性解法（BA）
为充分利用匹配的特征点，用到了随机一致性采样（RANSAC）。

ICP

（迭代最近点，ICP）；
已知一组匹配好的3D点，估计位姿（如：对两幅RGB-D图像）
那么问题是：PnP也用了ICP，和这个ICP有啥区别呢？
答：求解方法完全么的区别，就是初始条件有的区别，PnP中的空间点P是从第一帧（参考帧）中来的，投影到第二帧中，求得第二帧中的相机坐标系下的3D位姿，所以叫“3D-2D”的方法；但是这里的ICP初始值不是这样的，而是每帧他都有自己的深度值（如深度相机）每一帧都可以直接求得空间点P在各自帧中相机坐标系下 的空间坐标位置，不再是通过投影得到的了。所以说ICP就是要知道3D点在对应各自帧中相机坐标系下的点P位置坐标，再进行求解，所以叫“3D-3D”。
解法：线性解法（SVD）和非线性解法（BA）