张正友标定算法原理详解
Version:1.0StartHTML:000000220EndHTML:000477285StartFragment:000079865EndFragment:000477212StartSelection:000079865EndSelection:000477212SourceURL:https://blog.csdn.net/u010128736/article/details/52860364
本人邮箱：sylvester0510@163.com，欢迎交流讨论，
欢迎转载，转载请注明网址http://blog.csdn.net/u010128736/
”张正友标定”是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法[1]。文中提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。因此张氏标定法被广泛应用于计算机视觉方面。
根据之前博客介绍的摄像机模型，设三维世界坐标的点为X=[X,Y,Z,1]TX=[X,Y,Z,1]TX=[X,Y,Z,1]^T，二维相机平面像素坐标为m=[u,v,1]Tm=[u,v,1]Tm=[u,v,1]^T，所以标定用的棋盘格平面到图像平面的单应性关系为：
s0m=K[R,T]Xs0m=K[R,T]X
s_0m = K[R, T]X
其中s为尺度因子，K为摄像机内参数，R为旋转矩阵，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对于齐次坐标来说，不会改变齐次坐标值。张氏标定法中，将世界坐标系狗仔在棋盘格平面上，令棋盘格平面为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]叫做单应性矩阵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是一个齐次矩阵，所以有8个未知数，至少需要8个方程，每对对应点能提供两个方程，所以至少需要四个对应点，就可以算出世界平面到图像平面的单应性矩阵H。
由上式可得
λ=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
由于旋转矩阵是个酉矩阵，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
即每个单应性矩阵能提供两个方程，而内参数矩阵包含5个参数，要求解，至少需要3个单应性矩阵。为了得到三个不同的单应性矩阵，我们使用至少三幅棋盘格平面的图片进行标定。通过改变相机与标定板之间的相对位置来得到三个不同的图片。为了方便计算，定义如下：
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是一个对称阵，所以B的有效元素为六个，让这六个元素写成向量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
可以推导得到
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
通过上式，我们至少需要三幅包含棋盘格的图像，可以计算得到B，然后通过cholesky分解，得到相机的内参数矩阵K。
由之前的推导，可得
λ=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
上述的推导结果是基于理想情况下的解，但由于可能存在高斯噪声，所以使用最大似然估计进行优化。设我们采集了n副包含棋盘格的图像进行定标，每个图像里有棋盘格角点m个。令第i副图像上的角点Mj在上述计算得到的摄像机矩阵下图像上的投影点为：
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副图对应的旋转矩阵和平移向量，K是内参数矩阵。则角点mij的概率密度函数为：
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}}
构造似然函数：
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取得最大值，即让下面式子最小。这里使用的是多参数非线性系统优化问题的Levenberg-Marquardt算法[2]进行迭代求最优解。
∑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
张氏标定法只关注了影响最大的径向畸变。则数学表达式为：
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)是理想无畸变的像素坐标，(u^,v^)(u^,v^)(\hat u, \hat v)是实际畸变后的像素坐标。(u0,v0)代表主点，(x,y)是理想无畸变的连续图像坐标，(x^,y^)(x^,y^)(\hat x, \hat y)是实际畸变后的连续图像坐标。k1和k2为前两阶的畸变参数。
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
计算得到畸变系数k。
使用最大似然的思想优化得到的结果，即像上一步一样，LM法计算下列函数值最小的参数值：
∑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
到此，张氏标定法介绍完毕。我们也得到了相机内参、外参和畸变系数。
