📝笔记：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问题。

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

由于余弦定理可知：

$\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$ ，如下形式:

P3P