摄像机标定---1
1.2 摄像机标定问题描述
考虑某一张标定物图片，检测得到了若干个角点的像素坐标 { ( u 1 , v 1 ) , ⋯ , ( u m , v m ) } \{(u_1, v_1), \cdots,(u_m, v_m)\} {(u1,v1),⋯,(um,vm)}，分别对应的物点的世界坐标系下的坐标 { ( X W 1 , Y W 1 , 0 ) , ⋯ , ( X W m , Y W m , 0 ) } \{(X_{W_1}, Y_{W_1}, 0), \cdots,(X_{W_m}, Y_{W_m}, 0)\} {(XW1,YW1,0),⋯,(XWm,YWm,0)}（假设标定物在世界坐标系下的 Z W = 0 Z_W = 0 ZW=0的平面上）。
待求解的参数有
-
内参： f u , f v , u 0 , v 0 f_u, f_v, u_0, v_0 fu,fv,u0,v0， f u , f v f_u, f_v fu,fv的初始值待计算， ( u 0 , v 0 ) (u_0, v_0) (u0,v0)的初始值为图像的中心 ( w i d t h − 1 2 , h e i g h t − 1 2 ) \left( \frac{width - 1}{2}, \frac{height - 1}{2} \right) (2width−1,2height−1)
-
畸变参数： k 1 , k 2 , k 3 , k 4 , k 5 , k 6 , p 1 , p 2 k_1,k_2,k_3,k_4,k_5,k_6,p_1,p_2 k1,k2,k3,k4,k5,k6,p1,p2，默认全部初始化为0
-
外参： R 3 × 3 , t 3 × 1 \mathbf{R}_{3 \times 3},\mathbf{t}_{3 \times 1} R3×3,t3×1 （每一张标定物图片都有不同的一对旋转矩阵和平移向量）
-
R 3 × 3 \mathbf{R}_{3 \times 3} R3×3是旋转矩阵，可以表示为旋转向量 v 3 × 1 = [ v x , v y , v z ] T \mathbf{v}_{3 \times 1}=[v_x,v_y,v_z]^T v3×1=[vx,vy,vz]T
-
两者的转换
-
θ = ∣ ∣ v ∣ ∣ , in deg \theta = ||\mathbf{v}||, \ \text{in deg} θ=∣∣v∣∣, in deg
r = v θ = [ r x r y r z ] \mathbf{r} = \frac{\mathbf{v}}{\theta} = \begin{bmatrix} r_x\\ r_y\\ r_z \end{bmatrix} r=θv=⎣⎡rxryrz⎦⎤
R = cos ( θ ) I + ( 1 − cos ( θ ) ) r r T + sin ( θ ) ⋅ [ 0 − r z r y r z 0 − r x − r y r x 0 ] \mathbf{R} = \cos(\theta) \mathbf{I} + \left(1 - \cos(\theta) \right) \mathbf{r} \mathbf{r}^T + \sin(\theta) \cdot \begin{bmatrix} 0 &-r_z &r_y\\ r_z &0 &-r_x\\ -r_y &r_x &0 \end{bmatrix} R=cos(θ)I+(1−cos(θ))rrT+sin(θ)⋅⎣⎡0rz−ry−rz0rxry−rx0⎦⎤
sin ( θ ) ⋅ [ 0 − r z r y r z 0 − r x − r y r x 0 ] = R − R T 2 \sin(\theta) \cdot \begin{bmatrix} 0 &-r_z &r_y\\ r_z &0 &-r_x\\ -r_y &r_x &0 \end{bmatrix} = \frac{\mathbf{R} - \mathbf{R}^T}{2} sin(θ)⋅⎣⎡0rz−ry−rz0rxry−rx0⎦⎤=2R−RT
1.2.1 步骤一 求解内参初始值
这一步是为了求解 f u , f v f_u, f_v fu,fv的初始值，这里使用的是无畸变模型：
Z c ⋅ [ u v 1 ] = K ⋅ [ R , t ] ⋅ [ X W Y W Z W 1 ] Z_c \cdot \begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \mathbf{K} \cdot \begin{bmatrix} \mathbf{R},\mathbf{t} \end{bmatrix} \cdot \begin{bmatrix} X_W\\ Y_W\\ Z_W\\ 1 \end{bmatrix} Zc⋅⎣⎡uv1⎦⎤=K⋅[R,t]⋅⎣⎢⎢⎡XWYWZW1⎦⎥⎥⎤
R = d e f [ r 1 , r 2 , r 3 ] \mathbf{R} \stackrel{\mathrm{def}}{=} [\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3] R=def[r1,r2,r3]和 Z W = 0 Z_W = 0 ZW=0（标定物在世界坐标系的 Z W = 0 Z_W = 0 ZW=0的平面），代入上式：
Z c ⋅ [ u v 1 ] = K ⋅ [ r 1 , r 2 , t ] ⏟ H 3 × 3 ⋅ [ X W Y W 1 ] Z_c \cdot \begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \underbrace{\mathbf{K} \cdot \begin{bmatrix} \mathbf{r}_1, \mathbf{r}_2,\mathbf{t} \end{bmatrix}}_{\displaystyle \mathbf{H}_{3 \times 3}} \cdot \begin{bmatrix} X_W\\ Y_W\\ 1 \end{bmatrix} Zc⋅⎣⎡uv1⎦⎤=H3×3 K⋅[r1,r2,t]⋅⎣⎡XWYW1⎦⎤
H 3 × 3 \mathbf{H}_{3 \times 3} H3×3是单应性矩阵，有8个自由度：
H = d e f [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] \mathbf{H} \stackrel{\mathrm{def}}{=} \begin{bmatrix} h_{11} &h_{12} &h_{13}\\ h_{21} &h_{22} &h_{23}\\ h_{31} &h_{32} &h_{33} \end{bmatrix} H=def⎣⎡h11h21h31h12h22h32h13h23h33⎦⎤
对 H \mathbf{H} H,常用的两种约束方式（二选其一）有：
- h 33 = 1 h_{33} = 1 h33=1
- ∑ i = 1 3 ∑ j = 1 3 h i j 2 = 1 \displaystyle \sum^{3}_{i=1} \sum^{3}_{j=1} h^2_{ij} = 1 i=1∑3j=1∑3hij2=1
1.2.1.1 求解单应性矩阵
物点/像点的个数 m ≥ 4 m \ge 4 m≥4:
- 对物点/像点坐标规范化
X W ‾ = 1 m ∑ i = 1 m X W i Y W ‾ = 1 m ∑ i = 1 m Y W i σ X W = 1 m ∑ i = 1 m ∣ X W i − X W ‾ ∣ σ Y W = 1 m ∑ i = 1 m ∣ Y W i − Y W ‾ ∣ A i = d e f X W i − X W ‾ σ X W B i = d e f Y W i − Y W ‾ σ Y W ⇒ [ X W i Y W i 1 ] = [ σ X W 0 X W ‾ 0 σ Y W Y W ‾ 0 0 1 ] ⏟ Ω ⋅ [ A i B i 1 ] u ‾ = 1 m ∑ i = 1 m u i v ‾ = 1 m ∑ i = 1 m v i σ u = 1 m ∑ i = 1 m ∣ u i − u ‾ ∣ σ v = 1 m ∑ i = 1 m ∣ v i − v ‾ ∣ a i = d e f u i − u ‾ σ u b i = d e f v i − v ‾ σ v ⇒ [ u i v i 1 ] = [ σ u 0 u ‾ 0 σ v v ‾ 0 0 1 ] ⏟ Φ ⋅ [ a i b i 1 ] Z c ⋅ Φ ⋅ [ a i b i 1 ] = H ⋅ Ω ⋅ [ A i B i 1 ] ⇒ Z c ⋅ [ a i b i 1 ] = Φ − 1 ⋅ H ⋅ Ω ⏟ C ⋅ [ A i B i 1 ] \displaystyle \overline{X_W} = \frac{1}{m} \sum^{m}_{i=1}X_{W_i}\\ \overline{Y_W} = \frac{1}{m} \sum^{m}_{i=1}Y_{W_i}\\ \sigma_{X_W} = \frac{1}{m} \sum^{m}_{i=1}|X_{W_i}-\overline{X_W}|\\ \sigma_{Y_W} = \frac{1}{m} \sum^{m}_{i=1}|Y_{W_i}-\overline{Y_W}|\\ A_i \stackrel{\mathrm{def}}{=} \frac{X_{W_i}-\overline{X_W}}{\sigma_{X_W}}\\ B_i \stackrel{\mathrm{def}}{=} \frac{Y_{W_i}-\overline{Y_W}}{\sigma_{Y_W}}\\ \Rightarrow \begin{bmatrix} X_{W_i}\\ Y_{W_i}\\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} \sigma_{X_W} &0 &\overline{X_W}\\ 0 &\sigma_{Y_W} &\overline{Y_W}\\ 0 &0 &1 \end{bmatrix}}_{\displaystyle \mathbf{\Omega}} \cdot \begin{bmatrix} A_i\\ B_i\\ 1 \end{bmatrix} \\ \displaystyle \overline{u} = \frac{1}{m} \sum^{m}_{i=1}u_{i}\\ \overline{v} = \frac{1}{m} \sum^{m}_{i=1}v_{i}\\ \sigma_{u} = \frac{1}{m} \sum^{m}_{i=1}|u_{i}-\overline{u}|\\ \sigma_{v} = \frac{1}{m} \sum^{m}_{i=1}|v_{i}-\overline{v}|\\ a_i \stackrel{\mathrm{def}}{=} \frac{u_{i}-\overline{u}}{\sigma_{u}}\\ b_i \stackrel{\mathrm{def}}{=} \frac{v_{i}-\overline{v}}{\sigma_{v}}\\ \Rightarrow \begin{bmatrix} u_{i}\\ v_{i}\\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} \sigma_{u} &0 &\overline{u}\\ 0 &\sigma_{v} &\overline{v}\\ 0 &0 &1 \end{bmatrix}}_{\displaystyle \mathbf{\Phi}} \cdot \begin{bmatrix} a_i\\ b_i\\ 1 \end{bmatrix}\\ Z_c \cdot \mathbf{\Phi} \cdot \begin{bmatrix} a_i\\ b_i\\ 1 \end{bmatrix} = \mathbf{H} \cdot \mathbf{\Omega} \cdot \begin{bmatrix} A_i\\ B_i\\ 1 \end{bmatrix}\\ \Rightarrow Z_c \cdot \begin{bmatrix} a_i\\ b_i\\ 1 \end{bmatrix} = \underbrace{\mathbf{\Phi}^{-1} \cdot \mathbf{H} \cdot \mathbf{\Omega}}_{\displaystyle \mathbf{C}} \cdot \begin{bmatrix} A_i\\ B_i\\ 1 \end{bmatrix} XW=m1i=1∑mXWiYW=m1i=1∑mYWiσXW=m1i=1∑m∣XWi−XW∣σYW=m1i=1∑m∣YWi−YW∣Ai=defσXWXWi−XWBi=defσYWYWi−YW⇒⎣⎡XWiYWi1⎦⎤=Ω ⎣⎡σXW000σYW0XWYW1⎦⎤⋅⎣⎡AiBi1⎦⎤u=m1i=1∑muiv=m1i=1∑mviσu=m1i=1∑m∣ui−u∣σv=m1i=1∑m∣vi−v∣ai=defσuui−ubi=defσvvi−v⇒⎣⎡uivi1⎦⎤=Φ ⎣⎡σu000σv0uv1⎦⎤⋅⎣⎡aibi1⎦⎤Zc⋅Φ⋅⎣⎡aibi1⎦⎤=H⋅Ω⋅⎣⎡AiBi1⎦⎤⇒Zc⋅⎣⎡aibi1⎦⎤=C Φ−1⋅H⋅Ω⋅⎣⎡AiBi1⎦⎤
- 求 C \mathbf{C} C, C \mathbf{C} C也是一单应性矩阵
[ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ A i B i 1 0 0 0 − A i a i − B i a i − a i 0 0 0 A i B i 1 − A i b i − B i b i − b i ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ] ⏟ D ⋅ [ c 11 c 12 c 13 c 21 c 22 c 23 c 31 c 32 c 33 ] = [ 0 0 0 0 0 0 0 0 0 ] where ∑ i = 1 3 ∑ j = 1 3 c i j 2 = 1 \underbrace{\begin{bmatrix} \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots\\ A_i &B_i &1 &0 &0 &0 &-A_ia_i &-B_ia_i &-a_i\\ 0 &0 &0 &A_i &B_i &1 &-A_ib_i &-B_ib_i &-b_i\\ \vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots\\ \end{bmatrix}}_{\displaystyle \mathbf{D}} \cdot \begin{bmatrix} c_{11}\\ c_{12}\\ c_{13}\\ c_{21}\\ c_{22}\\ c_{23}\\ c_{31}\\ c_{32}\\ c_{33} \end{bmatrix} = \begin{bmatrix} 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0 \end{bmatrix}\\ \text{where} \quad \displaystyle \sum^{3}_{i=1} \sum^{3}_{j=1} c^2_{ij} = 1 D ⎣⎢⎢⎢⎢⎡⋮Ai0⋮⋮Bi0⋮⋮10⋮⋮0Ai⋮⋮0Bi⋮⋮01⋮⋮−Aiai−Aibi⋮⋮−Biai−Bibi⋮⋮−ai−bi⋮⎦⎥⎥⎥⎥⎤⋅⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡c11c12c13c21c22c23c31c32c33⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡000000000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤wherei=1∑3j=1∑3cij2=1
D 2 m × 9 \mathbf{D}_{2m \times 9} D2m×9,使用SVD求解如上的齐次线性方程组的近似解：
对于如下齐次线性方程组的解
A x = 0 where ∑ i = 1 n x i 2 = 1 \mathbf{A} \mathbf{x} = 0\\ \text{where} \quad \displaystyle \sum^{n}_{i=1} x^2_{i} = 1 Ax=0wherei=1∑nxi2=1
可近似等效为如下问题的解
min ∣ ∣ A x ∣ ∣ where ∑ i = 1 n x i 2 = 1 \min||\mathbf{A} \mathbf{x}||\\ \text{where} \quad \displaystyle \sum^{n}_{i=1} x^2_{i} = 1 min∣∣Ax∣∣wherei=1∑nxi2=1
上述问题的SVD解法是
A = U Σ V T ← SVD分解 x = V 的 最 后 一 列 \mathbf{A} = \mathbf{U}\mathbf{\Sigma}\mathbf{V}^T \leftarrow \quad \text{SVD分解}\\ \mathbf{x} = \mathbf{V}的最后一列 A=UΣVT←SVD分解x=V的最后一列
-
于是， H \mathbf{H} H的值就是
H = Φ ⋅ C ⋅ Ω − 1 H = H h 33 \mathbf{H} = \mathbf{\Phi} \cdot \mathbf{C} \cdot \mathbf{\Omega}^{-1} \\ \mathbf{H} = \frac{\mathbf{H}}{h_{33}} H=Φ⋅C⋅Ω−1H=h33H
-
如果物点/像点的个数 m > 4 m > 4 m>4，则基于重投影误差最小化的原则，构造重投影误差目标函数，使用LM算法进一步优化 H \mathbf{H} H的的值（ h 33 ≡ 1 h_{33} \equiv 1 h33≡1）
1.2.1.2 求解 f u , f v f_u, f_v fu,fv的初值
H = λ ⋅ K ⋅ [ r 1 , r 2 , t ] \mathbf{H} = \lambda \cdot \mathbf{K} \cdot \begin{bmatrix} \mathbf{r}_1, \mathbf{r}_2,\mathbf{t} \end{bmatrix} H=λ⋅K⋅[r1,r2,t]
H ^ = d e f [ 1 0 − u 0 0 1 − v 0 0 0 1 ] ⋅ H = λ ⋅ [ f u 0 0 0 f v 0 0 0 1 ] ⏟ G ⋅ [ r 1 , r 2 , t ] H ^ = [ h ^ 1 , h ^ 2 , h ^ 3 ] ⇒ r 1 = 1 λ G − 1 h ^ 1 ⇒ r 2 = 1 λ G − 1 h ^ 2 \mathbf{\hat{H}} \stackrel{\mathrm{def}}{=} \begin{bmatrix} 1 &0 &-u_0 \\ 0 &1 &-v_0 \\ 0 &0 &1 \\ \end{bmatrix} \cdot \mathbf{H} = \lambda \cdot \underbrace{ \begin{bmatrix} f_u &0 &0 \\ 0 &f_v &0 \\ 0 &0 &1 \\ \end{bmatrix} }_{\displaystyle \mathbf{G}} \cdot \begin{bmatrix} \mathbf{r}_1, \mathbf{r}_2,\mathbf{t} \end{bmatrix} \\ \mathbf{\hat{H}} = [\hat{\mathbf{h}}_1,\hat{\mathbf{h}}_2,\hat{\mathbf{h}}_3] \\ \Rightarrow \mathbf{r}_1 = \frac{1}{\lambda} \mathbf{G}^{-1} \hat{\mathbf{h}}_1 \\ \Rightarrow \mathbf{r}_2 = \frac{1}{\lambda} \mathbf{G}^{-1} \hat{\mathbf{h}}_2 H^=def⎣⎡100010−u0−v01⎦⎤⋅H=λ⋅G ⎣⎡fu000fv0001⎦⎤⋅[r1,r2,t]H^=[h^1,h^2,h^3]⇒r1=λ1G−1h^1⇒r2=λ1G−1h^2
由
r 1 T r 2 = 0 ∣ ∣ r 1 ∣ ∣ = ∣ ∣ r 2 ∣ ∣ = 1 \mathbf{r}^T_1 \mathbf{r}_2 = 0 \\ ||\mathbf{r}_1|| = ||\mathbf{r}_2|| = 1 r1Tr2=0∣∣r1∣∣=∣∣r2∣∣=1
可得
h ^ 1 T ( G − 1 ) T G − 1 h ^ 2 = 0 h ^ 1 T ( G − 1 ) T G − 1 h ^ 1 = h ^ 2 T ( G − 1 ) T G − 1 h ^ 2 \hat{\mathbf{h}}^T_1 (\mathbf{G}^{-1})^T \mathbf{G}^{-1} \hat{\mathbf{h}}_2 = 0 \\ \hat{\mathbf{h}}^T_1 (\mathbf{G}^{-1})^T \mathbf{G}^{-1} \hat{\mathbf{h}}_1 = \hat{\mathbf{h}}^T_2 (\mathbf{G}^{-1})^T \mathbf{G}^{-1} \hat{\mathbf{h}}_2 h^1T(G−1)TG−1h^2=0h^1T(G−1)TG−1h^1=h^2T(G−1)TG−1h^2
整理可得
[ h ^ 11 h ^ 12 h ^ 21 h ^ 22 h ^ 11 2 − h ^ 12 2 h ^ 21 2 − h ^ 22 2 ] ⏟ Q ⋅ [ 1 f u 2 1 f v 2 ] = [ − h ^ 31 h ^ 32 − ( h ^ 31 2 − h ^ 32 2 ) ] \underbrace{ \begin{bmatrix} \hat{h}_{11} \hat{h}_{12} &\hat{h}_{21} \hat{h}_{22} \\ \hat{h}^2_{11} - \hat{h}^2_{12} &\hat{h}^2_{21} - \hat{h}^2_{22} \\ \end{bmatrix} }_{\displaystyle \mathbf{Q}} \cdot \begin{bmatrix} \frac{1}{f^2_u} \\ \frac{1}{f^2_v} \\ \end{bmatrix} = \begin{bmatrix} -\hat{h}_{31} \hat{h}_{32} \\ -(\hat{h}^2_{31} - \hat{h}^2_{32}) \\ \end{bmatrix} Q [h^11h^12h^112−h^122h^21h^22h^212−h^222]⋅[fu21fv21]=[−h^31h^32−(h^312−h^322)]
其中，每多一个不同视图的标定物图片，就可为 Q \mathbf{Q} Q增加两行。对上式，在代码实现时有一些规范化的数值操作，如下
n = [ 1 h ^ 11 2 + h ^ 21 2 + h ^ 31 2 1 h ^ 12 2 + h ^ 22 2 + h ^ 32 2 1 ( h ^ 11 + h ^ 12 2 ) 2 + ( h ^ 21 + h ^ 22 2 ) 2 + ( h ^ 31 + h ^ 32 2 ) 2 1 ( h ^ 11 − h ^ 12 2 ) 2 + ( h ^ 21 − h ^ 22 2 ) 2 + ( h ^ 31 − h ^ 32 2 ) 2 ] w = n 1 ⋅ [ h ^ 11 h ^ 21 h ^ 31 ] v = n 2 ⋅ [ h ^ 12 h ^ 22 h ^ 32 ] d = n 3 ⋅ [ h ^ 11 + h ^ 12 2 h ^ 21 + h ^ 22 2 h ^ 31 + h ^ 32 2 ] p = n 4 ⋅ [ h ^ 11 − h ^ 12 2 h ^ 21 − h ^ 22 2 h ^ 31 − h ^ 32 2 ] ⇒ [ w 1 v 1 w 2 v 2 d 1 p 1 d 2 p 2 ] ⏟ Q ⋅ [ 1 f u 2 1 f v 2 ] = [ − w 3 v 3 − d 3 p 3 ] \mathbf{n} = \begin{bmatrix} \frac{1}{\displaystyle \sqrt{\hat{h}^2_{11} + \hat{h}^2_{21} + \hat{h}^2_{31} }} \\ \frac{1}{\displaystyle \sqrt{\hat{h}^2_{12} + \hat{h}^2_{22} + \hat{h}^2_{32} }} \\ \frac{1}{\displaystyle \sqrt{(\frac{\hat{h}_{11} + \hat{h}_{12}}{2})^2 + (\frac{\hat{h}_{21} + \hat{h}_{22}}{2})^2 + (\frac{\hat{h}_{31} + \hat{h}_{32}}{2})^2 }} \\ \frac{1}{\displaystyle \sqrt{(\frac{\hat{h}_{11} - \hat{h}_{12}}{2})^2 + (\frac{\hat{h}_{21} - \hat{h}_{22}}{2})^2 + (\frac{\hat{h}_{31} - \hat{h}_{32}}{2})^2 }} \\ \end{bmatrix} \\ \mathbf{w} = n_1 \cdot \begin{bmatrix} \hat{h}_{11} \\ \hat{h}_{21} \\ \hat{h}_{31} \\ \end{bmatrix} \\ \mathbf{v} = n_2 \cdot \begin{bmatrix} \hat{h}_{12} \\ \hat{h}_{22} \\ \hat{h}_{32} \\ \end{bmatrix} \\ \mathbf{d} = n_3 \cdot \begin{bmatrix} \displaystyle \frac{\hat{h}_{11} + \hat{h}_{12}}{2} \\ \displaystyle \frac{\hat{h}_{21} + \hat{h}_{22}}{2} \\ \displaystyle \frac{\hat{h}_{31} + \hat{h}_{32}}{2} \\ \end{bmatrix} \\ \mathbf{p} = n_4 \cdot \begin{bmatrix} \displaystyle \frac{\hat{h}_{11} - \hat{h}_{12}}{2} \\ \displaystyle \frac{\hat{h}_{21} - \hat{h}_{22}}{2} \\ \displaystyle \frac{\hat{h}_{31} - \hat{h}_{32}}{2} \\ \end{bmatrix} \\ \Rightarrow \underbrace{ \begin{bmatrix} w_1 v_1 &w_2 v_2 \\ d_1 p_1 &d_2 p_2 \\ \end{bmatrix} }_{\displaystyle \mathbf{Q}} \cdot \begin{bmatrix} \frac{1}{f^2_u} \\ \frac{1}{f^2_v} \\ \end{bmatrix}= \begin{bmatrix} -w_3 v_3 \\ -d_3 p_3 \\ \end{bmatrix} n=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡h^112+h^212+h^3121h^122+h^222+h^3221(2h^11+h^12)2+(2h^21+h^22)2+(2h^31+h^32)21(2h^11−h^12)2+(2h^21−h^22)2+(2h^31−h^32)21⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤w=n1⋅⎣⎡h^11h^21h^31⎦⎤v=n2⋅⎣⎡h^12h^22h^32⎦⎤d=n3⋅⎣⎢⎢⎢⎢⎢⎡2h^11+h^122h^21+h^222h^31+h^32⎦⎥⎥⎥⎥⎥⎤p=n4⋅⎣⎢⎢⎢⎢⎢⎡2h^11−h^122h^21−h^222h^31−h^32⎦⎥⎥⎥⎥⎥⎤⇒Q [w1v1d1p1w2v2d2p2]⋅[fu21fv21]=[−w3v3−d3p3]
使用SVD求解上述非齐次线性方程组，得到 f u , f v f_u, f_v fu,fv的初值。