🚀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\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}$

---

3️⃣ 另一种解法

⭐上述过程已经可以把理论上的外参求解出来了。
🐦这里提供另一种在实际工程中计算精度会更高的重投影迭代优化求解的思路，以飨读者。

输入：
空间中3D点的坐标、图像中2D点的坐标
输出：
相机外参，相机内参(我们认为相机内参也是随时间稍微变化的)

求解迭代初值：

我们令内外参的乘积为$M$：

$M=K_{3\times 3}{\left[\begin{array}{cc}R& T\end{array}\right]}_{3\times 4}$

😃与上述求解不考虑尺度的$F$类似，我们可以将$M_{3\times 4}$的整体数值求解出来(不考虑尺度)。

我们进一步将$M$写成如下形式：

$M=\left[\begin{array}{cc}{K}_{3\times 3}{R}_{3\times 3}& K_{3\times 3}{T}_{3\times 1}\end{array}\right]$

$①$首先，对$K_{3\times 3}{R}_{3\times 3}$进行QR分解，得到一个正交阵$q$(认定为旋转矩阵$R$)和上三角矩阵$r$(认定为内参$K$)：
$qr=QR(K_{3\times 3}{R}_{3\times 3})=RK$
$②$接着，将$K$代入$K_{3\times 3}{T}_{3\times 1}$，求解出位移向量$T$。

优化迭代：

添加新的匹配点，构造优化目标(重新投影逼近真值)如下：

$\underset{K,R,T}{\mathrm{argmin}}\sum _i\frac{1}{2}||\frac{1}{Z_C}M{X}_i-u_i|{|}^2$

$①$利用负梯度迭代法(对构成$M$的$K,R,T$求梯度)求解即可。
$②$其中，迭代初值指定为由无尺度$M$求出的$K,R,T$。
$③$$Z_C$也是变化的，可以由每步迭代的$Z_C=M_{31}{X}_W+M_{32}{Y}_W+M_{33}{Z}_W+M_{34}$计算得出。

😸另外，整体上也可以把整个$M$作为变量求梯度优化，最后再利用QR分解等技巧求出$K,R,T$，究竟选择哪种主要是看数据的表示方式和计算开销。