Version:1.0StartHTML:000000220EndHTML:000477285StartFragment:000079865EndFragment:000477212StartSelection:000079865EndSelection:000477212SourceURL:https://blog.csdn.net/u010128736/article/details/52860364

張正友標(biāo)定算法原理詳解

本人郵箱：sylvester0510@163.com，歡迎交流討論，

歡迎轉(zhuǎn)載，轉(zhuǎn)載請(qǐng)注明網(wǎng)址http://blog.csdn.net/u010128736/

一、背景

??”張正友標(biāo)定”是指張正友教授1998年提出的單平面棋盤格的攝像機(jī)標(biāo)定方法[1]。文中提出的方法介于傳統(tǒng)標(biāo)定法和自標(biāo)定法之間，但克服了傳統(tǒng)標(biāo)定法需要的高精度標(biāo)定物的缺點(diǎn)，而僅需使用一個(gè)打印出來(lái)的棋盤格就可以。同時(shí)也相對(duì)于自標(biāo)定而言，提高了精度，便于操作。因此張氏標(biāo)定法被廣泛應(yīng)用于計(jì)算機(jī)視覺(jué)方面。

二、計(jì)算內(nèi)參和外參的初值

1、計(jì)算單應(yīng)性矩陣H

根據(jù)之前博客介紹的攝像機(jī)模型，設(shè)三維世界坐標(biāo)的點(diǎn)為X=[X,Y,Z,1]TX=[X,Y,Z,1]TX=[X,Y,Z,1]^T，二維相機(jī)平面像素坐標(biāo)為m=[u,v,1]Tm=[u,v,1]Tm=[u,v,1]^T，所以標(biāo)定用的棋盤格平面到圖像平面的單應(yīng)性關(guān)系為：

s0m=K[R,T]Xs0m=K[R,T]X

s_0m = K[R, T]X

其中s為尺度因子，K為攝像機(jī)內(nèi)參數(shù)，R為旋轉(zhuǎn)矩陣，T為平移向量。令

K=???α00γβ0u0v01???K=[αγu00βv0001]

K=\left[ \begin{array}{ccc}

\alpha & \gamma & u_0 \\

0 & \beta & v_0 \\

0 & 0 & 1

\end{array}

\right]

注意，s對(duì)于齊次坐標(biāo)來(lái)說(shuō)，不會(huì)改變齊次坐標(biāo)值。張氏標(biāo)定法中，將世界坐標(biāo)系狗仔在棋盤格平面上，令棋盤格平面為Z=0的平面。則可得

s???uv1???=K[r1r2r3t]?????XY01?????=K[r1r2t]???XY1???s[uv1]=K[r1r2r3t][XY01]=K[r1r2t][XY1]

s\left[ \begin{array}{c} u \\ v \\ 1 \end{array} \right] = K\left[

\begin{array}{cccc} r_1 & r_2 & r_3 & t

\end{array}

\right]

\left[

\begin{array}{c}

X \\

Y \\

0 \\

1

\end{array}

\right]=K

\left[

\begin{array}{cccc} r_1 & r_2 & t

\end{array}

\right]

\left[

\begin{array}{c}

X \\

Y \\

1

\end{array}

\right]

我們把K[r1, r2, t]叫做單應(yīng)性矩陣H，即

s???uv1???=H???XY1???H=[h1?h2?h3]=λK[r1?r2?t]s[uv1]=H[XY1]H=[h1?h2?h3]=λK[r1?r2?t]

s \left[

\begin{array}{c}

u \\

v \\

1

\end{array}

\right]=

H

\left[

\begin{array}{c}

X \\

Y \\

1

\end{array}

\right]\\

H=[h_1\ h_2\ h3] = \lambda K[r_1\ r_2\ t]

