计算相机运动PnP

在运动估计过程中，得到了两幅图片的特征匹配后，下一步要做的就是计算这两张图片发生的相对运动。特征点对在第一幅图片中的3D信息是知道的，也就是知道特征点在世界坐标系下的3D坐标和在第二幅图片中的像素坐标（这在slam中是常见的求解位姿的情况）。有了这样的对应关系，对相机运动进行求解的方法叫做PnP：Perspective-n-Point。

PnP也叫做3D-2D的运动估计方法，它与2D-2D的对极几何求解方法不同的是，对极几何需要8个或8个以上的点对，且存在初始化，纯旋转和尺度的问题。然而，如果两张图像中一张特征点的3D位置已知，那么至少只需3个点对（以及一个额外点验证结果）就可以估计相机运动。^{[摘自十四讲]}

计算机视觉以及slam发展了这么多年，前人为了解决这个问题已经提出了多种方法：简单粗暴的代数方法DLT，基于几何方法求解的P3P，不太熟悉的EPnP和UPnP，还有非线性优化方法。
这些方法在OPenCV中有写好的函数solvePnP,它的函数原型：
https://docs.opencv.org/3.3.0/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d
这个函数实现的功能是求解PnP，默认使用SOLVEPNP_ITERATIVE的方法去求解PnP，原理是最小化重投影误差;还可以选择SOLVEPNP_P3P等方法。