张氏标定法算法原理详解本文张正友博士在1999年提出的相机内外参标定方法：张氏标定法，的算法原理进行了详细的梳理，对其原

张氏标定法

张氏标定法是张正友博士在1999年发表在国际顶级会议ICCV上的论文《Flexible Camera Calibration By Viewing a Plane From Unknown Orientations》中，提出的一种利用平面棋盘格进行相机标定的实用方法。

符号定义

世界坐标系坐标： $(X_w,Y_w,Z_w)$

相机坐标系坐标： $(X_c,Y_c,Z_c)$ ，坐标系 $Z$ 轴与光轴重合，正方向为光轴方向， $X$ 轴正方向向右， $Y$ 轴正方向向下

像素坐标系坐标： $(u,v)$ ，图像左上角为坐标原点， $u$ 正方向向右， $v$ 正方向向下

图像坐标系中心点在像素坐标下的坐标： $(u_0,v_0)$

相机焦距： $f$ ，相机坐标系中心与图像坐标系中心的距离

单个像素的横向尺寸： $d_x$

单个像素的纵向尺寸： $d_y$

棋盘平面到像素平面的单应矩阵： $H$

标定设置

打印一张棋盘格标定图纸，将其贴在平面物体的表面。
打印的棋盘图纸位于世界坐标系 $Z_w=0$ 的平面上，棋盘格左上角为世界坐标系原点， $X_w$ 轴正方向向右， $Y_w$ 轴正方向向下，如图所示

拍摄一组不同方向棋盘格的图片，可以通过移动相机来实现，也可以移动标定图片来实现

标定原理

坐标变换关系

棋盘格上点 $p$ 的世界坐标 $(X_w,Y_w,0)$ 到其相机坐标系的坐标 $(X_c,Y_c,Z_c)$ 的变换关系如下
$\begin{bmatrix} X_c\\ Y_c\\ Z_c \end{bmatrix} = R \begin{bmatrix} X_w\\ Y_w\\ 0 \end{bmatrix} + t = \begin{bmatrix} R_1 & R_2 & t \end{bmatrix} \begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\tag{1}$
其中， $R$ 和 $t$ 分别是相机坐标系相对于世界坐标系的旋转矩阵和平移向量， $R_1$ 和 $R_2$ 是旋转矩阵 $R$ 的前两列。
棋盘格上点 $p$ 的相机坐标系到像素坐标系的变换关系如下
$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \frac{1}{Z_c} \begin{bmatrix} \frac{f}{d_x} & \gamma & u_0\\ 0 & \frac{f}{d_y} & u_0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c\\ Y_c\\ Z_c \end{bmatrix}\tag{2}$
其中， $\gamma$ 是由于制造误差产生的两个坐标轴偏斜参数，通常很小。
令 $s=\frac{1}{Z_c}$ ， $f_x=\frac{f}{d_x}$ ， $f_y=\frac{f}{d_y}$ ，令相机内参矩阵为 $K$ ，即
$K= \begin{bmatrix} f_x & \gamma & u_0\\ 0 & f_y & v_0\\ 0 & 0 & 1 \end{bmatrix}\tag{3}$
则棋盘格上点 $p$ 的世界坐标 $(X_w,Y_w,0)$ 与其像素坐标的变换关系如下
$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = s K \begin{bmatrix} X_c\\ Y_c\\ Z_c \end{bmatrix} = s K \begin{bmatrix} R_1 & R_2 & t \end{bmatrix} \begin{bmatrix} X_w\\ Y_w\\ 1 \end{bmatrix}\tag{4}$

单应矩阵求解

对于棋盘格相对于相机的每个摆放位置，即对于每个具体的 $R$ 和 $t$ ，都对应一个具体的棋盘格平面到像素坐标的一个单应变换，令单应变换矩阵为 $H$ ，则

H = s K \begin{bmatrix} R_1 & R_2 & t \end{bmatrix}\tag{5}

假设棋盘格上的角点个数为 $n$ ，分别为 $p_1,p_2,\dots,p_n$ ，根据棋盘格的每个格子的尺寸，可以计算出棋盘格上每个角点的世界坐标

(x_1,y_1,0),(x_2,y_2,0),\dots,(x_n,y_n,0)

根据图像角点检测，计算出棋盘格上每个角点的像素坐标

(x'_1,y'_1),(x'_2,y'_2),\dots,(x'_n,y'_n)

