相机标定&张正有标定法

1 相机标定概念

在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程就称之为相机标定（或摄像机标定）。无论是在图像测量或者机器视觉应用中，相机参数的标定都是非常关键的环节，其标定结果的精度及算法的稳定性直接影响相机工作产生结果的准确性。因此，做好相机标定是做好后续工作的前提，提高标定精度是科研工作的重点所在。

2 坐标系及成像几何模型

相机标定过程建立在图像成像几何模型的基础上，涉及到了如下坐标系转换。

2.1 坐标系

2.1.1 世界坐标系

三维直角坐标系，可描述相机和实际物体的空间位置，一般用$(X_w,Y_w,Z_w)$进行表示。

世界坐标系的原点位置可以根据需要调整。

2.1.2 相机坐标系

三维直角坐标系，一般用$(x_c,y_c,z_c)$进行表示。

以镜头光心为原点，x轴和y轴和相机两边平行，z轴为光轴，与成像屏幕垂直。

2.1.3 图像坐标系

二维直角坐标系，图像坐标系是用物理单位（例如毫米）表示像素在图像中的位置。一般需要给出各像素在两个坐标轴上的物理尺寸，比如x方向上一个像素的长度表示为$dx$，y方向上一个像素的长度表示为$dy$。

以CCD 图像平面的中心为坐标原点，X轴和Y 轴分别平行于图像平面的两条垂直边，用( x , y )表示其坐标值。

2.1.4 像素坐标系

二维直角坐标系，表示图像上各点的像素位置。

u轴向右为正，v轴向下为正，用(u，v)表示像素值。

2.2 成像过程的几何模型

2.2.1 世界坐标系到相机坐标系的转换

世界坐标系中的一个点映射到图像上，首先需要把该点映射到相机坐标系中。而实际情况下，相机坐标系的原点和世界坐标系的原点并不重合，映射过程可以建模为刚性变换，包含旋转和平移两个变换过程。总体可建模为：

$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{c}Rt\\ 01\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

其中$R\in R^{3\times 3}$旋转矩阵，$t\in R^{3\times 1}$为平移矢量，$(x_c,y_c,z_c,1{)}^T$为相机坐标系的齐次坐标，$(x_w,y_w,z_w,1{)}^T$为世界坐标系的齐次坐标。

2.2.2 相机坐标系到图像坐标系的转换

相机坐标系到图像坐标系的转换过程即小孔成像的过程，如下图所示。

相机坐标系中的点$P$映射为图像坐标系中的$P^{{}^{\prime }}$。

因为图像坐标系的原点映射到图像坐标系上就是图像坐标系上的原点，所以$P^{{}^{\prime }}$点的坐标和$P$点的坐标等比例对应。

假设$P$点坐标为$(x_c,y_c,z_c)$，$P^{{}^{\prime }}$点坐标为$(x^{{}^{\prime }},y^{{}^{\prime }})$，成像时的焦距定义为图像坐标系和相机坐标系的距离，即上图中的$f$。因此有：
$$\begin{gathered} \frac{x^{{}^{\prime }}}{x_c}=\frac{f}{z_c} \\ \frac{y^{{}^{\prime }}}{y_c}=\frac{f}{z_c} \end{gathered}$$

用矩阵表示即为：
$$z_c\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{c}f000\\ 0f00\\ 0010\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$

2.2.3 图像坐标系到像素坐标系的转换

图像坐标系(x,y)到像素坐标系(u,v)的变换涉及到平移和尺度变换。平移是因为图像坐标系以物理成像屏幕的中心为原点，而像素坐标系以物理成像屏幕的左上角为原点。尺度变换是因为图像坐标系以实际的物理长度(如毫米)表示各像素点的位置，而图像坐标系中以数量值表示各像素点的位置，需要把图像坐标系中的物理长度转换为像素坐标系中的像素点的数量值。

两个坐标系之间的关系如下图所示：

