张正友标定算法原理详解

本人邮箱：[email protected]，欢迎交流讨论，
欢迎转载，转载请注明网址http://blog.csdn.net/u010128736/

一、背景

”张正友标定”是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法[1]。文中提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。因此张氏标定法被广泛应用于计算机视觉方面。

二、计算内参和外参的初值

1、计算单应性矩阵H

根据之前博客介绍的摄像机模型，设三维世界坐标的点为 $X=[X,Y,Z,1]^T$ ，二维相机平面像素坐标为 $m=[u,v,1]^T$ ，所以标定用的棋盘格平面到图像平面的单应性关系为：

s 0 m = K [R, T] X

$s_0m = K[R, T]X$
其中s为尺度因子，K为摄像机内参数，R为旋转矩阵，T为平移向量。令

K = ⎡ ⎣ ⎢ α 00 γ β 0 u 0 v 0 1 ⎤ ⎦ ⎥

$K=\left[ \begin{array}{ccc} \alpha & \gamma & u_0 \\ 0 & \beta & v_0 \\ 0 & 0 & 1 \end{array} \right]$
注意，s对于齐次坐标来说，不会改变齐次坐标值。张氏标定法中，将世界坐标系狗仔在棋盘格平面上，令棋盘格平面为Z=0的平面。则可得

s ⎡ ⎣ ⎢ u v 1 ⎤ ⎦ ⎥ = K [r 1 r 2 r 3 t] ⎡ ⎣ ⎢ ⎢ ⎢ X Y 01 ⎤ ⎦ ⎥ ⎥ ⎥ = K [r 1 r 2 t] ⎡ ⎣ ⎢ X Y 1 ⎤ ⎦ ⎥

$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 ⎡ ⎣ ⎢ u v 1 ⎤ ⎦ ⎥ = H ⎡ ⎣ ⎢ X Y 1 ⎤ ⎦ ⎥ H = [h 1 h 2 h 3] = λ K [r 1 r 2 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。

2、计算内参数矩阵

由上式可得

λ = 1 s r 1 = 1 λ K - 1 h 1 r 2 = 1 λ K - 1 h 2

$\lambda = \frac{1}{s} \\ r_1=\frac{1}{\lambda}K^{-1}h_1 \\ r_2=\frac{1}{\lambda}K^{-1}h_2$
由于旋转矩阵是个酉矩阵，r1和r2正交，可得

r T 1 r 2 = 0 | | r 1 | | = | | r 2 | | = 1

$r_1^Tr_2=0 \\ ||r_1||=||r_2||=1$
代入可得：

h T 1 K - T K - 1 h 2 = 0 h T 1 K - T K - 1 h 1 = h T 2 K - T K - 1 h 2

$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 - T K - 1 = ⎡ ⎣ ⎢ B 11 B 21 B 31 B 12 B 22 B 32 B 13 B 23 B 33 ⎤ ⎦ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 α 2 - γ α 2 β v 0 γ - u 0 β α 2 β - γ α 2 β γ 2 α 2 β 2 + 1 β 2 - γ ( v 0 γ - u 0 β ) α 2 β 2 - v 0 β 2 v 0 γ - u 0 β α 2 β - γ ( v 0 γ - u 0 β ) α 2 β 2 - v 0 β 2 ( v 0 γ - u 0 β ) 2 α 2 β 2 + v 0 β 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 = [B 11 B 12 B 22 B 13 B 23 B 33] T

$b=\left[ \begin{array}{cccccc} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} \end{array} \right]^T$
可以推导得到

h T i B h j = v T i j b v i j = [h i 1 h j 1 h i 1 h j 2 + h i 2 h j 1 h i 2 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3] 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$
利用约束条件可以得到：

[v T 12 (v 11 - v 22) T] b = 0

$\left[ \begin{array}{c} v^T_{12} \\ (v_{11}-v_{22})^T \end{array} \right]b=0$
通过上式，我们至少需要三幅包含棋盘格的图像，可以计算得到B，然后通过cholesky分解，得到相机的内参数矩阵K。

3、计算外参数矩阵

由之前的推导，可得

λ = 1 s = 1 ∥ A - 1 h 1 ∥ = 1 ∥ A - 1 h 2 ∥ r 1 = 1 λ K - 1 h 1 r 2 = 1 λ K - 1 h 2 r 3 = r 1 \times r 2 t = λ K - 1 h 3

$\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, R i, t i, M i j) = K [R | t] M i j

$\hat{m}(K,R_i,t_i,M_{ij}) = K[R|t]M_{ij}$
其中Ri和ti是第i副图对应的旋转矩阵和平移向量，K是内参数矩阵。则角点mij的概率密度函数为：

f (m i j) = 1 2 π - - \sqrt e - ( m ^ ( K , R i , t i , M i j ) - m i j ) 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, R i, t i, M i j) = \prod i = 1, j = 1 n, m f (m i j) = 1 2 π - - \sqrt e - \sum n i = 1 \sum m j = 1 ( m ^ ( K , R i , t i , M i j ) - m i j ) 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]进行迭代求最优解。

\sum i = 1 n \sum j = 1 m ∥ m^(K, R i, t i, M i j) - m i j ∥ 2

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

四、径向畸变估计

张氏标定法只关注了影响最大的径向畸变。则数学表达式为：

u^= u + (u - u 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 2] v^= v + (v - v 0) [k 1 (x 2 + y 2) + k 2 (x 2 + y 2) 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^) $(\hat u, \hat v)$ 是实际畸变后的像素坐标。(u0,v0)代表主点，(x,y)是理想无畸变的连续图像坐标，

(x^,y^) $(\hat x, \hat y)$ 是实际畸变后的连续图像坐标。k1和k2为前两阶的畸变参数。

u^= u 0 + α x^+ γ y^v^= v 0 + β y^

$\hat u = u_0 + \alpha \hat x + \gamma \hat y \\ \hat v = v_0 + \beta \hat y$
化作矩阵形式：

[(u - u 0) (x 2 + y 2) (v - v 0) (x 2 + y 2) (u - u 0) (x 2 + y 2) 2 (v - v 0) (x 2 + y 2) 2] [k 1 k 2] = [u^- u v^- 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]$
记做：

D k = d

$Dk=d$
则可得：

k = [k 1 k 2] T = (D T D) - 1 D T d

$k=[k_1\ k_2]^T = (D^TD)^{-1}D^Td$
计算得到畸变系数k。

使用最大似然的思想优化得到的结果，即像上一步一样，LM法计算下列函数值最小的参数值：

\sum i = 1 n \sum j = 1 m ∥ m^(K, k 1, k 2, R i, t i, M i j) - m i j ∥ 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$
到此，张氏标定法介绍完毕。我们也得到了相机内参、外参和畸变系数。

参考

1、Zhang, Zhengyou - 《IEEE Transactions on Pattern Analysis & Machine Intelligence》 - 2000
2、J.More.Thelevenberg-marquardtalgorithm,implementationandtheory.InG.A.Watson,
editor,NumericalAnalysis,LectureNotesinMathematics630.Springer-Verlag,1977.