则 $H$ 满足

\begin{bmatrix} x'_i\\ y'_i\\ 1 \end{bmatrix} \sim H \begin{bmatrix} x_i\\ y_i\\ 1 \end{bmatrix}\tag{6}

其中 $\sim$ 为对应坐标比例相等， $i=1,2,\dots,n$ . 当 $n\ge 4$ 时，可以计算得到 $H$ 。不过，采用直接线性解法通常很难得到最优解，所以实际使用中一般会用其他优化方法，如奇异值分解、Levenberg-Marquarat（LM）算法等进行求解。

单应变换矩阵 $H$ 与相机内参 $K$ 之间的关系

将单应矩阵表示为列形式，根据上面的讨论，有

H=[H_1,H_2,H_3]=sK[R_1,R_2,t]\tag{7}

即

\begin{aligned} H_1=sKR_1\ \ 或\ \ R_1=\frac{1}{s}K^{-1}H_1\\ H_2=sKR_2\ \ 或\ \ R_2=\frac{1}{s}K^{-1}H_2\\ H_3=sKt\ \ 或\ \ t=\frac{1}{s}K^{-1}H_3 \end{aligned}\tag{8}

根据旋转矩阵的性质可知， $R_1^TR_1=R_2^TR_2=1$ ， $R_1^TR_2=R_2^TR_1=0$ ，因此可得

\begin{aligned} &\frac{1}{s^2}H_1^T(K^{-1})^TK^{-1}H_1 = \frac{1}{s^2}H_2^T(K^{-1})^TK^{-1}H_2 = 1\\ &\frac{1}{s^2}H_1^T(K^{-1})^TK^{-1}H_2 = 0 \end{aligned}\tag{9}

令

B=(K^{-1})^TK^{-1}= \begin{bmatrix} \frac{1}{f_x^2} & -\frac{\gamma}{f_x^2f_y} & \frac{v_0\gamma-u_0f_y}{f_x^2f_y}\\ -\frac{\gamma}{f_x^2f_y} & \frac{\gamma^2}{f_x^2f_y^2}+\frac{1}{f_y^2} & -\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2}\\ \frac{v_0\gamma-u_0f_y}{f_x^2f_y} & -\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2} & \frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}+1 \end{bmatrix} \\ = \begin{bmatrix} B_{11} & B_{12} & B_{13}\\ B_{21} & B_{22} & B_{23}\\ B_{31} & B_{32} & B_{33} \end{bmatrix}\tag{10}

可知 $B$ 是对称矩阵，因此未知参数只有6个。将 $B$ 带入上面式子，可得

\begin{aligned} &H_1^TBH_1 = H_2^TBH_2\\ &H_1^TBH_2 = 0 \end{aligned}\tag{11}

令 $H_i=[h_{i1}, h_{i2}, h_{i3}]^T$ ，则有

H_i^TBH_j= \begin{bmatrix} 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{bmatrix}^T \begin{bmatrix} B_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33} \end{bmatrix}\tag{12}

令

v_{ij} = \begin{bmatrix} 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{bmatrix} ,\ \ b = \begin{bmatrix} B_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33} \end{bmatrix}\tag{13}

则有

H_i^TBH_j=v_{ij}^Tb\tag{14}

因此，最初的两个约束条件，最终可以转化为如下形式

\begin{aligned} & (v_{11}^T-v_{22}^T)b=0\\ & v_{12}^Tb=0 \end{aligned} \longrightarrow\ \ \begin{bmatrix} v_{11}^T-v_{22}^T\\ v_{12}^T \end{bmatrix} b=0\tag{15}

计算相机内参和外参

假设针对棋盘格不同摆放位置，进行了 $m$ 次拍摄，每个具体摆放对应的旋转和平移为

(R^{(1)},t^{(1)}),(R^{(2)},t^{(2)}),\dots,(R^{(m)},t^{(m)})

分别计算得到的单应变换矩阵为

H^{(i)}= s^{(i)} K \begin{bmatrix} R^{(i)}_1 & R^{(i)}_2 & t^{(i)} \end{bmatrix}\tag{16}

其中 $i=1,2,\dots,m$ . 根据单应矩阵和相机内参之间的关系可知， $b$ 有6个未知数需要求解，因此需要6个约束方程构成的方程组才能求解 $b$ ，而每个单应矩阵提供两个约束方程，因此要求解 $b$ ，至少需要 $m\ge3$ .

