pnp 算法求解

引言

PNP问题的描述以及定义是相对简单的，他的目的就是求解3D-2D点对运动的方法。简单来说，就是在已知n个三维空间点坐标（相对于某个指定的坐标系A）及其二维投影位置的情况下，如何估计相机的位姿（即相机在坐标系A下的姿态）。

举个例子，我们在一幅图像中，知道其中至少四个图像中确定的点在3D空间下的相对坐标位置，我们就可以估计出相机相对于这些点的姿态，或者说估计出这些3D点在相机坐标系下姿态。（上述说的姿态或者位姿，包括位置以及方向，即一个6自由度的状态） $^{[1.]}$

PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P 、直接线性变换(DLT)、EPnP、SDP、UPnP。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment $^{[2.]}$ 。

[x] Edit By Porter, 积水成渊,蛟龙生焉。
[x] Insert Date String 插件，ctrl+shift+I 插入时间字符串

pnp 求解过程

Perspective-n-Point问题(PnP)的已知条件：

n个世界坐标系中的3D参考点(3D reference points)坐标;
与这n个3D点对应的、投影在图像上的2D参考点(2D reference points)坐标;
摄像头的内参K KK;
求解PnP问题可以得到摄像头的位姿。

OpenCV中提供了solvePnP函数可以直接求解出旋转矩阵和平移矩阵。

pnp 求解原理

对于视觉里程计中，相机位姿的求解问题极为常见。对于双目相机，由于其可以直接计算出深度信息，所以在相机位姿求解上十分容易。但如果我们使用的是单目相机，如何从二维图像中求解出相机相对三维物体的位姿就需要一定的算法来完成 $^{[7.]}$ 。

直接线性变换(DLT) $^{[8.]}$ $^{[9.]}$

此部分按照《视觉slam十四讲》做了个笔记，直接线性变换仅仅需要6对3d-2d匹配点，即可求解出相机3d到2d的 $R| t$ 矩阵。

考虑空间一点 $P=(X,Y,Z,1)^{T}$ 。在图像 $I_{1}$ 中投影到图像特征点 $x_{1}=(u_{1}, v_{1}, 1)^{T}$ （以归一化平面齐次坐标表示）。我们需要求解相机位姿 $R,t$ ，与单应矩阵的求解类似，我们定义增广矩阵 $[R|T]$ 为一个3x4的矩阵，包含了旋转平移信息。将其展开如下形式

\begin{pmatrix} u_{1} \\ v_{1} \\ 1 \end{pmatrix} =\begin{pmatrix} t_{1} &t_{2} & t_{3} & t_{4}\\ t_{5} &t_{6} & t_{7} & t_{8}\\ t_{9} &t_{10} &t_{11} &t_{12} \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix}

用最后一行把s消除掉，得到两个约束(一对特征点，对应两个约束)

$u_{1} = \frac{t_{1}X + t_{2}Y+t_{3}Z+t_{4}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$

$v_{1} = \frac{t_{5}X + t_{6}Y+t_{7}Z+t_{8}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$

为了简化表示，定义 $T$ 的行向量

$\mathbf{t}_{1} = (t_{1}, t_{2}, t_{3}, t_{4})^{T}$

$\mathbf{t}_{2} = ( t_{5}, t_{6},t_{7}, t_{8})^{T}$

$\mathbf{t}_{3} = ( t_{9},t_{10},t_{11},t_{12})^{T}$

于是有

$\mathbf{t}_{1}^{T} P - t_{3}^{T} P u_{1} = 0,$

$\mathbf{t}_{2}^{T} P - t_{3}^{T} P v_{1} = 0$

请注意 $\mathbf{t}$ 是待求变量，可以看到，每个特征点提供两个关于 $\mathbf{t}$ 的线性约束，假设一共存在N对特征点，则可以列出如下线性方程：

$\begin{pmatrix} P_{1}^{T} &0 &-u_{1}P_{1}^{T} \\ 0 &P_{1}^{T} &-v_{1}P_{1}^{T} \\ ... &.... &... \\ P_{N}^{T} &0 &-u_{N}P_{1}^{T} \\ 0 &P_{N}^{T} &-v_{N}P_{1}^{T} \end{pmatrix}$

于是6对特诊点，即可得到12组方程式，正好对应12个待求变量 $\mathbf{t}$ 直接求解即可得到变量值。这种方法称为DLT。

直接求解中，当匹配点对大于6个点时，也可以用SVD $^{[11.]}$ 等方法对超定方程求最小二乘解。

P3P

可以参考前面一篇双目相机标定的文章，以及slam 十四讲里面的介绍。

EPnP $^{[12.]}$

与其他方法相比，EPnP方法的复杂度为O(n)，对于点对数量较多的PnP问题，非常高效。

核心思想是将3D点表示为4个控制点的组合，优化也只针对4个控制点，所以速度很快，在求解
$Mx=0$ 时，最多考虑了4个奇异向量，因此精度也很高。

AP3P

参考 opencv 的官方介绍 $^{[13.]}$

UPnP

opencv 中solvepnp函数求解变换矩阵的demo $^{[4.]}$

//定义世界坐标和图像坐标
vector<Point3d> World_Coor = {Point3f(0, 0, 0), Point3f(0, 26.5, 0), Point3f(67.5, 26.5, 0), Point3f(67.5, 0, 0)};
//传入图像坐标
vector<Point2d> Img_Coor;
Img_Coor.push_back(featrue[i].bl());
Img_Coor.push_back(featrue[i].tl());
Img_Coor.push_back(featrue[i].tr());
Img_Coor.push_back(featrue[i].br());
//pnp解算
solvePnP(objectSmallArmor, img_points, cameraMatrix, distcoeff, rvec, tvec, false, SOLVEPNP_IPPE);
Rodrigues(rvec, R_rvec);
// 转换格式
R_rvec.convertTo(R_rvec, CV_64FC1);
tvec.convertTo(tvec, CV_64FC1);
// 转成Eigen下的矩阵
Eigen::Matrix3f Rotated_matrix;
Eigen::Vector3f Tran_vector;
cv2eigen(R_rvec, Rotated_matrix);
cv2eigen(tvec, Tran_vector);

//计算欧拉角
Eigen::Vector3f euler_angles = Rotated_matrix.eulerAngles(0, 1, 2);

picth = euler_angles[0] * 180 / PI;
yaw = euler_angles[1] * 180 / PI;
roll = euler_angles[2] * 180 / PI;

//计算距离
distance = (COEFF_K * sqrt(Tran_vector.transpose() * Tran_vector) + COEFF_B) * cosf(pitch * PI / 180.f);