假设图像坐标系中的一个点$(x^{{}^{\prime }},y^{{}^{\prime }})$，映射到像素坐标系中为点$(u,v)$，可以表示为：
$$\begin{gathered} u=u_0+\frac{x^{{}^{\prime }}}{dx} \\ v=v_0+\frac{y^{{}^{\prime }}}{dy} \end{gathered}$$

其中，$(u_0,v_0)$为图像坐标系原点在像素坐标系中的位置。

上述映射过程用矩阵可以表示为：

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{dx}0u_0\\ 0\frac{1}{dy}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]$$

很多情况下还假设图像坐标系的u和v两个坐标轴不互相垂直(目前暂不知道原因)，两者之间的夹角为$\theta$，如下图所示：

此时有：
$$\begin{gathered} u=u_0+\frac{x^{{}^{\prime }}}{dx}-\frac{y^{{}^{\prime }}\cot (\theta )}{dx} \\ v=v_0+\frac{y^{{}^{\prime }}}{dy\sin (\theta )} \end{gathered}$$

表示为矩阵形式为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{dx}\frac{-\cot (\theta )}{dx}{u}_0\\ 0\frac{1}{dy\sin (\theta )}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]$$

2.2.4 总结

总结相机成像的几何模型，可以表示为：
$$\begin{gathered} z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=z_c\left[\begin{array}{c}\frac{1}{dx}\frac{-\cot (\theta )}{dx}{u}_0\\ 0\frac{1}{dy\sin (\theta )}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{dx}\frac{-\cot (\theta )}{dx}{u}_0\\ 0\frac{1}{dy\sin (\theta )}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}f000\\ 0f00\\ 0010\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right] \\ =\left[\begin{array}{c}\frac{1}{dx}\frac{-\cot (\theta )}{dx}{u}_0\\ 0\frac{1}{dy\sin (\theta )}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}f000\\ 0f00\\ 0010\end{array}\right]\left[\begin{array}{c}Rt\\ 01\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right] \\ =\left[\begin{array}{c}\frac{f}{dx}\frac{-f\cot (\theta )}{dx}{u}_{0}0\\ 0\frac{f}{dy\sin (\theta )}{v}_{0}0\\ 0010\end{array}\right]\left[\begin{array}{c}Rt\\ 01\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right] \end{gathered}$$

其中，

$\left[\begin{array}{c}\frac{f}{dx}\frac{-f\cot (\theta )}{dx}{u}_{0}0\\ 0\frac{f}{dy\sin (\theta )}{v}_{0}0\\ 0010\end{array}\right]$称为相机的内参数矩阵，是相机的固有参数，需要通过相机标定获取，且一次获取可以重复使用。

$\left[\begin{array}{c}Rt\\ 01\end{array}\right]$称为相机的外参数矩阵，用于衡量相机自身的姿态。

2.3 畸变

常见畸变分为径向畸变和切向畸变两种，畸变是相机本身的固有特性，和相机内参相同，标定一次之后即可。

径向畸变来自于透镜形状。切向畸变来自于整个摄像机的组装过程。其他类型的畸变，没有径向畸变、切向畸变显著，所以忽略不计。

特别注意：畸变都是作用在图像坐标系上的。

2.3.1 径向畸变

径向畸变就是沿着透镜半径方向分布的畸变，产生原因是光线在原理透镜中心的地方比靠近中心的地方更加弯曲，这种畸变在普通廉价的镜头中表现更加明显，径向畸变主要包括桶形畸变和枕形畸变两种。以下分别是枕形和桶形畸变示意图：

光轴中心的畸变为0，沿着镜头半径方向向边缘移动，畸变越来越严重。畸变的数学模型可以用主点（principle point）周围的泰勒级数展开式的前几项进行描述，通常使用前两项，即k1和k2，对于畸变很大的镜头，如鱼眼镜头，可以增加使用第三项k3来进行描述。成像仪上某点根据其在径向方向上的分布位置，调节公式为：
$$\begin{gathered} \hat{x}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ \hat{y}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \end{gathered}$$
$(\hat{x},\hat{y})$表示畸变后的点，$(x,y)$表示理想的点。$r^2=x^2+y^2$表示点到图像中心的距离。

