网站推广策略成功的案例,网站开发html书籍下载,如何免费自己创建网站,织梦首饰网站模板文章目录 #x1f680;PnP1️⃣ 求解不考虑尺度的解2️⃣ 恢复解的尺度3️⃣ 另一种解法 #x1f680;PnP
PnP(Perspective-n-Point)是求解3D到2D点相机外参的算法。PnP算法有DLT直接线性变换、P3P三对点估计位姿、EPnP(Efficient PnP)、BA(Bundle Adjustment)光速法平差。这… 文章目录 PnP1️⃣ 求解不考虑尺度的解2️⃣ 恢复解的尺度3️⃣ 另一种解法 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}RT\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_x0u_0\\0F_yv_0\\001 \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 Fxf11u0f31Fyf21v0f31f31Fxf12u0f32Fyf22v0f32f32Fxf13u0f33Fyf23v0f33f33Fxf14u0f34Fyf24v0f34f34 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_CuF_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_CvF_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_Cf_{31}X_Wf_{32}Y_Wf_{33}Z_Wf_{34} \end{cases} ⎩ ⎨ ⎧ZCuFxXWf11u0XWf31FxYWf12u0YWf32FxZWf13u0ZWf33Fxf14u0f34ZCvFyXWf21v0XWf31FyYWf22v0YWf32FyZWf23v0ZWf33Fyf24v0f34ZCf31XWf32YWf33ZWf34
把最后一个方程带入前两个有 { 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} {FxXWf11FxYWf12FxZWf13Fxf14(u0−u)XWf31(u0−u)YWf32(u0−u)ZWf33(u0−u)f340FyXWf21FyYWf22FyZWf23Fyf24(v0−v)XWf31(v0−v)YWf32(v0−v)ZWf33(v0−v)f340
也就是说每一组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_1F_xY_1F_xZ_1F_x0000(u_0-u)X_1(u_0-u)Y_1(u_0-u)Z_1u_0-u\\ 0000F_yX_1F_yY_1F_yZ_1F_y(u_0-u)X_1(v_0-v)Y_1(v_0-v)Z_1v_0-v\\ \dots\dots\dots\dots\dots\dots\dots\dots\dots\dots\dots\dots\\ F_xX_nF_xY_nF_xZ_nF_x0000(u_0-u)X_n(u_0-u)Y_n(u_0-u)Z_nu_0-u\\ 0000F_yX_nF_yY_nF_yZ_nF_y(u_0-u)X_n(v_0-v)Y_n(v_0-v)Z_nv_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×10
若有6组点对则可以得到唯一解。
但常常匹配点大于6组此时构造如下优化目标和约束条件(等于是强行规定一个尺度后续再把尺度补偿回来) { min ∥ A F ∥ 2 s . t . ∥ F ∥ 2 1 \begin{cases} \min\parallel AF\parallel_2\\ s.t.\;\parallel F\parallel_21 \end{cases} {min∥AF∥2s.t.∥F∥21
此时对 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 YV^TF YVTF此时由于酉矩阵的范数保持性 ∥ Y ∥ 2 1 \parallel Y\parallel_21 ∥Y∥21从而有 min ∥ Σ Y ∥ 2 \min\parallel\Sigma Y\parallel_2 min∥ΣY∥2
由于 Σ \Sigma Σ的奇异值从大到小排列所以解为 Y [ 0 0 … 1 ] T Y\begin{bmatrix}00\dots1\end{bmatrix}^T Y[00…1]T
由 Y V T F YV^TF YVTF且 V V V为实数矩阵有 F ( V T ) − 1 Y ( V T ) ∗ Y V Y V ( : e n d ) F(V^T)^{-1}Y(V^T)^{*}YVY V(:end) F(VT)−1Y(VT)∗YVYV(: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^TSVD(\hat R) U^Σ^V^TSVD(R^)
⭐这里严格数学推导比较复杂这里简单理解为真正的 ∥ R ∥ 1 \parallel R\parallel1 ∥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_C0\Rightarrow\beta(\hat f_{31}X_W\hat f_{32}Y_W\hat f_{33}Z_W\hat f_{34})0 ZC0⇒β(f^31XWf^32YWf^33ZWf^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^31XWf^32YWf^33ZWf^34)0 3️⃣ 另一种解法
⭐上述过程已经可以把理论上的外参求解出来了。 这里提供另一种在实际工程中计算精度会更高的重投影迭代优化求解的思路以飨读者。 输入 空间中3D点的坐标、图像中2D点的坐标 输出 相机外参相机内参(我们认为相机内参也是随时间稍微变化的) 求解迭代初值
我们令内外参的乘积为 M M M M K 3 × 3 [ R T ] 3 × 4 MK_{3\times3}\begin{bmatrix}RT\end{bmatrix}_{3\times4} MK3×3[RT]3×4
与上述求解不考虑尺度的 F F F类似我们可以将 M 3 × 4 M_{3\times4} M3×4的整体数值求解出来(不考虑尺度)。
我们进一步将 M M M写成如下形式 M [ K 3 × 3 R 3 × 3 K 3 × 3 T 3 × 1 ] M\begin{bmatrix} K_{3\times3}R_{3\times3}K_{3\times3}T_{3\times1} \end{bmatrix} M[K3×3R3×3K3×3T3×1] ① {\color{#E16B8C}{①}} ①首先对 K 3 × 3 R 3 × 3 K_{3\times3}R_{3\times3} K3×3R3×3进行QR分解得到一个正交阵 q q q(认定为旋转矩阵 R R R)和上三角矩阵 r r r(认定为内参 K K K) q r Q R ( K 3 × 3 R 3 × 3 ) R K qrQR(K_{3\times3}R_{3\times3})RK qrQR(K3×3R3×3)RK ② {\color{#E16B8C}{②}} ②接着将 K K K代入 K 3 × 3 T 3 × 1 K_{3\times3}T_{3\times1} K3×3T3×1求解出位移向量 T T T。 优化迭代
添加新的匹配点构造优化目标(重新投影逼近真值)如下 arg min K , R , T ∑ i 1 2 ∣ ∣ 1 Z C M X i − u i ∣ ∣ 2 \displaystyle \argmin_{K,R,T}\sum_i\frac{1}{2}||\frac{1}{Z_C}MX_{i}-u_i||^2 K,R,Targmini∑21∣∣ZC1MXi−ui∣∣2 ① {\color{#E16B8C}{①}} ①利用负梯度迭代法(对构成 M M M的 K , R , T K,R,T K,R,T求梯度)求解即可。 ② {\color{#E16B8C}{②}} ②其中迭代初值指定为由无尺度 M M M求出的 K , R , T K,R,T K,R,T。 ③ {\color{#E16B8C}{③}} ③ Z C Z_C ZC也是变化的可以由每步迭代的 Z C M 31 X W M 32 Y W M 33 Z W M 34 Z_CM_{31}X_WM_{32}Y_WM_{33}Z_WM_{34} ZCM31XWM32YWM33ZWM34计算得出。 另外整体上也可以把整个 M M M作为变量求梯度优化最后再利用QR分解等技巧求出 K , R , T K,R,T K,R,T究竟选择哪种主要是看数据的表示方式和计算开销。
