计算机视觉——相机参数标定

1. 前言

在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程就称之为相机标定（或摄像机标定）。
较早的相机标定的方法需要计算相机投影矩阵M中的11个未知参数，需要严格个出三个两两互相垂直的平面来做标定，该实验条件较为严格，一般情况难以实现。此后张正友于1998年在论文："A Flexible New Technique fro Camera Calibration"提出了基于单平面棋盘格的相机标定方法。该方法介于传统的标定方法和自标定方法之间，使用简单实用性强，有以下优点：

• 不需要额外的器材，一张打印的棋盘格即可。
• 标定简单，相机和标定板可以任意放置。
• 标定的精度高。

2. 基本原理

2.1 相机标定

同步标定内部参数和外部参数，一般包括两种策略:

1. 光学标定: 利用已知的几何信息(如定长棋盘格)实现参数求解。
2. 自标定: 在静态场景中利用 structure from motion估算参数。

2.2 相机模型

通过空间中已知坐标的(特征)点 $(Xi,Yi,Zi)$，以及它们在图像中的对应坐标 $(ui,vi)$，直接估算 11 个待求解的内部和外部参数。

$x \sim K[R|t]X=MX$
即为:
$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} \sim \begin{bmatrix} m_{00}& m_{01} & m_{02} &m_{03} \\ m_{10}& m_{11} & m_{12} &m_{13} \\ m_{20}&m_{21} & m_{22} & 1 \end{bmatrix} \begin{bmatrix} X\\ Y\\ Z \\ 1 \end{bmatrix}$
则有：
$u_i = \frac{m_{00}X_i+m_{01}Y_i+m_{02}Z_i+m_{03}}{m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1} \\ \quad \\v_i = \frac{m_{10}X_i+m_{11}Y_i+m_{12}Z_i+m_{13}}{m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1}$

2.2 线性回归标定参数

由相机模型公式变形可得：
$u_i (m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1)=m_{00}X_i+m_{01}Y_i+m_{02}Z_i+m_{03}\\\quad \\v_i(m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1)=m_{10}X_i+m_{11}Y_i+m_{12}Z_i+m_{13}$
即为如下矩阵：
$\begin{bmatrix} X_i& Y_i&Z_i & 1 & 0 & 0&0 & 0 & -u_i X_i& -u_i Y_i & -u_i Z_i &-u_i \\ 0& 0 & 0& 0& X_i& Y_i&Z_i & 1 & -v_i X_i& -v_i Y_i & -v_i Z_i &-v_i \end{bmatrix}=\begin{bmatrix} m_{00} \\ m_{01} \\ m_{02} \\ m_{03} \\ m_{10} \\ m_{11} \\ m_{12} \\ m_{13} \\ m_{20} \\ m_{21} \\ m_{22} \\ m_{23} \end{bmatrix}=\begin{bmatrix} 0\\ 0 \end{bmatrix}$
采用最小二乘法解决上述问题。

线性回归标定参数的优缺点
优点:

• 所有的相机参数集中在一个矩阵中，便于求解。
• 通过矩阵可以直接描述世界坐标中的三维点，到二维
  图像平面中点的映射关系。

缺点:

• 无法直接得知具体的内参数和外参数。（QR分解）
• 求解出的11个未知量，比待标定参数（9个）更多。带
  来了参数不独立/相关的问题
• 对噪声/误差敏感

2.3 非线性优化标定参数

