【视觉SLAM】4b-特征点法估计相机运动之PnP 3D-2D
文章目录
- 0. 前言
- 1. PnP求解
- 1.1 直接线性变换DLT
- 1.2 P3P
- 1.3 光束平差法BA
- 2. 实现
0. 前言
透视n点（Perspective-n-Point，PnP）问题是计算机视觉领域的经典问题，用于求解3D-2D的点运动。换句话说，当知道 N N 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 P=[X, Y, Z, 1]^T P=[X,Y,Z,1]T，在图像 I 1 I_1 I1中对应的投影像素点为 x 1 = [ u 1 , v 1 , 1 ] T x_1=[u_1, v_1, 1]^T x1=[u1,v1,1]T，根据相机小孔成像模型有：
s [ u 1 v 1 1 ] = [ R ∣ t ] P = [ t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ] [ X Y Z 1 ] s \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix}= \begin{bmatrix} R | t \end{bmatrix} P= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} s u1v11 =[R∣t]P= t1t5t9t2t6t10t3t7t11t4t8t12 XYZ1
其中 s = Z s=Z s=Z，利用最后一行将其消去有：
{ s u 1 = t 1 X + t 2 Y + t 3 Z + t 4 s v 1 = t 5 X + t 6 Y + t 7 Z + t 8 s = t 9 X + t 10 Y + t 11 Z + t 12 ⇒ { u 1 = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 v 1 = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 \begin{cases} s u_1 = t_1 X + t_2 Y + t_3 Z + t_4\\ s v_1 = t_5 X + t_6 Y + t_7 Z + t_8\\ s = t_9 X + t_{10} Y + t_{11} Z + t_{12} \end{cases} \Rightarrow \begin{cases} 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}} \\ \end{cases} \\ ⎩ ⎨ ⎧su1=t1X+t2Y+t3Z+t4sv1=t5X+t6