H是一個(gè)齊次矩陣，所以有8個(gè)未知數(shù)，至少需要8個(gè)方程，每對(duì)對(duì)應(yīng)點(diǎn)能提供兩個(gè)方程，所以至少需要四個(gè)對(duì)應(yīng)點(diǎn)，就可以算出世界平面到圖像平面的單應(yīng)性矩陣H。

2、計(jì)算內(nèi)參數(shù)矩陣

由上式可得

λ=1sr1=1λK?1h1r2=1λK?1h2λ=1sr1=1λK?1h1r2=1λK?1h2

\lambda = \frac{1}{s} \\

r_1=\frac{1}{\lambda}K^{-1}h_1? \\

r_2=\frac{1}{\lambda}K^{-1}h_2?

由于旋轉(zhuǎn)矩陣是個(gè)酉矩陣，r1和r2正交，可得

rT1r2=0||r1||=||r2||=1r1Tr2=0||r1||=||r2||=1

r_1^Tr_2=0 \\

||r_1||=||r_2||=1

代入可得：

hT1K?TK?1h2=0hT1K?TK?1h1=hT2K?TK?1h2h1TK?TK?1h2=0h1TK?TK?1h1=h2TK?TK?1h2

h_1^TK^{-T}K^{-1}h_2=0 \\

h_1^TK^{-T}K^{-1}h_1 = h_2^TK^{-T}K^{-1}h_2

即每個(gè)單應(yīng)性矩陣能提供兩個(gè)方程，而內(nèi)參數(shù)矩陣包含5個(gè)參數(shù)，要求解，至少需要3個(gè)單應(yīng)性矩陣。為了得到三個(gè)不同的單應(yīng)性矩陣，我們使用至少三幅棋盤格平面的圖片進(jìn)行標(biāo)定。通過(guò)改變相機(jī)與標(biāo)定板之間的相對(duì)位置來(lái)得到三個(gè)不同的圖片。為了方便計(jì)算，定義如下：

B=K?TK?1=???B11B21B31B12B22B32B13B23B33???=???????1α2?γα2βv0γ?u0βα2β?γα2βγ2α2β2+1β2?γ(v0γ?u0β)α2β2?v0β2v0γ?u0βα2β?γ(v0γ?u0β)α2β2?v0β2(v0γ?u0β)2α2β2+v0β2+1???????B=K?TK?1=[B11B12B13B21B22B23B31B32B33]=[1α2?γα2βv0γ?u0βα2β?γα2βγ2α2β2+1β2?γ(v0γ?u0β)α2β2?v0β2v0γ?u0βα2β?γ(v0γ?u0β)α2β2?v0β2(v0γ?u0β)2α2β2+v0β2+1]

B=K^{-T}K^{-1}=

\left[

\begin{array}{ccc}

B_{11} & B_{12} & B_{13} \\

B_{21} & B_{22} & B_{23} \\

B_{31} & B_{32} & B_{33}

\end{array}

\right]=

\left[

\begin{array}{ccc}

\frac{1}{\alpha^2} & -\frac{\gamma}{\alpha^2\beta} & \frac{v_0\gamma-u_0\beta}{\alpha^2\beta} \\

-\frac{\gamma}{\alpha^2\beta} & \frac{\gamma^2}{\alpha^2\beta^2}+\frac{1}{\beta^2} & -\frac{\gamma(v_0\gamma-u_0\beta)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} \\

\frac{v_0\gamma-u_0\beta}{\alpha^2\beta} & -\frac{\gamma(v_0\gamma-u_0\beta)}{\alpha^2\beta^2}-\frac{v_0}{\beta^2} & \frac{(v_0\gamma-u_0\beta)^2}{\alpha^2\beta^2}+\frac{v_0}{\beta^2}+1

\end{array}

\right]

可以看到，B是一個(gè)對(duì)稱陣，所以B的有效元素為六個(gè)，讓這六個(gè)元素寫成向量b，即

b=[B11B12B22B13B23B33]Tb=[B11B12B22B13B23B33]T

b=\left[ \begin{array}{cccccc} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} \end{array} \right]^T