用概率的视角去看最小二乘问题
特征点投影方程如下：
$u_i=f(M,x_i)+n_i=\hat{u_i}+n_i,n_i \sim N(0, \sigma ) \\\quad \\v_i=f(M,x_i)+m_i=\hat{u_i}+m_i,m_i \sim N(0, \sigma )$
给定{(ui,vi)}，标定参数矩阵 M 的概率为：
$L = \prod_{i}p(u_i|\hat{u_i})p(v_i|\hat{v_i}) \\= \prod_{i}e^{-(u_i-\hat{u_i})^2/\sigma^2 }e^{-(v_i-\hat{v_i})^2/\sigma^2 }$
给定{(ui,vi)}，标定参数矩阵 M 的似然函数为：
$C=-logL=\sum_{i}(u_i-\hat{u_i})^2/\sigma^2+(v_i-\hat{v_i})^2/\sigma^2 \\ =\sum_{i}^{N} \frac{1}{\sigma^2}(u _i-\frac{m_{00}X_i+m_{01}Y_i+m_{02}Z_i+m_{03}}{m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1})^2+\frac{1}{\sigma^2}(v_i-\frac{m_{10}X_i+m_{11}Y_i+m_{12}Z_i+m_{13}}{m_{20}X_i+m_{21}Y_i+m_{22}Z_i+1})^2$

2.4 张正友相机标定原理

2.4.1 基本概念

• 2D图像点： $m=\begin{bmatrix}u,v\end{bmatrix}^T$
• 3D图像点： $M=\begin{bmatrix}X,Y,Z\end{bmatrix}^T$
• 齐次坐标： $\tilde{m}=\begin{bmatrix}u,v,1\end{bmatrix}^T,\tilde{M}=\begin{bmatrix}X,Y,Z,1\end{bmatrix}^T$
• 描述空间坐标到图像坐标的映射
  $s\tilde{m}=K\begin{bmatrix}R\quad t\end{bmatrix}\tilde{M} \quad K=\begin{bmatrix}\alpha & \gamma &u_0\\ 0&\beta&v_0 \\ 0&0&1\end{bmatrix}$
• $s$: 世界坐标系到图像坐标系的尺度因子
• $K$: 相机内参矩阵
• $(u0,v0)$: 像主点坐标
• $α, β$: 焦距与像素横纵比的融合
• $γ$: 径向畸变参数

2.4.2 单应性矩阵

不妨设棋盘格位于 $Z=0$,定义旋转矩阵R的第i列为 $ri$, 则有：
$s\begin{bmatrix}u\\v\\1\end{bmatrix}=K\begin{bmatrix}r_1&r_2&r_3&t\end{bmatrix}\begin{bmatrix}X\\Y\\0\\1\end{bmatrix}=K\begin{bmatrix}r_1&r_2&t\end{bmatrix}\begin{bmatrix}X\\Y\\1\end{bmatrix}$
于是空间到图像的映射可改为：
$s\tilde{m}=H\tilde{M} H=K\begin{bmatrix}r_1&r_2&t\end{bmatrix}$
其中H 是描述Homographic矩阵,可通过最小二乘，从角点世界坐标到图像坐标的关系求解。

2.4.3 内部参数

令 $H$为 $H=\begin{bmatrix}h_1&h_2&h_3\end{bmatrix}$
$\begin{bmatrix}h_1&h_2&h_3\end{bmatrix}=\lambda K\begin{bmatrix}r_1&r_2&t\end{bmatrix}$
Homography有8个自由度，通过上述等式的矩阵运算，根据 $r_1$和 $r_2$正交，以及归一化的约束可以得到如下等式：
$K^{-1}\begin{bmatrix}h_1&h_2&h_3\end{bmatrix}=\lambda \begin{bmatrix}r_1&r_2&t\end{bmatrix}$
$r_1^Tr_2=0 \Rightarrow h_1^TK^{-T}h_2=0\qquad 正交$
$\left \| r_1 \right \|\left \| r_2 \right \| \Rightarrow h_1^TK^{-T}K^{-1}h_1= h_2^TK^{-T}K^{-1}h_2$

2.4.4 闭合解

定义
$B=K^{-T}K^{-1}=\begin{bmatrix} B_{11}& B_{21}& B_{31} \\ B_{12}&B_{22} &B_{32} \\ B_{13}&B_{23} &B_{33} \end{bmatrix}$

其中
$K=\begin{bmatrix}\alpha & \gamma &u_0\\ 0&\beta&v_0 \\ 0&0&1\end{bmatrix}$

