PnP

本文主要介绍关于PnP（pespective-n-point）的一些方法。这个问题是一个经典的视觉测量问题。这里介绍一下常用的几种方法

问题的方案、解决思路

最小PnP问题

P3P问题中假设没有噪声，使用几何约束，可以解得相机的位姿。不具有唯一解。
P4P问题中分为线性方法和基于P3P的方法。

最小二乘的观点

• 迭代最小化一个代价函数（平方误差）。这些方法相对于之前的方法更加准确，在一定的噪声的情况下，返回一个最大似然估计。
• 直接最小二乘方法DLS

常用方法

• NPL: The N-Point Linear (NPL) method of Ansar and Daniilidis [1].
• EPnP: The approach of Lepitit et al. [16].
• SDP: The Semi Definite Program (SDP) approach of Schweighofer and Pinz [23].
• DLS: The Direct Least-Squares (DLS) solution presented in this paper. An open source implementation of DLS is available at www.umn.edu/ ̃joel
• DLS-LM: Maximum-likelihood estimate, computed using iterative Levenberg-Marquardt (LM) minimization of the sum of the squared reprojection errors, initialized with DLS.

opencv

solvePnP里有三种解法：P3P， EPnP，迭代法（默认）；opencv2里参数分别为CV_P3P，CV_EPNP，CV_ITERATIVE （opencv3里多了DLS和UPnP解法）。
注意点2：solvePnP需要至少3组点：P3P只使用4组点，3组求出多个解，第四组确定最优解；EPnP使用大于等于3组点；迭代法调用cvFindExtrinsicCameraParams2，进而使用SVD分解并调用cvFindHomography，而cvFindHomography需要至少4组点。
具体过程如下
- 将空间点和图像点齐次化，得到图像点矩阵$\mathbf{m}$空间点矩阵$\mathbf{M}$，求取矩阵M的平均值$M_c$，
- 计算另外一个矩阵$\mathbf{mm}=( \mathbf{M}− M_c ) ^T ( \mathbf{M} − M_c )$
- 对空间点矩阵$\mathbf{mm}$进行SVD分解，$\mathbf{mm}=\mathbf{UWV}$
- $\mathbf{R}_t=\mathbf{V}$
- $\mathbf{T}_t=-M_c\mathbf{R}_t$
- $\mathbf{M}_{xy}=\mathbf{V}_t\mathbf{M}^T+\mathbf{T}_t$
- find homography between ($\mathbf{m}$和$\mathbf{M}_{xy}$)得到矩阵$\mathbf{H}$
- $\mathbf{H}=[\mathbf{h}_1,\mathbf{h}_2,\mathbf{t}]$，然后归一化
- $\mathbf{h}_1=\frac{\mathbf{h}_1}{\|\mathbf{h}_1\|}$
- $\mathbf{t}=\frac{\mathbf{t}}{\|\mathbf{h}_1\|+\|\mathbf{h}_2\|}$
- $\mathbf{h}_3=\mathbf{h}_1\times\mathbf{h}_2$
- $\mathbf{H}:=[\mathbf{h}_1,\mathbf{h}_2,\mathbf{h}_3]$
- 最终结果$\mathbf{R}_f=\mathbf{H}*\mathbf{R}_t$
- $\mathbf{t}_f=\mathbf{H}*\mathbf{T}_t+\mathbf{t}$

其他

R的第i行 表示摄像机坐标系中的第i个坐标轴方向的单位向量在世界坐标系里的坐标；
R的第i列 表示世界坐标系中的第i个坐标轴方向的单位向量在摄像机坐标系里的坐标；
t 表示世界坐标系的原点在摄像机坐标系的坐标；
-R的转置 * t 表示摄像机坐标系的原点在世界坐标系的坐标。（原理如下图，t表示平移，T表示转置）

DLS

1. http://onlinelibrary.wiley.com/doi/10.1002/rob.21620/epdf
2. http://www.voidcn.com/blog/abc20002929/article/p-2288889.html
3. http://blog.csdn.net/aptx704610875/article/details/48915149
4. https://github.com/gaoxiang12/rgbd-slam-tutor2/blob/master/src/pnp.cpp