可以推導(dǎo)得到

hTiBhj=vTijbvij=[hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3]ThiTBhj=vijTbvij=[hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3]T

h_i^TBh_j = v^T_{ij}b \\

v_{ij}=\left[ \begin{array}{cccccc} h_{i1}h_{j1} & h_{i1}h_{j2}+h_{i2}h_{j1} & h_{i2}h_{j2} & h_{i3}h_{j1}+h_{i1}h_{j3} & h_{i3}h_{j2}+h_{i2}h_{j3} & h_{i3}h_{j3} \end{array} \right]^T

利用約束條件可以得到：

[vT12(v11?v22)T]b=0[v12T(v11?v22)T]b=0

\left[ \begin{array}{c} v^T_{12} \\ (v_{11}-v_{22})^T \end{array} \right]b=0

通過(guò)上式，我們至少需要三幅包含棋盤格的圖像，可以計(jì)算得到B，然后通過(guò)cholesky分解，得到相機(jī)的內(nèi)參數(shù)矩陣K。

3、計(jì)算外參數(shù)矩陣

由之前的推導(dǎo)，可得

λ=1s=1∥A?1h1∥=1∥A?1h2∥r1=1λK?1h1r2=1λK?1h2r3=r1×r2t=λK?1h3λ=1s=1‖A?1h1‖=1‖A?1h2‖r1=1λK?1h1r2=1λK?1h2r3=r1×r2t=λK?1h3

\lambda = \frac{1}{s}=\frac{1}{\|A^{-1}h_1\|}=\frac{1}{\|A^{-1}h_2\|} \\

r_1=\frac{1}{\lambda}K^{-1}h_1? \\

r_2=\frac{1}{\lambda}K^{-1}h_2? \\

r_3 = r_1 \times r_2 \\

t=\lambda K^{-1}h_3

三、最大似然估計(jì)

上述的推導(dǎo)結(jié)果是基于理想情況下的解，但由于可能存在高斯噪聲，所以使用最大似然估計(jì)進(jìn)行優(yōu)化。設(shè)我們采集了n副包含棋盤格的圖像進(jìn)行定標(biāo)，每個(gè)圖像里有棋盤格角點(diǎn)m個(gè)。令第i副圖像上的角點(diǎn)Mj在上述計(jì)算得到的攝像機(jī)矩陣下圖像上的投影點(diǎn)為：

m^(K,Ri,ti,Mij)=K[R|t]Mijm^(K,Ri,ti,Mij)=K[R|t]Mij

\hat{m}(K,R_i,t_i,M_{ij}) = K[R|t]M_{ij}

其中Ri和ti是第i副圖對(duì)應(yīng)的旋轉(zhuǎn)矩陣和平移向量，K是內(nèi)參數(shù)矩陣。則角點(diǎn)mij的概率密度函數(shù)為：

f(mij)=12π??√e?(m^(K,Ri,ti,Mij)?mij)2σ2f(mij)=12πe?(m^(K,Ri,ti,Mij)?mij)2σ2

