文章目录
- 🚀PnP
-
- [1️⃣ 求解不考虑尺度的解](#1️⃣ 求解不考虑尺度的解)
- [2️⃣ 恢复解的尺度](#2️⃣ 恢复解的尺度)
🚀PnP
PnP(Perspective-n-Point)是求解3D到2D点相机外参的算法。PnP算法有DLT直接线性变换 、P3P三对点估计位姿、EPnP(Efficient PnP)、BA(Bundle Adjustment)光速法平差。这里主要讲解DLT。
推理过程涉及一些知识点，可以参考以下博文：
【对比学习】正交阵/酉矩阵，对称矩阵/Hermite矩阵，正交相似对角化/奇异值分解的内在联系
【相机标定】相机标定中的坐标变换，内外参求解，畸变校正，标定代码
输入：
空间中3D点的坐标、图像中2D点的坐标，内参矩阵
输出：相机外参
1️⃣ 求解不考虑尺度的解
写出矩阵变换方程：
Z C [ u v 1 ] = K 3 × 3 [ R T ] 3 × 4 [ X W Y W Z W 1 ] Z_C\begin{bmatrix}u\\v\\1\end{bmatrix}=K_{3\times3}\begin{bmatrix}R&T\end{bmatrix}_{3\times4}\begin{bmatrix}X_W\\Y_W\\Z_W\\1\end{bmatrix} ZC uv1 =K3×3[RT]3×4 XWYWZW1
将内外参数展开：
Z C [ u v 1 ] = [ F x 0 u 0 0 F y v 0 0 0 1 ] [ f 11 f 12 f 13 f 14 f 21 f 22 f 23 f 24 f 31 f 32 f 33 f 34 ] [ X W Y W Z W 1 ] = [ 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 ] [ X W Y W Z W 1 ] Z_C\begin{bmatrix}u\\v\\1\end{bmatrix}= \begin{bmatrix} F_x&0&u_0\\0&F_y&v_0\\0&0&1 \end{bmatrix} \begin{bmatrix}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{bmatrix}\begin{bmatrix}X_W\\Y_W\\Z_W\\1\end{bmatrix}\\= \begin{bmatrix} F_xf_{11}+u_0f_{31}&F_xf_{12}+u_0f_{32}&F_xf_{13}+u_0f_{33}&F_xf_{14}+u_0f_{34}\\ F_yf_{21}+v_0f_{31}&F_yf_{22}+v_0f_{32}&F_yf_{23}+v_0f_{33}&F_yf_{24}+v_0f_{34}\\ f_{31}&f_{32}&f_{33}&f_{34} \end{bmatrix}\begin{bmatrix}X_W\\Y_W\\Z_W\\1\end{bmatrix} ZC uv1 = Fx000Fy0u0v01 f11f21f31f12f22f32f13f23f33f14f24f34 XWYWZW1 = Fxf11+u0f31Fyf21+v0f31f31Fxf12+u0f32Fyf22+v0f32f32Fxf13+u0f33Fyf23+v0f33f33Fxf14+u0f34Fyf24+v0f34f34 XWYWZW1
进一步展开，写成方程组的形式：
{ 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 \begin{cases} Z_Cu=F_xX_Wf_{11}+u_0X_Wf_{31}+F_xY_Wf_{12}+u_0Y_Wf_{32}+F_xZ_Wf_{13}+u_0Z_Wf_{33}+F_xf_{14}+u_0f_{34}\\ Z_Cv=F_yX_Wf_{21}+v_0X_Wf_{31}+F_yY_Wf_{22}+v_0Y_Wf_{32}+F_yZ_Wf_{23}+v_0Z_Wf_{33}+F_yf_{24}+v_0f_{34}\\ Z_C=f_{31}X_W+f_{32}Y_W+f_{33}Z_W+f_{34} \end{cases} ⎩ ⎨ ⎧ZCu=FxXWf11+u0XWf31+FxYWf12+u0YWf32+FxZWf13+u0ZWf33+Fxf14+u0f34ZCv=FyXWf21+v0XWf31+FyYWf22+v0YWf32+FyZWf23+v0ZWf33+Fyf24+v0f34ZC=f31XW+f32YW+f33ZW+f34
把最后一个方程带入前两个有：
{ 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 \begin{cases} F_xX_Wf_{11}+F_xY_Wf_{12}+F_xZ_Wf_{13}+F_xf_{14}+(u_0-u)X_Wf_{31}+(u_0-u)Y_Wf_{32}+(u_0-u)Z_Wf_{33}+(u_0-u)f_{34}=0\\ F_yX_Wf_{21}+F_yY_Wf_{22}+F_yZ_Wf_{23}+F_yf_{24}+(v_0-v)X_Wf_{31}+(v_0-v)Y_Wf_{32}+(v_0-v)Z_Wf_{33}+(v_0-v)f_{34}=0 \end{cases} {FxXWf11+FxYWf12+FxZWf13+Fxf14+(u0−u)XWf31+(u0−u)YWf32+(u0−u)ZWf33+(u0−u)f34=0FyXWf21+FyYWf22+FyZWf23+Fyf24+(v0−v)XWf31+(v0−v)YWf32+(v0−v)ZWf33+(v0−v)f34=0
也就是说每一组3D-2D的匹配点就能对应两个方程，其中共有12个未知数(或者说11个未知数+1个尺度参数)，则至少需要6组匹配点来解出所有未知数。
设有n组匹配点，则：
[ 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 ... ... ... ... ... ... ... ... ... ... ... ... 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 ] [ f 11 f 12 f 13 f 14 f 21 f 22 f 23 f 24 f 31 f 32 f 33 f 34 ] = 0 \begin{bmatrix} F_xX_1&F_xY_1&F_xZ_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_yX_1&F_yY_1&F_yZ_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_xX_n&F_xY_n&F_xZ_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_yX_n&F_yY_n&F_yZ_n&F_y&(u_0-u)X_n&(v_0-v)Y_n&(v_0-v)Z_n&v_0-v\\ \end{bmatrix} \begin{bmatrix} 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{bmatrix}=\mathbf{0} FxX10...FxXn0FxY10...FxYn0FxZ10...FxZn0Fx0...Fx00FyX1...0FyXn0FyY1...0FyYn0FyZ1...0FyZn0Fy...0Fy(u0−u)X1(u0−u)X1...(u0−u)Xn(u0−u)Xn(u0−u)Y1(v0−v)Y1...(u0−u)Yn(v0−v)Yn(u0−u)Z1(v0−v)Z1...(u0−u)Zn(v0−v)Znu0−uv0−v...u0−uv0−v f11f12f13f14f21f22f23f24f31f32f33f34 =0
将上式写作：
A 2 n × 12 F 12 × 1 = 0 A_{2n\times 12}F_{12\times1}=\mathbf{0} A2n×12F12×1=0
若有6组点对，则可以得到唯一解。
🌔但常常匹配点大于6组，此时构造如下优化目标和约束条件(等于是强行规定一个尺度，后续再把尺度补偿回来)：
{ min ∥ A F ∥ 2 s . t . ∥ F ∥ 2 = 1 \begin{cases} \min\parallel AF\parallel_2\\ s.t.\;\parallel F\parallel_2=1 \end{cases} {min∥AF∥2s.t.∥F∥2=1
此时，对 A A A进行SVD分解有：
min ∥ ( U Σ V T ) F ∥ 2 \min\parallel(U\Sigma V^T)F\parallel_2 min∥(UΣVT)F∥2
由酉矩阵的范数保持性有：
min ∥ Σ V T F ∥ 2 \min\parallel\Sigma V^TF\parallel_2 min∥ΣVTF∥2
令 Y = V T F Y=V^TF Y=VTF，此时由于酉矩阵的范数保持性 ， ∥ Y ∥ 2 = 1 \parallel Y\parallel_2=1 ∥Y∥2=1，从而有：
min ∥ Σ Y ∥ 2 \min\parallel\Sigma Y\parallel_2 min∥ΣY∥2
由于 Σ \Sigma Σ的奇异值从大到小排列，所以解为：
Y = [ 0 0 ... 1 ] T Y=\begin{bmatrix}0&0&\dots&1\end{bmatrix}^T Y=[00...1]T
由 Y = V T F Y=V^TF Y=VTF，且 V V V为实数矩阵，有：
F = ( V T ) − 1 Y = ( V T ) ∗ Y = V Y = V ( : e n d ) F=(V^T)^{-1}Y=(V^T)^{*}Y=VY= V(:end) F=(VT)−1Y=(VT)∗Y=VY=V(:end)
即解 F F F为 V V V的最后一列，这里不妨令这个不含尺度的解为 F ^ \hat F F^，而实际解为：
F = β F ^ F=\beta\hat F F=βF^
其中 β \beta β是接下来要求解的尺度因子。
2️⃣ 恢复解的尺度
我们利用旋转变换的标准正交性 来恢复尺度，由 F ^ \hat F F^有：
R ^ = [ f ^ 11 f ^ 12 f ^ 13 f ^ 21 f ^ 22 f ^ 23 f ^ 31 f ^ 32 f ^ 33 ] \hat R=\begin{bmatrix}\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{bmatrix} R^= f^11f^21f^31f^12f^22f^32f^13f^23f^33
对其进行SVD分解有：
U ^ Σ ^ V ^ T = S V D ( R ^ ) \hat U\hat \Sigma \hat V^T=SVD(\hat R) U^Σ^V^T=SVD(R^)
⭐这里，严格数学推导比较复杂，这里简单理解为真正的 ∥ R ∥ = 1 \parallel R\parallel=1 ∥R∥=1，且为正交阵，而 ∥ R ^ ∥ ≠ 1 \parallel\hat R\parallel\neq1 ∥R^∥=1，把缩放变换 Σ ^ \hat \Sigma Σ^拿掉使之恢复为两酉矩阵的乘积，使得其模为1，把这个结果作为最优解。
则带有尺度的最优解为：
R = ± U ^ V ^ T R=\pm\hat U\hat V^T R=±U^V^T
而尺度因子可以用 Σ \Sigma Σ各个奇异值的平均值来估计：
β = ± 1 t r ( Σ ^ ) / 3 \beta=\pm\frac{1}{tr(\hat \Sigma)/3} β=±tr(Σ^)/31
考虑到3D点在相机的前方：
Z C > 0 ⇒ β ( f ^ 31 X W + f ^ 32 Y W + f ^ 33 Z W + f ^ 34 ) > 0 Z_C>0\Rightarrow\beta(\hat f_{31}X_W+\hat f_{32}Y_W+\hat f_{33}Z_W+\hat f_{34})>0 ZC>0⇒β(f^31XW+f^32YW+f^33ZW+f^34)>0
由此可以确定 R R R和 β \beta β的符号，进而可以求得恢复尺度的平移向量：
T = β [ f ^ 14 f ^ 24 f ^ 34 ] T T=\beta\begin{bmatrix}\hat f_{14}&\hat f_{24}&\hat f_{34}\end{bmatrix}^T T=β[f^14f^24f^34]T
😄综上，有：
{ R = ± U ^ V ^ T T = β [ f ^ 14 f ^ 24 f ^ 34 ] T β = ± 1 t r ( Σ ^ ) / 3 β ( f ^ 31 X W + f ^ 32 Y W + f ^ 33 Z W + f ^ 34 ) > 0 \begin{cases} R=\pm \hat U\hat V^T\\ T=\beta\begin{bmatrix}\hat f_{14}&\hat f_{24}&\hat f_{34}\end{bmatrix}^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{cases} ⎩ ⎨ ⎧R=±U^V^TT=β[f^14f^24f^34]Tβ=±tr(Σ^)/31β(f^31XW+f^32YW+f^33ZW+f^34)>0