🚀PnP

PnP(Perspective-n-Point)是求解3D到2D点相机外参的算法。PnP算法有DLT直接线性变换、P3P三对点估计位姿、EPnP(Efficient PnP)、BA(Bundle Adjustment)光速法平差。这里主要讲解DLT。

推理过程涉及一些知识点，可以参考以下博文：
【对比学习】正交阵/酉矩阵，对称矩阵/Hermite矩阵，正交相似对角化/奇异值分解的内在联系
【相机标定】相机标定中的坐标变换，内外参求解，畸变校正，标定代码

输入：
空间中3D点的坐标、图像中2D点的坐标，内参矩阵
输出：
相机外参

1️⃣ 求解不考虑尺度的解

写出矩阵变换方程：

$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K_{3\times 3}{\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]}_{3\times 4}\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]$

将内外参数展开：

$$\begin{gathered} Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{F}_x& 0& u_0\\ 0& F_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{f}_{11}& f_{12}& f_{13}& f_{14}\\ f_{21}& f_{22}& f_{23}& f_{24}\\ f_{31}& f_{32}& f_{33}& f_{34}\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right] \\ =\left[\begin{array}{cccc}{F}_x{f}_{11}+u_0{f}_{31}& F_x{f}_{12}+u_0{f}_{32}& F_x{f}_{13}+u_0{f}_{33}& F_x{f}_{14}+u_0{f}_{34}\\ F_y{f}_{21}+v_0{f}_{31}& F_y{f}_{22}+v_0{f}_{32}& F_y{f}_{23}+v_0{f}_{33}& F_y{f}_{24}+v_0{f}_{34}\\ f_{31}& f_{32}& f_{33}& f_{34}\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right] \end{gathered}$$

进一步展开，写成方程组的形式：

