PNP&&ICP

一、PnP
  它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。求解PnP算法的主要方式大致是直接线性变换（DLT），P3P，EPnP。P3P和EPnP都是通过把2D-3D（世界坐标系）匹配关系，变换成3D-3D点的匹配关系，这样就可以求解ICP或者是非线性优化的方式求解。

  P3P的原理就是利用了三角形的相似性质，求解投影点a,b,c在相机坐标系下的3D坐标，最后把问题转换成一个3D到3D的位姿估计问题。

EPnP

四个权重a就是要求解的内容。


算法流程：

• 选择控制点
• 根据3D-2D匹配对中的3D点和控制点，算出a
• 然后根据这个a和2D点，算出控制点在相机坐标系下的3D坐标
• 最后根据3D坐标，和之前的3D坐标构建3D-3D的求解问题。

二、ICP
  知道一对匹配好的3D特征点（相机坐标系），想要找到一个欧式变换R,t使其满足两个3D匹配点的变换关系。在激光SLAM中也会碰到ICP问题，不过由于激光数据的特征不够丰富，因此无法知道两个点集的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。ICP的求解方式分为两种：线性代数求解（SVD）；非线性优化求解。
SVD求解步骤：

参考：视觉SLAM十四讲