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

Posted on Edited on In 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\)，如下形式:

参考