B时对称阵，其未知量可表示为6D向量b
$b=[B_{11}\quad B_{12}\quad B_{22}\quad B_{13}\quad B_{23}\quad B_{33}]$
设 $H$中的第 $i$列为 $h_i$
$h_i=\begin{bmatrix}h_{i1}& h_{i2} &h_{i3}\end{bmatrix}^T$
根据b的定义，可以推出如下公式：
$h_i^TBh_j=v_{ij}^Tb$
$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}^T$
可以推导出
$\begin{bmatrix}v_{12}^T \\ (v_{11}-v_{22})^T\end{bmatrix}b=0$
如果有 $n$组观察图像，则 $V$是 $2n*6$的矩阵
$Vb=0$
根据最小二乘定义， $V b = 0$ 的解是 $V^TV$ 最小特征值对应的特征向量。因此, 可以直接估算出 $b$，后续可以通过 $b$求解内参。

• 当观测平面 $n ≥ 3$ 时，可以得到 $b$的唯一解
• 当 $n = 2$时, 一般可令畸变参数 $γ$ = 0
• 当 $n = 1$时, 仅能估算出 $α$ 与 $β$, 此时一般可假定像主点坐标 $u_0$ 与 $v_0$ 为0.

内部参数可通过如下公式计算(cholesky分解)：

$v_0=(B_{12}B_{13}-B_{11}B_{23})/(B_{11}B_{22}-B_{12}^2)$
$\lambda = B_{33}-[B_{13}^2+v_0(B_{12}B_{13}-B_{11}B_{23})]/B_{11}$
$\alpha = \sqrt{\lambda/B_{11}}$
$\beta = \sqrt{\lambda B_{11}/(B_{11}B_{22}-B_{12}^2)}$
$\gamma = -B_{12}\alpha^2\beta/ \lambda$
$u_0=\gamma v_0/\alpha-B_{13}\alpha^2/\lambda$

外部参数可通过Homography求解:
由 $H=\begin{bmatrix} h_1&h_2&h_3\end{bmatrix}=\lambda K \begin{bmatrix} r_1&r_2&t\end{bmatrix}$可推出：
$r_1=\lambda K^{-1}h_{1} \quad r_2=\lambda K^{-1}h_{2} \\ r_3=r_1\times r_2 \quad t=\lambda K^{-1}h_{3}\\ \lambda=1/\left \|K^{-1}h_{1} \right \| =1/\left \|K^{-1}h_{2} \right \|$
一般而言，求解出的 $R =\begin{bmatrix}r_1&r_2&r_3\end{bmatrix}$不会满足正交与归一的标准。
在实际操作中， $R$ 可以通过SVD分解实现规范化。

2.4.3 极大似然估计

• 给定 $n$ 张棋盘格图像，每张图像有 $m$ 个角点

• 最小化下述公式等同于极大似然估计
  $\sum_{i=1}^{n}\sum_{j=1}^{m}\left \| m_{ij}-\hat{m}(K,R_i,t_i,M_j)\right \|^2$

• 上述非线性优化问题可以利用Levenberg-Marquardt 算法求解

• 需要初值 $K,{R_i,t_i|i=1...n}$

2.4.5 最小化重投影误差

重投影误差：是像素坐标（观测到的投影位置）与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差。

如下图所示，我们通过特征匹配知道，观测值 $p_1$和 $p_2$是同一个空间点 $p$的投影， $p$的投影 $\hat {p_2}$与观测值 $p_2$之间有一定的距离，也就是重投影误差。于是我们调整相机的位姿，使这个距离变小，由于这个调整需要考虑很多个点，所以最后每个点的误差通常不会精确到0。

在使用已知标志点给摄像机定位时，重投影误差并非最好的选择，因为重投影误差模型会认为标志点存在误差，从而重新估计标志点的坐标，引入多余的误差；而此时，事实上，重投影误差已经退化为单边几何变换误差，所以在这种情况下，单边几何变换误差才是最好的代价函数。

3. 张正友相机标定基本流程

