3D-2D PnP相关算法

PnP算法大体分为直接法和优化法

常见的直接法包括：P3P、DLT、EPnP
优化的算法包括：LHM、Only pos BA

PnP算法的指标主要包括：匹配点数、鲁棒性、速度、精度
P3P：3对匹配点，需要相机内参
DLT：不需要内参，4点法求出单应矩阵，DLT分解出K、R、T
EPnP：最少4个点，性价比高，精度较高，需要内参K
LHM：复杂度较高，具有全局凸性，收敛性好，精度高。（在物体坐标系建立误差模型）
Only pos BA：复杂度高，需要初始解，精度高。（在图像坐标系建立误差模型）

EPnP算法

EPnP与P3P等直接法主要有两个步骤：
1.利用刚体结构不变性，得到机体坐标系下的3D点坐标（但是欠尺度）
2.根据世界坐标系和机体坐标系对应的3D点求解R、T
2D --> 3D(欠尺度）–> ICP --> R、T

EPnP最大的特点：
1.用世界坐标系/机体坐标系下4个控制点(原点+三个方向点)表示所有世界坐标系/机体坐标系下的3D点
2.再最小二乘求解后使用高斯牛顿法优化控制点提高精度

步骤
1.根据世界坐标系下的3D点，用四个控制点构建质心坐标系
2.使用4个控制点构建的质心坐标系表示每一个点、
3.求解机体坐标系下控制点
4.确定N的数值和（控制点间距离不变性）恢复x的尺度

DLT分解法

给