《视觉SLAM十四讲》学习笔记-3D->2D: PnP问题的由来

PnP问题

PnP为 Perspective-n-Point的简称，是求解3D到2D点对的运动的方法:即给出n个3D空间点时，如何求解相机的位姿。
典型的PnP问题求解方式有很多种，例如P3P， 直接线性变换(DLT), EPnP(Efficient PnP), UPnP。还有非线性的Bundle Adjustment.

DLT, 直接线性变换

高空间点 $P$ 的齐次方程为 $\mathbf{P}=(X, Y, Z)^\top$ ，投影到特征点 $\vec{x}_1=(u_1, v_1, 1)$ ，为求解 $\mathbf{R}$ 和 $\vec{t}$ ,定义增广矩阵 $[\mathbf{R}|\vec{t}]$ :
展开等式后可得到：

s [\begin{matrix} u_{1} \\ v_{1} \\ 1 \end{matrix}] = [\begin{matrix} 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{matrix}] [\begin{matrix} X \\ Y \\ Z \\ 1 \end{matrix}]

$s\begin{bmatrix} u_1\\ v_1 \\ 1 \end{bmatrix}= \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

$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_{1} 2} v_{1} = \frac{t_{5} X + t_{6} Y + t_{7} Z + t_{8}}{t_{9} X + t_{10} Y + t_{11} Z + t_{1} 2}

$u_1 = \frac{t_1X + t_2Y + t_3Z + t_4}{t_9X + t_{10}Y + t_{11}Z + t_12}~~v_1=\frac{t_5X + t_6Y + t_7Z + t_8}{t_9X + t_{10}Y + t_{11}Z + t_12}$
假设：

{\vec{t}}_{1} = (t_{1}, t_{2}, t_{3}, t_{4})^{⊤}, {\vec{t}}_{2} = (t_{5}, t_{6}, t_{7}, t_{8})^{⊤}, {\vec{t}}_{3} = (t_{9}, t_{10}, t_{11}, t_{12})^{⊤}

$\vec{t}_1 = (t_1, t_2, t_3, t_4)^\top,~~\vec{t}_2 = (t_5, t_6, t_7, t_8)^\top,~~\vec{t}_3 = (t_9, t_{10}, t_{11}, t_{12})^\top$
则有：

{\vec{t}}_{1}^{⊤} P - {\vec{t}}_{3}^{⊤} P u_{1} = 0, {\vec{t}}_{2}^{⊤} P - {\vec{t}}_{3}^{⊤} P v_{1} = 0

$\vec{t}_1^\top\mathbf{P} - \vec{t}_3^\top\mathbf{P}u_1 = 0, \vec{t}_2^\top\mathbf{P} - \vec{t}_3^\top\mathbf{P}v_1 = 0$
上式中

\vec{t}

$\vec{t}$ 是待求的变量。易知一个特征点可提供两个关于

\vec{t}

$\vec{t}$ 的约束，假若存在

N

$N$ 个特征点，则有如下方程成立：

[\begin{matrix} P_{1}^{⊤} & 0 & - u_{1} P_{1}^{⊤} \\ 0 & P_{1}^{⊤} & - v_{1} P_{1}^{⊤} \\ ⋮ & ⋮ & ⋮ \\ P_{N}^{⊤} & 0 & - u_{N} P_{N}^{⊤} \\ 0 & P_{N}^{⊤} & - v_{N} P_{N}^{⊤} \end{matrix}] [\begin{matrix} {\vec{t}}_{1} \\ {\vec{t}}_{2} \\ {\vec{t}}_{3} \end{matrix}] = 0

$\begin{bmatrix} \mathbf{P}_1^\top & 0 & -u_1\mathbf{P}_1^\top \\ 0 & \mathbf{P}_1^\top & -v_1\mathbf{P}_1^\top \\ \vdots& \vdots & \vdots \\ \mathbf{P}_N^\top & 0 & -u_N\mathbf{P}_N^\top \\ 0 & \mathbf{P}_N^\top & -v_N\mathbf{P}_N^\top \\ \end{bmatrix} \begin{bmatrix} \vec{t}_1\\ \vec{t}_2 \\ \vec{t}_3 \end{bmatrix} =0$

观察到 $\vec{t}$ 有12个变量，通过方程形式可知最少可以通过6对匹配点即可得到 $\mathbf{T}$ 的解。所以本方法又可称为直接线性变换法；当匹配点大于六对时，可以使用SVD等方法对超定方程求最小二乘解。