$\{\begin{array}{l}{Z}_{C}u=F_x{X}_W{f}_{11}+u_0{X}_W{f}_{31}+F_x{Y}_W{f}_{12}+u_0{Y}_W{f}_{32}+F_x{Z}_W{f}_{13}+u_0{Z}_W{f}_{33}+F_x{f}_{14}+u_0{f}_{34}\\ Z_{C}v=F_y{X}_W{f}_{21}+v_0{X}_W{f}_{31}+F_y{Y}_W{f}_{22}+v_0{Y}_W{f}_{32}+F_y{Z}_W{f}_{23}+v_0{Z}_W{f}_{33}+F_y{f}_{24}+v_0{f}_{34}\\ Z_C=f_{31}{X}_W+f_{32}{Y}_W+f_{33}{Z}_W+f_{34}\end{array}$

把最后一个方程带入前两个有：

$\{\begin{array}{l}{F}_x{X}_W{f}_{11}+F_x{Y}_W{f}_{12}+F_x{Z}_W{f}_{13}+F_x{f}_{14}+(u_0-u)X_W{f}_{31}+(u_0-u)Y_W{f}_{32}+(u_0-u)Z_W{f}_{33}+(u_0-u)f_{34}=0\\ F_y{X}_W{f}_{21}+F_y{Y}_W{f}_{22}+F_y{Z}_W{f}_{23}+F_y{f}_{24}+(v_0-v)X_W{f}_{31}+(v_0-v)Y_W{f}_{32}+(v_0-v)Z_W{f}_{33}+(v_0-v)f_{34}=0\end{array}$

也就是说每一组3D-2D的匹配点就能对应两个方程，其中共有12个未知数(或者说11个未知数+1个尺度参数)，则至少需要6组匹配点来解出所有未知数。

设有n组匹配点，则：

$\left[\begin{array}{cccccccccccc}{F}_x{X}_1& F_x{Y}_1& F_x{Z}_1& F_x& 0& 0& 0& 0& (u_0-u)X_1& (u_0-u)Y_1& (u_0-u)Z_1& u_0-u\\ 0& 0& 0& 0& F_y{X}_1& F_y{Y}_1& F_y{Z}_1& F_y& (u_0-u)X_1& (v_0-v)Y_1& (v_0-v)Z_1& v_0-v\\ \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots & \dots \\ F_x{X}_n& F_x{Y}_n& F_x{Z}_n& F_x& 0& 0& 0& 0& (u_0-u)X_n& (u_0-u)Y_n& (u_0-u)Z_n& u_0-u\\ 0& 0& 0& 0& F_y{X}_n& F_y{Y}_n& F_y{Z}_n& F_y& (u_0-u)X_n& (v_0-v)Y_n& (v_0-v)Z_n& v_0-v\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ f_{14}\\ f_{21}\\ f_{22}\\ f_{23}\\ f_{24}\\ f_{31}\\ f_{32}\\ f_{33}\\ f_{34}\end{array}\right]=0$

将上式写作：

$A_{2n\times 12}{F}_{12\times 1}=0$

若有6组点对，则可以得到唯一解。

🌔但常常匹配点大于6组，此时构造如下优化目标和约束条件(等于是强行规定一个尺度，后续再把尺度补偿回来)：

$\{\begin{array}{l}\min \parallel AF{\parallel }_2\\ s.t.\parallel F{\parallel }_2=1\end{array}$

此时，对$A$进行SVD分解有：

$\min \parallel (U\Sigma V^T)F{\parallel }_2$

由酉矩阵的范数保持性有：

$\min \parallel \Sigma V^{T}F{\parallel }_2$

令$Y=V^{T}F$，此时由于酉矩阵的范数保持性，$\parallel Y{\parallel }_2=1$，从而有：

$\min \parallel \Sigma Y{\parallel }_2$

由于$\Sigma$的奇异值从大到小排列，所以解为：

$Y={\left[\begin{array}{cccc}0& 0& \dots & 1\end{array}\right]}^T$

由$Y=V^{T}F$，且$V$为实数矩阵，有：

$F=(V^T{)}^{-1}Y=(V^T{)}^{\ast }Y=VY=V(:end)$

即解 $F$为$V$的最后一列，这里不妨令这个不含尺度的解为$\hat{F}$，而实际解为：

$F=\beta \hat{F}$

其中$\beta$是接下来要求解的尺度因子。

---

2️⃣ 恢复解的尺度

我们利用旋转变换的标准正交性来恢复尺度，由$\hat{F}$有：

$\hat{R}=\left[\begin{array}{ccc}{\hat{f}}_{11}& {\hat{f}}_{12}& {\hat{f}}_{13}\\ {\hat{f}}_{21}& {\hat{f}}_{22}& {\hat{f}}_{23}\\ {\hat{f}}_{31}& {\hat{f}}_{32}& {\hat{f}}_{33}\end{array}\right]$

对其进行SVD分解有：

$\hat{U}\hat{\Sigma }{\hat{V}}^T=SVD(\hat{R})$

⭐这里，严格数学推导比较复杂，这里简单理解为真正的$\parallel R\parallel =1$，且为正交阵，而$\parallel \hat{R}\parallel \ne 1$，把缩放变换$\hat{\Sigma }$拿掉使之恢复为两酉矩阵的乘积，使得其模为1，把这个结果作为最优解。

则带有尺度的最优解为：

$R=\pm \hat{U}{\hat{V}}^T$

而尺度因子可以用$\Sigma$各个奇异值的平均值来估计：

$\beta =\pm \frac{1}{tr(\hat{\Sigma })/3}$

考虑到3D点在相机的前方：

$Z_C>0\Rightarrow \beta ({\hat{f}}_{31}{X}_W+{\hat{f}}_{32}{Y}_W+{\hat{f}}_{33}{Z}_W+{\hat{f}}_{34})>0$

由此可以确定$R$和$\beta$的符号，进而可以求得恢复尺度的平移向量：

$T=\beta {\left[\begin{array}{ccc}{\hat{f}}_{14}& {\hat{f}}_{24}& {\hat{f}}_{34}\end{array}\right]}^T$

😄综上，有：

$\{\begin{array}{l}R=\pm \hat{U}{\hat{V}}^T\\ T=\beta {\left[\begin{array}{ccc}{\hat{f}}_{14}& {\hat{f}}_{24}& {\hat{f}}_{34}\end{array}\right]}^T\\ \beta =\pm \frac{1}{tr(\hat{\Sigma })/3}\\ \beta ({\hat{f}}_{31}{X}_W+{\hat{f}}_{32}{Y}_W+{\hat{f}}_{33}{Z}_W+{\hat{f}}_{34})>0\end{array}$