f(m_{ij})=\frac{1}{\sqrt{2\pi}}e^{\frac{-(\hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{\sigma^2}}

構(gòu)造似然函數(shù)：

L(A,Ri,ti,Mij)=∏i=1,j=1n,mf(mij)=12π??√e?∑ni=1∑mj=1(m^(K,Ri,ti,Mij)?mij)2σ2L(A,Ri,ti,Mij)=∏i=1,j=1n,mf(mij)=12πe?∑i=1n∑j=1m(m^(K,Ri,ti,Mij)?mij)2σ2

L(A,R_i,t_i,M_{ij}) = \prod^{n,m}_{i=1,j=1}f(m_{ij})=\frac{1}{\sqrt{2\pi}}e^{\frac{-\sum^n_{i=1}\sum^m_{j=1}(\hat{m}(K,R_i,t_i,M_{ij})-m_{ij})^2}{\sigma^2}}

讓L取得最大值，即讓下面式子最小。這里使用的是多參數(shù)非線性系統(tǒng)優(yōu)化問(wèn)題的Levenberg-Marquardt算法[2]進(jìn)行迭代求最優(yōu)解。

∑i=1n∑j=1m∥m^(K,Ri,ti,Mij)?mij∥2∑i=1n∑j=1m‖m^(K,Ri,ti,Mij)?mij‖2

\sum^n_{i=1}\sum^m_{j=1} \| \hat{m}(K,R_i,t_i,M_{ij})-m_{ij} \|^2

四、徑向畸變估計(jì)

張氏標(biāo)定法只關(guān)注了影響最大的徑向畸變。則數(shù)學(xué)表達(dá)式為：

u^=u+(u?u0)[k1(x2+y2)+k2(x2+y2)2]v^=v+(v?v0)[k1(x2+y2)+k2(x2+y2)2]u^=u+(u?u0)[k1(x2+y2)+k2(x2+y2)2]v^=v+(v?v0)[k1(x2+y2)+k2(x2+y2)2]

\hat u = u + (u-u_0)[k_1(x^2+y^2)+k_2(x^2+y^2)^2] \\

\hat v = v + (v-v_0)[k_1(x^2+y^2)+k_2(x^2+y^2)^2]

其中，(u,v)是理想無(wú)畸變的像素坐標(biāo)，(u^,v^)(u^,v^)(\hat u, \hat v)是實(shí)際畸變后的像素坐標(biāo)。(u0,v0)代表主點(diǎn)，(x,y)是理想無(wú)畸變的連續(xù)圖像坐標(biāo)，(x^,y^)(x^,y^)(\hat x, \hat y)是實(shí)際畸變后的連續(xù)圖像坐標(biāo)。k1和k2為前兩階的畸變參數(shù)。

u^=u0+αx^+γy^v^=v0+βy^u^=u0+αx^+γy^v^=v0+βy^

\hat u = u_0 + \alpha \hat x + \gamma \hat y \\

\hat v = v_0 + \beta \hat y

化作矩陣形式：

[(u?u0)(x2+y2)(v?v0)(x2+y2)(u?u0)(x2+y2)2(v?v0)(x2+y2)2][k1k2]=[u^?uv^?v][(u?u0)(x2+y2)(u?u0)(x2+y2)2(v?v0)(x2+y2)(v?v0)(x2+y2)2][k1k2]=[u^?uv^?v]

\left[

\begin{array}{cc}

(u-u_0)(x^2+y^2) & (u-u_0)(x^2+y^2)^2 \\

(v-v_0)(x^2+y^2) & (v-v_0)(x^2+y^2)^2

\end{array}

\right]

\left[

\begin{array}{c}

k_1 \\ k_2

\end{array}

\right]=

\left[

\begin{array}{c}

\hat u -u \\ \hat v -v

\end{array}

\right]

記做：

Dk=dDk=d

Dk=d

則可得：

k=[k1?k2]T=(DTD)?1DTdk=[k1?k2]T=(DTD)?1DTd

k=[k_1\ k_2]^T = (D^TD)^{-1}D^Td

計(jì)算得到畸變系數(shù)k。

使用最大似然的思想優(yōu)化得到的結(jié)果，即像上一步一樣，LM法計(jì)算下列函數(shù)值最小的參數(shù)值：

∑i=1n∑j=1m∥m^(K,k1,k2,Ri,ti,Mij)?mij∥2∑i=1n∑j=1m‖m^(K,k1,k2,Ri,ti,Mij)?mij‖2

\sum^n_{i=1}\sum^m_{j=1} \| \hat{m}(K,k_1,k_2,R_i,t_i,M_{ij})-m_{ij} \|^2

到此，張氏標(biāo)定法介紹完畢。我們也得到了相機(jī)內(nèi)參、外參和畸變系數(shù)。