📝笔记：SLAM常见问题(三)：PNP

Posted on 2019-08-11 Edited on 2024-05-11 In SLAM Waline: Views:

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问题。

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

由于余弦定理可知：

\[ \begin{array}{l}{O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle= A B^{2}} \\ {O B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle= B C^{2}} \\ {O A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle= A C^{2}}\end{array} \]

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

\[ \begin{array}{l}{x^{2}+y^{2}-2 x y \cos \langle a, b\rangle= A B^{2} / O C^{2}} \\ {y^{2}+1^{2}-2 y \cos \langle b, c\rangle= B C^{2} / O C^{2}} \\ {x^{2}+1^{2}-2 x \cos \langle a, c\rangle= A C^{2} / O C^{2}}\end{array} \]

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

\[ \begin{array}{l}{x^{2}+y^{2}-2 x y \cos \langle a, b\rangle- v=0} \\ {y^{2}+1^{2}-2 y \cos \langle b, c\rangle- u v=0} \\ {x^{2}+1^{2}-2 x \cos \langle a, c\rangle- w v=0}\end{array} \]

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

\[ \begin{array}{l}{(1-u) y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+ 1=0} \\ {(1-w) x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+ 1=0}\end{array} \]

上式中几个余弦角度$\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$

求：世界坐标系到相机系的变换矩阵\[T_{c w}=\left[\begin{array}{cc}{R_{c w}} & {t} \\ {0} & {1}\end{array}\right]\]

算法假设

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

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

EPnP算法将参考点的坐标表示为控制点坐标的加权和： \[ \mathbf{p}_{i}^{w}=\sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{w}, \text { with } \sum_{j=1}^{4} \alpha_{i j}=1 \] 其中$\alpha_{i, j}, j=1, \cdots, 4$是加权系数，一旦虚拟控制点确定后，且满足4个控制点不共面的前提，$\alpha_{i, j}$是唯一的。

控制点的存在性

现在讨论控制点的存在性，上式可以写成： \[ \left[\begin{array}{c}{p_{i}^{w}} \\ {1}\end{array}\right]=\left[\begin{array}{cccc}{C_{1}^{w}} & {C_{2}^{w}} & {C_{3}^{w}} & {C_{4}^{w}} \\ {1} & {1} & {1} & {1}\end{array}\right] \alpha_{i} \stackrel{令}{=} C \alpha_{i} \] 可见只要$C$非奇异，就一定可以找到满足条件的$\alpha_{i}$，即：

\[ \left[\begin{array}{l}{\alpha_{i 1}} \\ {\alpha_{i 2}} \\ {\alpha_{i 3}} \\ {\alpha_{i 4}}\end{array}\right]=C^{-1}\left[\begin{array}{c}{\mathbf{p}_{i}^{w}} \\ {1}\end{array}\right] \]

接下来，我们讨论相机坐标系下，控制点和参考$3D$点之间的关系： \[ p_{i}^{c}=R_{c w} p_{i}^{w}+t=R_{c w}\left(\sum_{j=1}^{4} \alpha_{i j} c_{i}^{w}\right)+t \] 由于$\sum_{j=1}^{4} \alpha_{i j}=1$，因此$t=\sum_{j=1}^{4} \alpha_{i j} t$,带入上式，得： \[ p_{i}^{c}=\sum_{j=1}^{4} \alpha_{i j}\left(R_{c w} c_{i}^{w}\right)+t )=\sum_{j=1}^{4} \alpha_{i j} c_{i}^{c} \] 可见系数$\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点集为$\left\{\mathbf{p}_{i}^{w}, i=1, \cdots, n\right\}$,第一个控制点是所有3D点的重心:

\[ \mathbf{c}_{1}^{w}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}^{w} \] 对所有3D点去中心化，这些点罗列成矩阵形式： \[ A=\left[\begin{array}{c}{\mathbf{p}_{1}^{w^{T}}-\mathbf{c}_{1}^{w^{T}}} \\ {\cdots} \\ {\mathbf{p}_{n}^{w^{T}}-\mathbf{c}_{1}^{w^{T}}}\end{array}\right] \] 对$A^TA$进行特征值分解（注意此时并非对A进行SVD分解，是为了减低时间复杂度，SVD分解的复杂度为$SO(3)$），其特征值为$\lambda_{c, i}, i=1,2,3$，对应的特征向量为$\mathbf{v}_{c, i}, i=1,2,3$，则剩余的3个控制点表示为如下公式：

\[ \mathbf{c}_{j}^{w}=\mathbf{c}_{1}^{w}+\lambda_{c, j-1}^{\frac{1}{2}} \mathbf{v}_{c, j-1}, j=2,3,4 \]

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

记$\left\{\mathbf{u}_{i}\right\}_{i=1, \cdots, n}$为相机下$3D$点$\left\{\mathbf{p}^c_{i}\right\}_{i=1, \cdots, n}$的图像坐标，则： \[ \forall i, \quad w_{i}\left[\begin{array}{c}{\mathbf{u}_{i}} \\ {1}\end{array}\right]=K \mathbf{p}_{i}^{c}=K \sum_{j=1}^{4} \alpha_{i j} \mathbf{c}_{j}^{c} \]

其中$w_i$是尺度因子，将控制点$\mathbf{c}_{j}^{c}=\left[x_{j}^{c}, y_{j}^{c}, z_{j}^{c}\right]^{T}$带入上式，得： \[ \forall i, \quad w_{i}\left[\begin{array}{c}{u_{i}} \\ {v_{i}} \\ {1}\end{array}\right]=\left[\begin{array}{ccc}{f_{u}} & {0} & {u_{c}} \\ {0} & {f_{v}} & {v_{c}} \\ {0} & {0} & {1}\end{array}\right] \sum_{j=1}^{4} \alpha_{i j}\left[\begin{array}{c}{x_{j}^{c}} \\ {y_{j}^{c}} \\ {z_{j}^{c}}\end{array}\right] \] 上式可以得到两个线性方程： \[ \begin{array}{l}{\sum_{j=1}^{4} \alpha_{i j} f_{u} x_{j}^{c}+\alpha_{i j}\left(u_{c}-u_{i}\right) z_{j}^{c}=0} \\ {\sum_{j=1}^{4} \alpha_{i j} f_{v} y_{j}^{c}+\alpha_{i j}\left(v_{c}-v_{j}\right) z_{j}^{c}=0}\end{array} \] 把这N个点的约束罗列在一起，我们就可以得到如下矩阵： \[ M\mathbf{x} = \mathbf{0} \] 其中$\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$，如下形式: