1. 相机模型
1.1. 针孔模型
1.1.1. 不带畸变的模型
- 从世界坐标系到摄像机坐标系
[ X c Y c Z c ] = [ R , t ] ⋅ [ X W Y W Z W 1 ] \begin{bmatrix} X_c\\ Y_c\\ Z_c \end{bmatrix} = \begin{bmatrix} \mathbf{R}, \mathbf{t} \end{bmatrix} \cdot \begin{bmatrix} X_W\\ Y_W\\ Z_W\\ 1 \end{bmatrix} XcYcZc =[R,t]⋅ XWYWZW1
- 从摄像机坐标系到像素坐标系
[ x 1 y 1 1 ] = d e f [ X c / Z c Y c / Z c 1 ] \begin{bmatrix} x_1\\ y_1\\ 1 \end{bmatrix} \stackrel{\mathrm{def}}{=} \begin{bmatrix} X_c/Z_c\\ Y_c/Z_c\\ 1 \end{bmatrix} x1y11 =def Xc/ZcYc/Zc1
[ u v 1 ] = [ f u 0 u 0 0 f v v 0 0 0 1 ] ⏟ K ⋅ [ x 1 y 1 1 ] \begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} f_u &0 &u_0\\ 0 &f_v &v_0\\ 0 &0 &1 \end{bmatrix}}_{\displaystyle \mathbf{K}} \cdot \begin{bmatrix} x_1\\ y_1\\ 1 \end{bmatrix} uv1 =K fu000fv0u0v01 ⋅ x1y11
总结起来是
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
1.1.2. 带畸变的模型
畸变使得 [ x 1 , y 1 , 1 ] T [x_1, y_1, 1]^T [x1,y1,1]T变成了 [ x 2 , y 2 , 1 ] T [x_2, y_2, 1]^T [x2,y2,1]T
x 2 = x 1 ⋅ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + 2 p 1 x 1 y 1 + p 2 ( r 2 + 2 x 1 2 ) y 2 = y 1 ⋅ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + p 1 ( r 2 + 2 y 1 2 ) + 2 p 2 x 1 y 1 where r 2 = x 1 2 + y 1 2 x_2 = x_1 \cdot \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + 2 p_1 x_1 y_1 + p_2(r^2 + 2 x_1^2) \\ y_2 = y_1 \cdot \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y_1^2) + 2 p_2 x_1 y_1 \\ \text{where} \ r^2 = x_1^2 + y_1^2 x2=x1⋅1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+2p1x1y1+p2(r2+2x12)y2=y1⋅1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+p1(r2+2y12)+2p2x1y1where r2=x12+y12
径向畸变参数： k 1 , k 2 , k 3 , k 4 , k 5 , k 6 k_1,k_2,k_3,k_4,k_5,k_6 k1,k2,k3,k4,k5,k6
切向畸变参数： p 1 , p 2 p_1,p_2 p1,p2
1.1.3. 图像去畸变
相机原始图像 I \mathbf{I} I，去畸变后的图像 I d \mathbf{I}_d Id。
I d \mathbf{I}_d Id 的一点 [ u d , v d , 1 ] ⊤ [u_d, v_d, 1]^\top [ud,vd,1]⊤，对应 I \mathbf{I} I 的一点 [ u , v , 1 ] ⊤ [u, v, 1]^\top [u,v,1]⊤。
[ x 1 y 1 1 ] = K − 1 [ u d v d 1 ] \begin{bmatrix} x_1 \\ y_1 \\ 1 \end{bmatrix} = \mathbf{K}^{-1} \begin{bmatrix} u_d\\ v_d\\ 1 \end{bmatrix} x1y11 =K−1 udvd1
x 2 = x 1 ⋅ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + 2 p 1 x 1 y 1 + p 2 ( r 2 + 2 x 1 2 ) y 2 = y 1 ⋅ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + p 1 ( r 2 + 2 y 1 2 ) + 2 p 2 x 1 y 1 where r 2 = x 1 2 + y 1 2 x_2 = x_1 \cdot \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + 2 p_1 x_1 y_1 + p_2(r^2 + 2 x_1^2) \\ y_2 = y_1 \cdot \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y_1^2) + 2 p_2 x_1 y_1 \\ \text{where} \ r^2 = x_1^2 + y_1^2 x2=x1⋅1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+2p1x1y1+p2(r2+2x12)y2=y1⋅1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+p1(r2+2y12)+2p2x1y1where r2=x12+y12
[ u v 1 ] = K [ x 2 y 2 1 ] \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} x_2\\ y_2\\ 1 \end{bmatrix} uv1 =K x2y21
2. 相机标定
2.1. 张正友标定法
考虑某一张标定物图片，检测得到了若干个角点的像素坐标 { ( 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
2.1.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
2.1.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）
2.1.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的初值。
2.1.2. 步骤二 求解外参初始值
上一步已经得到内参的初值，这里考虑无畸变模型：
Z
c
⋅
[
x
c
y
c
1
]
=
Z
c
⋅
K
−
1
⋅
[
u
v
1
]
=
[
r
1
,
r
2
,
t
]
⏟
H
3
×
3
⋅
[
X
W
Y
W
1
]
Z_c \cdot \begin{bmatrix} x_c \\ y_c \\ 1 \\ \end{bmatrix} = Z_c \cdot \mathbf{K}^{-1} \cdot \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \underbrace{ \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⋅
xcyc1
=Zc⋅K−1⋅
uv1
=H3×3
[r1,r2,t]⋅
XWYW1
2.1.2.1. 求解单应性矩阵
H \mathbf{H} H的求解参照步骤一的求解方法。
2.1.2.2. 求解外参的初值
H = [ h 1 , h 2 , h 3 ] R = [ r 1 , r 2 , r 3 ] \mathbf{H} = [\mathbf{h}_1,\mathbf{h}_2,\mathbf{h}_3] \\ \mathbf{R} = [\mathbf{r}_1,\mathbf{r}_2,\mathbf{r}_3] \\ H=[h1,h2,h3]R=[r1,r2,r3]
于是
r
1
=
h
1
∣
∣
h
1
∣
∣
r
2
=
h
2
∣
∣
h
2
∣
∣
r
3
=
r
1
×
r
2
t
=
h
3
(
∣
∣
h
1
∣
∣
+
∣
∣
h
2
∣
∣
)
/
2
\mathbf{r}_1 = \frac{\mathbf{h}_1}{||\mathbf{h}_1||} \\ \mathbf{r}_2 = \frac{\mathbf{h}_2}{||\mathbf{h}_2||} \\ \mathbf{r}_3 = \mathbf{r}_1 \times \mathbf{r}_2 \\ \mathbf{t} = \frac{\mathbf{h}_3}{(||\mathbf{h}_1||+||\mathbf{h}_2||)/2}
r1=∣∣h1∣∣h1r2=∣∣h2∣∣h2r3=r1×r2t=(∣∣h1∣∣+∣∣h2∣∣)/2h3
由上一步得到的
R
\mathbf{R}
R,可利用Rodrigues公式，先转换到旋转向量，再转换回旋转矩阵的方式去规范化
R
\mathbf{R}
R的初值。
2.1.2.3. 代码实现时的规范化操作
在代码实现时，会对物点坐标做数值上的规范化
[
X
^
W
Y
^
W
0
]
=
[
X
W
Y
W
0
]
−
[
X
W
‾
Y
W
‾
0
]
\begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 0 \\ \end{bmatrix} = \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} - \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix}
X^WY^W0
=
XWYW0
−
XWYW0
相当于对标定物的世界坐标系做了平移，平移向量为 [ X W ‾ , Y W ‾ , 0 ] T [\overline{X_W},\overline{Y_W},0]^T [XW,YW,0]T，这样
Z c ⋅ [ x c y c 1 ] = Z c ⋅ K − 1 ⋅ [ u v 1 ] = [ r ^ 1 , r ^ 2 , t ^ ] ⏟ H ^ 3 × 3 ⋅ [ X ^ W Y ^ W 1 ] Z_c \cdot \begin{bmatrix} x_c \\ y_c \\ 1 \\ \end{bmatrix} = Z_c \cdot \mathbf{K}^{-1} \cdot \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \underbrace{ \begin{bmatrix} \hat{\mathbf{r}}_1, \hat{\mathbf{r}}_2,\hat{\mathbf{t}} \end{bmatrix} }_{\displaystyle \hat{\mathbf{H}}_{3 \times 3}} \cdot \begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 1 \\ \end{bmatrix} Zc⋅ xcyc1 =Zc⋅K−1⋅ uv1 =H^3×3 [r^1,r^2,t^]⋅ X^WY^W1
按照之前的解法，解得 H ^ , R ^ , t ^ \hat{\mathbf{H}},\hat{\mathbf{R}},\hat{\mathbf{t}} H^,R^,t^。由物点在摄像机坐标系下的坐标不变，可得
[ X c Y c Z c ] = R ⋅ [ X W Y W 0 ] + t = R ^ ⋅ [ X ^ W Y ^ W 0 ] + t ^ = R ^ ⋅ ( [ X W Y W 0 ] − [ X W ‾ Y W ‾ 0 ] ) + t ^ = R ^ ⋅ [ X W Y W 0 ] + ( − R ^ ⋅ [ X W ‾ Y W ‾ 0 ] + t ^ ) \begin{align} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ \end{bmatrix} &= \mathbf{R} \cdot \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} + \mathbf{t} \\ &= \hat{\mathbf{R}} \cdot \begin{bmatrix} \hat{X}_W \\ \hat{Y}_W \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} \\ &= \hat{\mathbf{R}} \cdot \left( \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} - \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} \right) + \hat{\mathbf{t}} \\ & = \hat{\mathbf{R}} \cdot \begin{bmatrix} X_W \\ Y_W \\ 0 \\ \end{bmatrix} + \left( - \hat{\mathbf{R}} \cdot \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} \right) \end{align} XcYcZc =R⋅ XWYW0 +t=R^⋅ X^WY^W0 +t^=R^⋅ XWYW0 − XWYW0 +t^=R^⋅ XWYW0 + −R^⋅ XWYW0 +t^
于是
R = R ^ t = − R ^ ⋅ [ X W ‾ Y W ‾ 0 ] + t ^ \mathbf{R} = \hat{\mathbf{R}} \\ \mathbf{t} = - \hat{\mathbf{R}} \cdot \begin{bmatrix} \overline{X_W} \\ \overline{Y_W} \\ 0 \\ \end{bmatrix} + \hat{\mathbf{t}} R=R^t=−R^⋅ XWYW0 +t^
2.1.3. 步骤三 LM算法求解内参、外参、畸变参数的最优解
前两步已经得到了所有的待解参数的初始值，这一步基于重投影误差最小化准则，使用LM算法去迭代求解所有待解参数的最优解。至此，标定结束。
1433
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
