Vins-Fusion初始化位姿——3D-2D：PNP求解当前帧位姿

继上一篇博文Vins-Fusion 外参camera-imu 标定，本文继续介绍Vins-Fusion初始化时，通过PNP求解当前帧位姿。

一、3D-2D：PNP

PnP是求解3D到2D点对运动的估计，描述的是知道n个3D空间的点及其投影位置，如何估计相机的位姿。因为其无需对极约束，故更少匹配点可获得较好的运动估计。特征点的3D位置可直接由三角化或者深度图确定，双目相机无需进行特定的初始化（如单目初始化时需使相机产生一定平移）。
PnP求解常用方法有，直接线性变换（DLT）和非线性优化方法。

1.优化迭代方法

2.P3P：可以使用任意4个特征点求解，不要求共面，特征点数量不为4时报错

参考论文《complete solution classification for perspective-n-point problem》

3.EPNP：只要特征点数量大于3就可求出正解

参考论文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》

源码中直接调用的cv solvePnP求解。

二、当前帧位姿3D-2D：PNP 求解流程

1.准备路标点位姿(3d坐标)及其在相机归一化平面的投影点

源码中对第一帧未作处理，因为此时路标点还未完成三角化&#