下图是距离光心不同距离上的点经过透镜径向畸变后点位的偏移示意图，可以看到，距离光心越远，径向位移越大，表示畸变也越大，在光心附近，几乎没有偏移。

2.3.2 切向畸变

切向畸变是由于透镜本身与相机传感器平面（成像平面）或图像平面不平行而产生的，这种情况多是由于透镜被粘贴到镜头模组上的安装偏差导致。

畸变模型可以用两个额外的参数p1和p2来描述：

$$\begin{gathered} \hat{x}=x+(2p_{1}y+p_2(r^2+2x^2)) \\ \hat{y}=y+(p_1(r^2+2y^2)+2p_{2}x) \end{gathered}$$

$(x,y)$表示理想的坐标点，$(\hat{x},\hat{y})$表示畸变点的位置。

2.3.3 畸变校正

相机的畸变模型共涉及了5个参数，分别是纵向畸变的$k_1,k_2,k_3$和切向畸变的$p_1,p_2$。畸变校正就是根据畸变图像和理想图像的特征点的对应关系，估计5个参数的过程。根据估计的参数对畸变图像进行反向校正，进而通过区域裁剪等办法对畸变图像进行处理得到理想的图像。

3 张正有标定法

3.1 总体思路

张正有标定法需要使用一个棋盘格标定板，如下图所示：
实际标定过程中，需要打印一张标定板贴于某平面上，使用相机拍摄该标定板，得到一张图像。变换标定板的姿态和位置，可以得到多幅图像。这样采集的多幅图像就是标定算法的输入。

张正有标定法假设每幅标定板图像拍摄时，标定板都位于世界坐标系中$Z_w=0$的平面上，且$X_w$和$Y_w$和标定板的两边平行。根据各个棋盘格子的宽度，可以得到各黑白块接触角点的世界坐标。对当前的标定板使用相机成像后，可以利用角点检测算法获取图像中各黑白接触角点的像素坐标。因为这些角点是一一对应的，所以可以建立多组角点从世界坐标系到像素坐标系的映射关系，也就是建立了多个方程组。通过多个方程组可以求解未知系数，也就是得到相机的内外参数矩阵和畸变系数。

3.2 处理流程

算法的处理过程为：

3.3 数学推导

本节内容摘抄自：https://zhuanlan.zhihu.com/p/94244568

张正友标定法标定相机的内外参数的思路如下：
1.求解内参矩阵与外参矩阵的积；
2.求解内参矩阵；
3.求解外参矩阵；
4.求解径向畸变系数。

3.3.1 求解内参数矩阵和外参数矩阵的积

将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标$z_w=0$，因此，原单点无畸变的成像模型可以化为下式。其中，$[R_1,R_2]$为旋转矩阵$R$ 的前两列。为了简便，将内参矩阵记为$A$。

$$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{f}{dx}\frac{-f\cot (\theta )}{dx}{u}_{0}0\\ 0\frac{f}{dy\sin (\theta )}{v}_{0}0\\ 0010\end{array}\right]\left[\begin{array}{c}Rt\\ 01\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=A(R_1{R}_{2}t)\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]$$

对于不同的图片，内参矩阵A为定值；对于同一张图片，内参矩阵A、外参矩阵$(R_1,R_2,t)$为定值；对同一张图像上的单个特征点，内参矩阵A、外参矩阵$(R_1,R_2,t)$和尺度因子$z_c$为定值。

我们将$A(R_1{R}_{2}t)$记为矩阵$H$，$H$即为内参矩阵和外参矩阵的积，记矩阵$H$的三列为$(H_1,H_2,H_3)$，则有：
$$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}{H}_{11}{H}_{12}{H}_{13}\\ H_{21}{H}_{22}{H}_{23}\\ H_{31}{H}_{32}{H}_{33}\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]$$

