SLAM常见问题(三):PNP

PNP即“Perspective-N-Points”,是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n个3D空间点以及它们在图像上的位置时,如何估计相机所在的位姿。PnP 问题有很多种求解方法,例如用三对点估计位姿的 P3P(通常需要额外一个点进行验证结果),直接线性变换(DLT),EPnP(Efficient PnP,已知内参时用),UPnP(内参未知时用) 等等)。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment

P3P

已知:$3D-2D$匹配点,$3D$点的世界坐标记为$A, B, C$,图像上的2D点记为$a, b, c$。

未知:相机系下3D点的坐标是未知的,即$OA,OB,OC$,一旦$ 3D$ 点在相机坐标系下的坐标能够算出,我们就得到了$3D-3D$的对应点,把PnP问题转换为了ICP问题。

我们的目标就是通过纯几何的方法求出上述未知量,过程如下。

由于余弦定理可知:

对上面三式全体除以$OC^{2}$,记$x=O A / O C, y=O B / O C$,得:

记$v=A B^{2} / O C^{2}, u v=B C^{2} / O C^{2}, w v=A C^{2} / O C^{2}$,得:

将第一个式子中$v = x^{2}+y^{2}-2 x y \cos \langle a, b\rangle$带入后面两个式子中,得:

上式中几个余弦角度$\cos \langle a, b\rangle, \cos \langle b, c\rangle, \cos \langle a, c\rangle$是已知的,$u=B C^{2} / A B^{2}, w=A C^{2} / A B^{2}$也是已知的,所以未知量仅有$x,y$,解析地求解该方程组是一个复杂的过程,需要用吴消元法。这样就可以求得$x,y$,然后带入$v = x^{2}+y^{2}-2 x y \cos \langle a, b\rangle$求解$v$,即可得到$OC$,进而得到$OB,OA$。该方程最多可能得到四个解,但我们可以用第4个验证点来计算最可能的解,得到$ A, B, C$ 在相机坐标系下的$3D$坐标。然后,根据$ 3D-3D $的点对,计算相机的运动 $R,t$,此处可参考文献Least-Squares Rigid Motion Using SVD

EPnP

问题描述

EPnP即Efficient PnP,参考文献 EPnP: An Accurate O(n) Solution to the PnP Problem

问题描述如PnP,更加具体的,我们已知一组特征点,对于每个特征点$i$,我们有如下信息:

  • 特征点 $i$ 在世界坐标系的坐标$P_{i}^{w}=\left[\begin{array}{c}{x_{i}^{w}} \ {y_{i}^{w}} \ {z_{i}^{w}}\end{array}\right]$

  • 特征点在成像平面上的坐标$p_{i}=\left[\begin{array}{l}{u_{i}} \ {v_{i}}\end{array}\right]$

  • 已知相机内参$K$

求:世界坐标系到相机系的变换矩阵

算法假设

EPnP的思想是无论世界系还是相机系下的$3D$点都可以由4个控制点线性组合,记:

  • 世界系下4个控制点表示为:$\mathbf{c}_{j}^{w}, j=1, \cdots, 4$
  • 相机系下4个控制点表示为:$\mathbf{c}_{j}^{c}, j=1, \cdots, 4$

EPnP算法将参考点的坐标表示为控制点坐标的加权和:

其中$\alpha_{i, j}, j=1, \cdots, 4$是加权系数,一旦虚拟控制点确定后,且满足4个控制点不共面的前提,$\alpha_{i, j}$是唯一的。

控制点的存在性

现在讨论控制点的存在性,上式可以写成:

可见只要$C$非奇异,就一定可以找到满足条件的$\alpha_{i} $,即:

接下来,我们讨论相机坐标系下,控制点和参考$3D$点之间的关系:

由于$\sum_{j=1}^{4} \alpha_{i j}=1$,因此$t=\sum_{j=1}^{4} \alpha_{i j} t$,带入上式,得:

可见系数$\alpha_{i} $具有不变性,如果我们能够求出控制点在相机坐标系中的坐标$c_{1}^{c}, c_{2}^{c},c_{3}^{c},c_{4}^{c}$,那么对于任意一个3D点k,我们可以求得其在相机系下的坐标:$p_{k}^{c}=\sum_{j=1}^{4} \alpha_{k j} c_{i}^{c}$,这就变成了如P3P同样的问题了,即求解3D-3D位姿估计问题。

如何选择控制点

记世界系下所有3D点集为,第一个控制点是所有3D点的重心:

对所有3D点去中心化,这些点罗列成矩阵形式:

对$A^TA$进行特征值分解(注意此时并非对A进行SVD分解,是为了减低时间复杂度,SVD分解的复杂度为$SO(3)$),其特征值为$\lambda_{c, i}, i=1,2,3$,对应的特征向量为$\mathbf{v}_{c, i}, i=1,2,3$,则剩余的3个控制点表示为如下公式:

求解控制点在相机系下的坐标

为相机下$3D$点的图像坐标,则:

其中$w_{i}$是尺度因子,将控制点$\mathbf{c}_{j}^{c}=\left[x_{j}^{c}, y_{j}^{c}, z_{j}^{c}\right]^{T}$带入上式,得:

上式可以得到两个线性方程:

把这N个点的约束罗列在一起,我们就可以得到如下矩阵:

其中$\mathbf{x}=\left[\mathbf{c}_{1}^{c \top}, \mathbf{c}_{2}^{c \top}, \mathbf{c}_{3}^{c \top}, \mathbf{c}_{4}^{c \top}\right]^{\top}$为12维向量,$\mathbf{M}$维度$2n\times 12$,如下形式:

未完待续…

参考