1. 打印一张棋盘格A4纸张（黑白间距已知），并贴在一个平板上
2. 针对棋盘格拍摄若干张图片（一般10-20张）
3. 在图片中检测特征点（Harris角点）
4. 根据角点位置信息及图像中的坐标，求解Homographic矩阵
5. 利用解析解估算方法计算出5个内部参数，以及6个外部参数
6. 根据极大似然估计策略，设计优化目标并实现
   参数的refinement

4. 实验准备及数据

本次实验中使用的相机为：IPhone7后置摄像头。
制作棋盘格并打印，其pdf文件如下：

链接：https://pan.baidu.com/s/1kZRrMKz8CLdax15fmnKxtw
提取码：7fmr

将打印出的棋盘格固定在平板上，进行拍摄照片，图片数量最好为15~20张，本次实验拍摄17张图像。构造数据集如下：

5. 实验代码

# -*- coding: utf-8 -*-
"""
Homework: Calibrate the Camera with ZhangZhengyou Method.
Picture File Folder: ".\pic\RGB_camera_calib_img", Without Distort.
By YouZhiyuan 2019.11.18
"""
import os
import re
import numpy as np
import cv2
import glob
np.set_printoptions(suppress=True)
def calib(inter_corner_shape, size_per_grid, img_dir, img_type):
    # criteria: only for subpix calibration, which is not used here.
    # criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    w, h = inter_corner_shape
    # cp_int: corner point in int form, save the coordinate of corner points in world sapce in 'int' form
    # like (0,0,0), (1,0,0), (2,0,0) ....,(10,7,0).
    cp_int = np.zeros((w * h, 3), np.float32)
    cp_int[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
    # cp_world: corner point in world space, save the coordinate of corner points in world space.
    cp_world = cp_int * size_per_grid
    obj_points = []  # the points in world space
    img_points = []  # the points in image space (relevant to obj_points)
    images = glob.glob(img_dir + os.sep + '**.' + img_type)
    for fname in images:
        # print(re.sub("\D", "", fname))
        img = cv2.imread(fname)
        gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        # find the corners, cp_img: corner points in pixel space.
        ret, cp_img = cv2.findChessboardCorners(gray_img, (w, h), None)
        # if ret is True, save.
        if ret == True:
            # cv2.cornerSubPix(gray_img,cp_img,(11,11),(-1,-1),criteria)
            obj_points.append(cp_world)
            img_points.append(cp_img)
            # view the corners
            cv2.drawChessboardCorners(img, (w, h), cp_img, ret)
            cv2.imwrite('FoundC'+re.sub("\D", "", fname)+'.jpg', img)
            # cv2.waitKey(1000)
    # cv2.destroyAllWindows()
    # calibrate the camera
    ret, mat_inter, coff_dis, v_rot, v_trans = cv2.calibrateCamera(obj_points, img_points, gray_img.shape[::-1], None,
                                                                   None)
    print(("ret:"), ret)  # 重投影误差
    print(("internal matrix:\n"), mat_inter)  # 内参数矩阵
    # in the form of (k_1,k_2,p_1,p_2,k_3)
    print(("distortion cofficients:\n"), coff_dis)  # 畸变系数
    print(("rotation vectors:\n"), v_rot)  # 旋转向量  # 外参数
    print(("translation vectors:\n"), v_trans)  # 平移向量  # 外参数
    # calculate the error of reproject
    total_error = 0
    for i in range(len(obj_points)):
        img_points_repro, _ = cv2.projectPoints(obj_points[i], v_rot[i], v_trans[i], mat_inter, coff_dis)
        error = cv2.norm(img_points[i], img_points_repro, cv2.NORM_L2) / len(img_points_repro)
        total_error += error
    print(("Average Error of Reproject: "), total_error / len(obj_points))
    return mat_inter, coff_dis
if __name__ == '__main__':
    inter_corner_shape = (8, 6)
    size_per_grid = 0.028
    img_dir = ".\\data"
    img_type = "jpg"
    calib(inter_corner_shape, size_per_grid, img_dir, img_type)

6. 实验结果及分析

6.1 角点检测结果及分析

在代码运行中保存每张图片的角点检测图片，选取FoundCorner6.jpg放大观察如下：

由上图的放大区域可以看到共检测到了24个角点，可以看到放大区域中的所有黑色方格和白色方格的四个角都被检测为角点。

6.2 内部参数计算结果

控制台输出内部参数计算结果截图如下。其中1号方框内的数据表示：重投影误差；2号方框内的数据表示：内参数矩阵；3号方框内的数据表示：畸变系数；

6.3 外部参数计算结果

外部参数（旋转向量）的输出较多，为方便复制的输出结果如下：

rotation vectors:
 [array([[ 0.08544752],[ 0.01262193],[-1.27658653]]),
 array([[ 0.00017702],[-0.22666298], [-1.31589393]]), 
 array([[ 0.05786146],[ 0.32043949],[ 1.35247793]]),
 array([[-0.22217286],[ 0.19649448],[-1.23054809]]), 
 array([[ 0.29268055], [-0.51259816],[-1.22304696]]), 
 array([[-0.18142884], [-0.26229903],[ 0.25036144]]),
  array([[-0.07664752],[ 0.02384459], [-0.00115754]]), 
  array([[-0.02962394], [-0.04646427], [-1.13415801]]),
  array([[ 0.31597367], [ 0.52081219], [ 1.09633532]]), 
  array([[-0.15250206], [ 0.31823889], [-0.18611884]]), 
  array([[ 0.0429856 ],[ 0.02530346], [-1.42615273]]), 
  array([[-0.01434251], [-0.04721317],[-1.42809882]]), 
  array([[-0.13343727], [-0.06032883],[-1.38794802]]), 
  array([[-0.05500072], [ 0.01296218],[ 1.56714706]]), 
  array([[-0.06118106],[ 0.05681526],[ 1.456494  ]]), 
  array([[-0.04264622],[ 0.02104299],[ 1.45450974]]), 
  array([[-0.03200693], [-0.05149377],[-1.56555323]])]

外部参数（平移向量）的输出较多，为方便复制的输出结果如下：

translation vectors:
 [array([[-0.09091744], [ 0.082059  ], [ 0.39976627]]), 
  array([[-0.06494083],[ 0.06902907],[ 0.36145401]]), 
  array([[ 0.02344282],[-0.10982814], [ 0.40461869]]), 
  array([[-0.09944981], [ 0.06591263],[ 0.44676297]]), 
  array([[-0.08148695],[ 0.07102436],[ 0.30257503]]), 
  array([[-0.04677319],[-0.08328905], [ 0.37345418]]), 
  array([[-0.09775231],[-0.0598347 ], [ 0.35787261]]), 
  array([[-0.10861916], [ 0.06690967], [ 0.41883281]]), 
  array([[ 0.01580203],[-0.1134467 ], [ 0.3564163 ]]), 
  array([[-0.12227016],[-0.04945632], [ 0.41903326]]), 
  array([[-0.0691557 ], [ 0.11330688], [ 0.35711506]]), 
  array([[-0.0708636 ], [ 0.08392852],[ 0.312731  ]]), 
  array([[-0.10549313], [ 0.05052104], [ 0.450821  ]]), 
  array([[ 0.07601946], [-0.10353966],[ 0.29816626]]), 
  array([[ 0.08873036],[-0.14247998],[ 0.45183176]]), 
  array([[ 0.06259065],[-0.10878105], [ 0.30669367]]),
  array([[-0.07005296], [ 0.09441181], [ 0.34586461]])]

6.4 外部参数可视化结果

针对相机参数的外部参数的可视化可以通过Matlab实现。
1.以棋盘格为中心

2.以相机为中心

7. 实验遇到的问题

实验运行结束时观察输出的相机参数，可以看到内部参数使用科学计数法输出，观察起来较为不便，如图所示：

所以考虑取消科学计数法，即代码中加入如下语句即可取消科学计数法输出：

import numpy as np
np.set_printoptions(suppress=True)