利用上式，消除尺度因子$z_c$，可得：
$$\begin{gathered} u=\frac{H_{11}{x}_w+H_{12}{y}_w+H_{13}}{H_{31}{x}_w+H_{32}{y}_w+H_{33}} \\ v=\frac{H_{21}{x}_w+H_{22}{y}_w+H_{23}}{H_{31}{x}_w+H_{32}{y}_w+H_{33}} \end{gathered}$$

因为已经消除了尺度因子$z_c$，所以上式对一张图像上的所有角点均成立。 $(u,v)$是像素坐标系下的标定板角点的坐标， $(x_w,y_w)$是世界坐标系下的标定板角点的坐标。通过图像识别算法，我们可以得到标定板角点的像素坐标$(u,v)$，又由于标定板的世界坐标系是人为定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到世界坐标系下的$(x_w,y_w)$。

由这里的$H$是齐次矩阵，有8个独立未知元素。每一个标定板角点可以提供两个约束方程（ $u,U,V$的对应关系、$v,U,V$的对应关系提供了两个约束方程），因此，当一张图片上的标定板角点数量等于4时，即可求得该图片对应的矩阵$H$。当一张图片上的标定板角点数量大于4时，利用最小二乘法回归最佳的矩阵$H$。

至此，求得了内参参数矩阵的乘积$H$。

3.3.2 求解内参数矩阵

我们已知了矩阵$H=(H_1{H}_2{H}_3)=A(R_1{R}_{2}t)$ ，接下来需要求解相机的内参矩阵$A$ 。

我们利用$R_1,R_2$作为旋转矩阵$R$的两列，存在单位正交的关系，即：
$$\begin{gathered} R_1^T{R}_2=0 \\ {R}_1^T{R}_1=R_2^T{R}_2=1 \end{gathered}$$

由$H$和$R_1,R_2$的关系，可得：
$$\begin{gathered} R_1=A^{-1}H1 \\ {R}_2=A^{-1}H2 \end{gathered}$$

代入可得：
$$\begin{gathered} H{1}^T{A}^{-T}{A}^{-1}H2=0 \\ H{1}^T{A}^{-T}{A}^{-1}H1=H{2}^T{A}^{-T}{A}^{-1}H2=1 \end{gathered}$$

另外，我们发现，上述两个约束方程中均存在矩阵$A^{-T}{A}^{-1}$。因此，我们记$A^{-T}{A}^{-1}=B$，则$B$为对称阵。我们试图先求解出矩阵$B$ ，通过矩阵$B$再求解相机的内参矩阵$A$。

3.3.3 求解外参数矩阵

这里再次强调一下，对于同一个相机，相机的内参矩阵取决于相机的内部参数，无论标定板和相机的位置关系是怎么样的，相机的内参矩阵不变。这也正是在3.3.2节“求解内参矩阵”中，我们可以利用不同的图片（标定板和相机位置关系不同）获取的矩阵$H$，共同求解相机内参矩阵$B$的原因。

但是，外参矩阵反映的是标定板和相机的位置关系。对于不同的图片，标定板和相机的位置关系已经改变，此时每一张图片对应的外参矩阵都是不同的。

在关系：$H=A(R_1{R}_{2}t)$中，我们已经求解得到了矩阵$H$（对于同一张图片相同，对于不同的图片不同）、矩阵$A$（对于不同的图片都相同）。通过公式：$(R_1{R}_{2}t)=A^{-1}H$，即可求得每一张图片对应的外参矩阵$(R_1{R}_{2}t)$。

注意，这里值得指出，完整的外参矩阵为$\left[\begin{array}{c}Rt\\ 01\end{array}\right]$。但是，由于张正友标定板将世界坐标系的原点选取在棋盘格上，则棋盘格上任一点的物理坐标$z_w=0$，将旋转矩阵$R$的第三列$R_3$消掉，因此， $R_3$在坐标转化中并没有作用。但是$R_3$要使得$R$满足旋转矩阵的性质，即列与列之间单位正交，因此可以通过向量$R_1,R_2$的叉乘，即$R_3=R_1\times R_2$，计算得到$R_3$。