根据计算得到的 $b$ 的每个分量，与 $B$ 中对应分量相差一个常量倍数，即存在 $\lambda$ 使得

b = \begin{bmatrix} \hat{B}_{11}\\ \hat{B}_{12}\\ \hat{B}_{22}\\ \hat{B}_{13}\\ \hat{B}_{23}\\ \hat{B}_{33} \end{bmatrix} = \begin{bmatrix} \lambda B_{11}\\ \lambda B_{12}\\ \lambda B_{22}\\ \lambda B_{13}\\ \lambda B_{23}\\ \lambda B_{33} \end{bmatrix}\tag{17}

因此，可得

\begin{bmatrix} \frac{1}{f_x^2} & -\frac{\gamma}{f_x^2f_y} & \frac{v_0\gamma-u_0f_y}{f_x^2f_y}\\ -\frac{\gamma}{f_x^2f_y} & \frac{\gamma^2}{f_x^2f_y^2}+\frac{1}{f_y^2} & -\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2}\\ \frac{v_0\gamma-u_0f_y}{f_x^2f_y} & -\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2} & \frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}+1 \end{bmatrix} \\ = \begin{bmatrix} B_{11} & B_{12} & B_{13}\\ B_{21} & B_{22} & B_{23}\\ B_{31} & B_{32} & B_{33} \end{bmatrix} = \begin{bmatrix} B_{11} & B_{12} & B_{13}\\ B_{12} & B_{22} & B_{23}\\ B_{13} & B_{23} & B_{33} \end{bmatrix} = \begin{bmatrix} \frac{1}{\lambda}\hat{B}_{11} & \frac{1}{\lambda}\hat{B}_{12} & \frac{1}{\lambda}\hat{B}_{13}\\ \frac{1}{\lambda}\hat{B}_{12} & \frac{1}{\lambda}\hat{B}_{22} & \frac{1}{\lambda}\hat{B}_{23}\\ \frac{1}{\lambda}\hat{B}_{13} & \frac{1}{\lambda}\hat{B}_{23} & \frac{1}{\lambda}\hat{B}_{33} \end{bmatrix}\tag{18}

根据公式 $(10)$ ，可以求得相机内参 $K$ 中每个位置的值

\left\{ \begin{aligned} & f_x=\sqrt\frac{\lambda}{\hat{B}_{11}}\\ & f_y=\sqrt{\frac{\lambda \hat{B}_{11}}{\hat{B}_{11}\hat{B}_{22}-\hat{B}_{12}^2}}\\ & u_0=\frac{\gamma v_0}{f_y}-\frac{\hat{B}_{13}f_x^2}{\lambda}\\ & v_0=\frac{\hat{B}_{12}\hat{B}_{13}-\hat{B}_{11}\hat{B}_{23}}{\hat{B}_{11}\hat{B}_{22}-\hat{B}_{12}^2}\\ & \gamma=-\frac{\hat{B}_{12}f_x^2f_y}{\lambda}\\ & \lambda=\hat{B}_{33}-\frac{\hat{B}_{13}^2+v_0(\hat{B}_{12}\hat{B}_{13}-\hat{B}_{11}\hat{B}_{23})}{\hat{B}_{11}} \end{aligned} \right.\tag{19}

得到内参后，根据公式 $(8)$ ，以及 $\|R_1\|=\|R_2\|=1$ ， $R_3=R_1\times R_2$ ，可计算出外参 $R$ 和 $t$ .

注：公式 $(19)$ 中关于 $\lambda$ 的表达式的计算推导如下

\begin{aligned} &\hat{B}_{33}-\frac{\hat{B}_{13}^2+v_0(\hat{B}_{12}\hat{B}_{13}-\hat{B}_{11}\hat{B}_{23})}{\hat{B}_{11}}\\ &=\lambda B_{33}-\frac{(\lambda B_{13})^2+v_0(\lambda B_{12}\lambda B_{13}-\lambda B_{11}\lambda B_{23})}{\lambda B_{11}}\\ &=\lambda\left(B_{33}-\frac{B_{13}^2+v_0(B_{12}B_{13}-B_{11}B_{23})}{B_{11}}\right)\\ &=\lambda\left[\frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}+1\right]\\ & \ \ \ \ \ \ \ \lambda-\frac{(\frac{v_0\gamma-u_0f_y}{f_x^2f_y})^2+v_0\left[(-\frac{\gamma}{f_x^2f_y})(\frac{v_0\gamma-u_0f_y}{f_x^2f_y})-(\frac{1}{f_x^2})(-\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2})\right]}{\frac{1}{f_x^2}}\\ &=\lambda\left[\frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}+1\right]\\ &\ \ \ \ \ \ \ \ \lambda-\left[\frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+v_0\left[(-\frac{\gamma}{f_y})(\frac{v_0\gamma-u_0f_y}{f_x^2f_y})-(-\frac{\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}-\frac{v_0}{f_y^2})\right]\right]\\ &=\lambda\left[\frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}+1\right]\\ &\ \ \ \ \ \ \ \ -\lambda\left[\frac{(v_0\gamma-u_0f_y)^2}{f_x^2f_y^2}-\frac{v_0\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}+\frac{v_0\gamma(v_0\gamma-u_0f_y)}{f_x^2f_y^2}+\frac{v_0^2}{f_y^2}\right]\\ &=\lambda\cdot1=\lambda \end{aligned}

