【视觉SLAM】4b-特征点法估计相机运动之PnP 3D-2D

0. 前言

透视n点（Perspective-n-Point，PnP）问题是计算机视觉领域的经典问题，用于求解3D-2D的点运动。换句话说，当知道$N$个世界坐标系中3D空间点的坐标以及它们在图像上的投影点像素坐标时，可以使用PnP算法来估计相机在世界坐标系的姿态。P3P是最简化的PnP形式，即最少只需3个点即可估计当前的相机姿态（解不唯一）。

总体来说，PnP的求解方法有P3P、直接线性变换（Direct Linear Transformation，DLT）、EPnP（Efficient PnP）和UPnP等。此外，还有非线性优化解法，通过构建最小二乘问题并迭代求解，即万金油式的光束平差法（Bundle Adjustment，BA） 。

1. PnP求解

1.1 直接线性变换DLT

假设有世界坐标系中的3D点$P=[X,Y,Z,1{]}^T$，在图像$I_1$中对应的投影像素点为$x_1=[u_1,v_1,1{]}^T$，根据相机小孔成像模型有：

$$s\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{c}R|t\end{array}\right]P=\left[\begin{array}{cccc}{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{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$

其中$s=Z$，利用最后一行将其消去有：

$$\{\begin{array}{l}s{u}_1=t_{1}X+t_{2}Y+t_{3}Z+t_4\\ s{v}_1=t_{5}X+t_6\end{array}$$