注意到DLT解出的T是由R和t两部分构成的，因而 $\mathbf{R}$ 满足 $\mathbf{R}=SO(3)$ ，所以对于T矩阵需要寻找一个最好的旋转矩阵，这可以由QR分解完成，相当于把结果从矩阵空间重影到 $SE(3)$ 流形上，转成旋转和平移两部分。

P3P问题

首先设标记符号定义如上图所示。其中A,B,C为世界坐标系。图中为3D到3D的对应点，所以是把PnP问题转化为ICP问题。

先利用三解形近似关系有以下三解形相似：

△ O a b - △ O A B, △ O b c - △ O B C, △ O a c - △ O A C

$\triangle Oab -\triangle OAB, ~~\triangle Obc -\triangle OBC, ~~\triangle Oac -\triangle OAC$

考虑余弦关系：

\begin{aligned} O A^{2} + O B^{2} - 2 \cdot O A \cdot O B \cdot \cos < a, b >= A B^{2} \\ O B^{2} + O C^{2} - 2 \cdot O B \cdot O C \cdot \cos < b, c >= B C^{2} \\ O A^{2} + O B C^{2} - 2 \cdot O A \cdot O C \cdot \cos < a, c >= A C^{2} \end{aligned}

$\begin{aligned} OA^2 + OB^2-2\cdot OA \cdot OB \cdot \cos<a, b> = AB^2\\ OB^2 + OC^2-2\cdot OB \cdot OC \cdot \cos<b, c> = BC^2\\ OA^2 + OBC^2-2\cdot OA \cdot OC \cdot \cos<a,c> = AC^2\\ \end{aligned}$
左右两边同时除以

O C^{2}

$OC^2$ , 令

x = O A / O C

$x=OA/OC$ ,

y = O B / O C

$y=OB/OC$ 有：

\begin{aligned} x^{2} + y^{2} - 2 x y \cos < a, b >= A B^{2} / O C^{2} \\ y^{2} + 1^{2} - 2 y c o s < b, c >= B C^{2} / O C^{2} \\ x^{2} + 1^{2} - 2 x \cos < a, c >= A C^{2} / O C^{2} \end{aligned}

$\begin{aligned} x^2 + y^2 -2xy\cos<a,b> = AB^2/OC^2\\ y^2 + 1^2 - 2ycos<b,c> = BC^2/OC^2\\ x^2 + 1^2 -2x\cos<a,c> = AC^2/OC^2 \end{aligned}$
再令

v = A B^{2} / O C^{2}

$v=AB^2/OC^2$ ,

v = B C^{2} / O C^{2} O C^{2} / A B^{2}

$v=BC^2/OC^2OC^2/AB^2$ ,

w = A C^{2} / O C^{2} O C^{2} / A B^{2}

$w=AC^2/OC^2 OC^2/AB^2$ ,有

\begin{aligned} x^{2} + y^{2} - 2 x y \cos < a, b > - v = 0 \\ y^{2} + 1^{2} - 2 y \cos < b, c > - u v = 0 \\ x^{2} + 1^{2} - 2 x \cos < a, c > - w v = 0 \end{aligned}

$\begin{aligned} x^2 + y^2 -2xy\cos<a,b> - v = 0\\ y^2 + 1^2 -2y \cos<b,c> -uv = 0\\ x^2 + 1^2 -2x \cos<a,c> -wv = 0 \end{aligned}$
从上式中先解出

v

$v$ ，代入第二和第三个式子，有

\begin{aligned} (1 - u) y^{2} - u x^{2} - \cos < b, c > y + 2 u x y \cos < a, b > + 1 = 0 \\ (1 - w) x^{2} - w y^{2} - \cos < a, c > x + 2 w x y \cos < a, b > + 1 = 0 \\ x^{2} + 1^{2} - 2 x \cos < a, c > - w v = 0 \end{aligned}

$\begin{aligned} (1-u)y^2 - ux^2 -\cos<b,c>y + 2uxy\cos<a,b> + 1 = 0\\ (1-w)x^2 - wy^2 -\cos<a,c>x + 2wxy\cos<a,b> + 1 = 0\\ x^2 + 1^2 -2x \cos<a,c> -wv = 0 \end{aligned}$
上式中由于

A, B, C

$A, B, C$ 已知，

x, y

$x, y$ 未知。是一个二元二次方程，最多可得到4个解。需要一个验证点来获得最优解。

可以看到， 利用三角形的相似性质，PnP问题转为了一个3D到3D的位姿估计问题。

P3P的问题：
1. 只利用三个点的信息，当给定的配对点多于3组时，难以利用更多的信息；
2. 如果数据点存在噪声时，或者匹配是误匹配的情况下，算法失败。

PnP在SLAM问题中的角色：进行相机位姿估计，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment, BA)