实际应用中计算相机内外参的方法

上述的推导结果是基于理想情况下的解，从理论上证明了张氏标定算法的可行性。但在实际标定过程中，一般使用最大似然估计进行优化。假设我们拍摄了 $m$ 张标定图片，每张图片里有 $n$ 个棋盘格角点。

令，三维空间点 $p_j$ 在图片上对应的二维像素为 $x_{ij}$ ，三维空间点经过相机内参 $K$ ，外参 $R_i$ 和 $t_i$ 变换后得到的二维像素为 $x'(K,R_i,t_i,p_j)$ ，其中 $i=1,2,\dots,m$ ， $j=1,2,\dots,n$ .

通过最小化每个空间点的实际像素值与经过计算得到的像素值的距离，来求解上述最大似然估计问题

\sum^m_{i=1}\sum^n_{j=1}\|x_{ij}-x'(K,R_i,t_i,p_j)\|^2\tag{20}

从而解出相机具体的内参 $K$ 和外参 $R$ 、 $t$ .

相机存在畸变的情况下，计算相机内外参和畸变参数

透镜的畸变主要分为径向畸变和切向畸变，还有薄透镜畸变等等，但都没有径向和切向畸变影响显著，所以我们在这里只考虑径向和切向畸变。

径向畸变是由于透镜形状的制造工艺导致。且越向透镜边缘移动径向畸变越严重。下图所示是径向畸变的两种类型：桶形畸变和枕形畸变。

矫正径向畸变前后的图像坐标关系为

