文章目录
- 🚀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