此时，相机的内参矩阵和外参矩阵均已得到。

注：以上推导都是假设不存在畸变参数的情况下成立的。但是事实上，相机是存在畸变参数的，因此，张正友标定法还需要通过L-M算法对于参数进行迭代优化。

3.3.4 求解相机的径向畸变系数

张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。

$$\begin{gathered} \hat{x}=x^{{}^{\prime }}(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ \hat{y}=y^{{}^{\prime }}(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \end{gathered}$$
$(\hat{x},\hat{y})$表示畸变后的点，$(x^{{}^{\prime }},y^{{}^{\prime }})$表示理想的点。$r^2=x^2+y^2$表示点到图像中心的距离。

图像坐标系和像素坐标系的映射关系为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{dx}\frac{-\cot (\theta )}{dx}{u}_0\\ 0\frac{1}{dy\sin (\theta )}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]$$

由于$\theta$接近90°，上式可以简化为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{dx}0u_0\\ 0\frac{1}{dy}{v}_0\\ 001\end{array}\right]\left[\begin{array}{c}{x}^{{}^{\prime }}\\ y^{{}^{\prime }}\\ 1\end{array}\right]$$
即：
$$\begin{gathered} u=u_0+\frac{x^{{}^{\prime }}}{dx} \\ v=v_0+\frac{y^{{}^{\prime }}}{dy} \end{gathered}$$

那么对于畸变点$(\hat{x},\hat{y})$，有：
$$\begin{gathered} \hat{u}=u_0+\frac{\hat{x}}{dx} \\ \hat{v}=v_0+\frac{\hat{y}}{dy} \end{gathered}$$

代入径向畸变的公式中可得：
$$\begin{array}{l}\hat{u}-u_0=\left(u-u_0\right)\left(1+k_1{r}^2+k_2{r}^4\right)\\ \hat{v}-v_0=\left(v-v_0\right)\left(1+k_1{r}^2+k_2{r}^4\right)\end{array}$$

$$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A(R_1{R}_{2}t)\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]=H\left[\begin{array}{c}{x}_w\\ y_w\\ 1\end{array}\right]$$

利用上式，消除尺度因子$z_c$，可得：
$$\begin{gathered} u=\frac{H_{11}{x}_w+H_{12}{y}_w+H_{13}}{H_{31}{x}_w+H_{32}{y}_w+H_{33}} \\ v=\frac{H_{21}{x}_w+H_{22}{y}_w+H_{23}}{H_{31}{x}_w+H_{32}{y}_w+H_{33}} \end{gathered}$$

即可得到理想的、无畸变的像素坐标$(u,v)$。当然，由于外参矩阵$(R_1{R}_{2}t)$和内参矩阵$A$是在有畸变的情况下获得的，这里得到的像素坐标$(u,v)$并不是完全理想的、无畸变的。我们的总逻辑是，在进行内参矩阵和外参矩阵的求解的时候，我们假设不存在畸变；在进行畸变系数的求解的时候，我们假设求得的内参矩阵和外参矩阵是无误差的。最后，我们再通过L-M算法对于参数进行迭代优化。

需要指出，上述公式推导的时候以2阶径向畸变为例，但实际上更高阶的径向畸变同理，只是需要的约束方程个数更多而已。

3.4 代码

见opencv\sources\samples\cpp\tutorial_code\calib3d\camera_calibration.cpp，参数文件见同路径下的in_VID5.xml。

如果想调试底层代码，可以源码编译opencv，单步调试。

参考

https://wenku.baidu.com/view/66f55b0702020740be1e9bd8.html
https://blog.csdn.net/lql0716/article/details/71973318?locationNum=8&fps=1
https://blog.csdn.net/a083614/article/details/78579163
https://www.cnblogs.com/wdmzslh/p/6593406.html
https://blog.csdn.net/zashizhi3299/article/details/94484748
https://zhuanlan.zhihu.com/p/94445955
https://blog.csdn.net/qq_28514991/article/details/88559590
https://zhuanlan.zhihu.com/p/94244568