\left\{ \begin{aligned} &\hat{x}=x(1+k_1r^2+k_2r^4+k_3r^6)\\ &\hat{y}=y(1+k_1r^2+k_2r^4+k_3r^6) \end{aligned} \right.\tag{21}

切向畸变是由于透镜和CMOS或者CCD的安装位置误差导致。因此，如果存在切向畸变，一个矩形被投影到成像平面上时，很可能会变成一个梯形。切向畸变需要两个额外的畸变参数来描述，矫正前后的图像坐标关系为

\left\{ \begin{aligned} &\hat{x}=x+[2p_1xy+p_2(r^2+2x^2)]\\ &\hat{y}=y+[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right.\tag{22}

径向畸变和切向畸变联合矫正图像坐标关系式为

\left\{ \begin{aligned} &\hat{x}=x(1+k_1r^2+k_2r^4+k_3r^6)+[2p_1xy+p_2(r^2+2x^2)]\\ &\hat{y}=y(1+k_1r^2+k_2r^4+k_3r^6)+[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right.\tag{23}

其中， $(x,y)$ 为理想的无畸变的图像坐标， $(\hat{x},\hat{y})$ 为畸变后的图像坐标， $r$ 为图像坐标点到图像中心点的距离，即 $r^2=x^2+y^2$ ， $k_1,k_2,k_3$ 是径向畸变参数， $p_1,p_2$ 切向畸变参数。

假设 $(u,v)$ 是理想无畸变的像素坐标， $(\hat{u},\hat{v})$ 是畸变后的像素坐标，则根据内参 $K$ 和图像坐标与像素坐标的关系，它们与 $(x,y)$ 和 $(\hat{x},\hat{y})$ 的关系如下

\left\{ \begin{aligned} &u=u_0+f_x x+\gamma y\\ &v=v_0+f_y y\\ &--------\\ &\hat{u}=u_0+f_x \hat{x}+\gamma \hat{y}\\ &\hat{v}=v_0+f_y \hat{y} \end{aligned} \right. \Longrightarrow \left\{ \begin{aligned} &x=\frac{u-u_0}{f_x}-\frac{\gamma(v-v_0)}{f_xf_y}\\ &y=\frac{v-v_0}{f_y}\\ &--------\\ &\hat{x}=\frac{\hat{u}-u_0}{f_x}-\frac{\gamma(\hat{v}-v_0)}{f_xf_y}\\ &\hat{y}=\frac{\hat{v}-v_0}{f_y} \end{aligned} \right. \tag{24}

把 $(24)$ 带入 $(21)$ 可得

\begin{aligned} &\left\{ \begin{aligned} &\frac{\hat{u}-u_0}{f_x}-\frac{\gamma(\hat{v}-v_0)}{f_xf_y}=(\frac{u-u_0}{f_x}-\frac{\gamma(v-v_0)}{f_xf_y})(1+k_1r^2+k_2r^4+k_3r^6)\\ &\frac{\hat{v}-v_0}{f_y}=\frac{v-v_0}{f_y}(1+k_1r^2+k_2r^4+k_3r^6) \end{aligned} \right.\\ \Longrightarrow &\left\{ \begin{aligned} &\hat{u}=u+(u-u_0)(k_1r^2+k_2r^4+k_3r^6)\\ &\hat{v}=v+(v-v_0)(k_1r^2+k_2r^4+k_3r^6) \end{aligned} \right. \end{aligned} \tag{25}

把 $(24)$ 带入 $(22)$ 可得

\begin{aligned} &\left\{ \begin{aligned} &\frac{\hat{u}-u_0}{f_x}-\frac{\gamma(\hat{v}-v_0)}{f_xf_y}=\frac{u-u_0}{f_x}-\frac{\gamma(v-v_0)}{f_xf_y}+[2p_1xy+p_2(r^2+2x^2)]\\ &\frac{\hat{v}-v_0}{f_y}=\frac{v-v_0}{f_y}+[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right.\\ \Longrightarrow &\left\{ \begin{aligned} &\hat{u}=u+f_x[2p_1xy+p_2(r^2+2x^2)]+\gamma[p_1(r^2+2y^2)+2p_2xy]\\ &\hat{v}=v+f_y[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right. \end{aligned}\tag{26}

把 $(24)$ 带入 $(23)$ 可得

\begin{aligned} &\left\{ \begin{aligned} &\frac{\hat{u}-u_0}{f_x}-\frac{\gamma(\hat{v}-v_0)}{f_xf_y}\\ &=(\frac{u-u_0}{f_x}-\frac{\gamma(v-v_0)}{f_xf_y})(1+k_1r^2+k_2r^4+k_3r^6)+[2p_1xy+p_2(r^2+2x^2)]\\ &\frac{\hat{v}-v_0}{f_y}=\frac{v-v_0}{f_y}(1+k_1r^2+k_2r^4+k_3r^6)+[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right.\\ \Longrightarrow &\left\{ \begin{aligned} &\hat{u}=u+(u-u_0)(k_1r^2+k_2r^4+k_3r^6)+f_x[2p_1xy+p_2(r^2+2x^2)]\\ &+\gamma[p_1(r^2+2y^2)+2p_2xy]\\ &\hat{v}=v+(v-v_0)(k_1r^2+k_2r^4+k_3r^6)+f_y[p_1(r^2+2y^2)+2p_2xy] \end{aligned} \right. \end{aligned}\tag{27}

现在考虑受透镜畸变影响时，相机内外参的求解。由于径向畸变的影响相对较明显，实践中主要考虑径向畸变参数，通常只考虑径向畸变的前两个参数 $k_1,k_2$ （增加更多的参数会使得模型变的复杂且不稳定），新的待优化函数如下

\sum^m_{i=1}\sum^n_{j=1}\|x_{ij}-x'(K,R_i,t_i,p_j,k_1,k_2)\|^2\tag{28}

上述非线性优化问题通常用Levenberg-Marquardt（LM）算法进行迭代求解，从而解出相机具体的内参 $K$ 和外参 $R$ 、 $t$ ，还有畸变参数 $k_1,k_2$ .

张氏标定法算法原理详解