【相机标定】相机标定中的坐标变换，内外参求解，畸变校正，标定代码

本文循序渐进，从坐标系变换、求解单应$H$、求解内参$K$、求解外参$\left[\begin{array}{ccc}{R}_1& R_2& T\end{array}\right]$、径向畸变校正$\left[\begin{array}{ccc}{k}_1& k_2& k_3\end{array}\right]$、标定代码的6个方面概括性的总结了传统相机校正的理论推导和工程应用，文章比较精炼，适合备为复习和应用参考。

1 坐标系变换

设世界坐标系$X_W$，相机坐标系$X_C$，图像物理坐标系$x$，图像像素坐标系$u$.

则世界坐标系与相机坐标系有以下变换关系：

$\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\times 3}& T_{3\times 1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]$

相机坐标系与图像物理坐标系有以下变换关系，$f$为物理焦距：

$Z_C\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{X}_C\\ Y_C\\ Z_C\\ 1\end{array}\right]$

图像物理坐标系与图像像素坐标系有以下变换关系，$a$为像元尺寸，$(u_0,v_0)$为像平面中心像素坐标：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{a_x}& 0& u_0\\ 0& \frac{1}{a_y}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$

综合起来，世界坐标系与图像像素坐标系有以下变换关系（像素焦距$F_x=\frac{f}{a_x},F_y=\frac{f}{a_y}$）：

$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{F}_x& 0& u_0& 0\\ 0& F_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}{R}_{3\times 3}& T_{3\times 1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]$​

简化后有：

$Z_C\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{F}_x& 0& u_0\\ 0& F_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_{3\times 3}& T_{3\times 1}\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]=K\left[\begin{array}{cc}{R}_{3\times 3}& T_{3\times 1}\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ Z_W\\ 1\end{array}\right]$

当$Z_W\equiv 0$时（e.g. 对象是标定板），有以下单应变换：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z_C}K\left[\begin{array}{ccc}{R}_1& R_2& T\end{array}\right]\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]=\left[\begin{array}{ccc}{H}_1& H_2& H_3\end{array}\right]\left[\begin{array}{c}{X}_W\\ Y_W\\ 1\end{array}\right]$

2 求解单应$H$

其中$H$有9个元素，但其中一个元素作为齐次参数(令$h_{33}$=1)，固只需要求解其中的8个未知参数,

也就说，单张图片至少要检测4个角点即获取4对$(u,v,1)-(X_W,Y_W,1)$的关系式得到8个方程来求解$H$的数值.

3 求解内参$K$

由$\frac{1}{Z_C}K\left[\begin{array}{ccc}{R}_1& R_2& T\end{array}\right]=\left[\begin{array}{ccc}{H}_1& H_2& H_3\end{array}\right]$有：

$\{\begin{array}{l}{R}_1=Z_C{K}^{-1},\\ R_2=Z_C{K}^{-1}{H}_2\end{array}$

由$R_1^T{R}_2=0$和$R_1^T{R}_1=R_2^T{R}_2$有：

$\{\begin{array}{l}{H}_1^T{K}^{-T}{K}^{-1}{H}_2=0,\\ H_1^T{K}^{-T}{K}^{-1}{H}_1=H_2^T{K}^{-T}{K}^{-1}{H}_2\end{array}$

因为$K$中有4个未知参数，则至少需要2张图片，每张图片提供一组上述的两个方程，构成4个方程，此时将构造出方程$Ax=b$，$x$为4个未知参数构成的向量($x$等效为内参$K$，同时$A,b$表示一些常量).

使用最小二乘法最小化方程平方误差，由此可以求得$K$：

$\mathbf{min}||Ax-b|{|}^2\Rightarrow$

$A^T(Ax-b)=0\Rightarrow$

$x=(A^{T}A{)}^{-1}{A}^{T}b$

4 求解外参$\left[\begin{array}{ccc}{R}_1& R_2& T\end{array}\right]$

对于每一张图片，其外参都不一样.

由$R_1^T{R}_1=1$有：

$Z_C^2{H}_1^T{K}^{-T}{K}^{-1}{H}_1=1$

由此可以解有：

$Z_C=\frac{1}{||K^{-1}{H}_1||}$

由此有：

$\left[\begin{array}{ccc}{R}_1& R_2& T\end{array}\right]=Z_C{K}^{-1}\left[\begin{array}{ccc}{H}_1& H_2& H_3\end{array}\right]$

5 径向畸变的校正

径向畸变是由透镜自身引起的，不可忽略，而切向畸变一般是由安装引起的，可以忽略。

对于径向畸变，有以下校正公式，$r$为像素到像素中心的距离：

$\{\begin{array}{l}{u}_{corr}=u(1+k_1{r}^2+k_2{r}^4+k_3{r}^6),\\ v_{corr}=v(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\end{array}$

对于切向畸变，有以下校正公式，$r$为像素到像素中心的距离：

$\{\begin{array}{l}{u}_{corr}=u+[2p_{1}uv+p_2(r^2+2u^2)],\\ v_{corr}=v+[2p_{2}uv+p_1(r^2+2v^2)]\end{array}$

设以下符号表示：

• 图像像素坐标系点坐标：$m$为$(u,v)$, 世界坐标系点坐标：$M$为$(X_W,Y_W)$；
• 图像的个数为$n$，每张图像的角点数为$m$；
• ${\hat{m}}_{ij}=f(K,R_i,T_i,M_{ij})$为在不考虑径向畸变时，用第$i$张图像的第$j$个角点的世界坐标经重投影函数$f$后得到的像素坐标估计${\hat{m}}_{ij}$；
• ${\hat{m}}_{ij}=g(K,R_i,T_i,k_1,k_2,k_3,M_{ij})$为在考虑径向畸变时，用第$i$张图像的第$j$个角点的世界坐标经重投影函数$g$后得到的像素坐标估计${\hat{m}}_{ij}$​.

则可以通过最小二乘的方法求得畸变参数$k_1,k_2,k_3$：

$\mathbf{min}{\sum }_{i=1}^n{\sum }_{j=1}^m||m_{ij}-{\hat{m}}_{ij}|{|}^2$​

6 标定代码

以下是封装好重要函数的相机标定Python程序和注释解释。

#重要的函数
#1 ok, corners = detect_chessboard(img, rows, cols)  角点检测
#2 cv2.drawChessboardCorners(img, (cols, rows), corners, ok) 画角点
#3 ok, camera_mat, dist_coeff, rvecs, tvecs = cv2.calibrateCamera( 标定
#4      object_points, img_points, img_wh, camera_mat, dist_coeff,
#       criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 100, 1e-6))
#5 proj_points = cv2.projectPoints(object_points[i], rvecs[i], tvecs[i], camera_mat, dist_coeff)[0] 重投影
import os
import cv2
import numpy as np
np.set_printoptions(precision=4, suppress=True) #关闭科学计数法，便于显示


def image_from_camera():
    # 从摄像头中读取图像并保存（按键盘s键, 按ESC键退出程序）
    cap = cv2.VideoCapture(0)
    assert cap.isOpened(), 'Fail to open camera!'

    # 指定一个目录用于存放摄像头中截取的图像
    save_dir = '../data'
    # 图像文件命名范式: 0001.jpg, 0002.jpg, ...
    name_format = '%04d.jpg'
    name_idx = 1

    while True:
        ok, img = cap.read()
        assert ok, 'Fail to read camera!'
        cv2.imshow('1', img)
        key = cv2.waitKey(1) #以1ms为限制等待
        if key == -1:  # 什么键也没有按下
            continue
        if (key == 27):  # ESC键
            break
        if chr(key) == 's':  # s 键
            name = os.path.join(save_dir, name_format % name_idx)
            name_idx += 1
            cv2.imwrite(name, img)
            print('Saved image to \'%s\'', name)
    return

def detect_chessboard(img, rows=12, cols=6, subpix_sz=5):
    """
    从棋盘格图像中检测棋盘格角点
    :param img: 棋盘格图像，BGR或gray
    :param rows: 角点行数（不是方块的个数, 不算边缘角点） height
    :param cols: 角点列数  width
    :param subpix_sz: 对棋盘格角点坐标优化时的局部搜索范围，应不大于图像中最小方块的边长，单位是像素
    :return: 检测的角点，shape=(rows*cols,1,2)。如果实际检测的角点数目小于rows*cols则报错。
    corners (num,1,2)
    """

    ok, corners = cv2.findChessboardCorners(
        img, (cols, rows),
        cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK)

    if ok:
        #亚像素级优化(GRAY)，增加其准确性
        #zeroZone设置排除的区域，设置为(-1,-1)表示全部区域都检测
        #criteria迭代终止条件(次数或精度)
        corners = cv2.cornerSubPix(
            img if len(img.shape) == 2 else cv2.cvtColor(img, cv2.COLOR_BGR2GRAY),
            corners, (subpix_sz, subpix_sz), (-1, -1),
            (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01))

    return ok, corners


if __name__ == '__main__':
    # 第一步，用摄像头采集一些棋盘格图片
    # image_from_camera()

    # 第二步，读取每张图片，进行角点检测
    imgs = []  # 所选取的图像
    img_paths = []  # 对应的路径
    img_points = []  #9,72,1,2# 从图像中检测的角点
    cols, rows = 12, 6
    img_num = len(os.listdir(r'../data1'))
    draw = False
    for i in range(1, img_num+1):
        img_path = '../data1/%04d.jpg' % i
        img = cv2.imread(img_path) #BGR或GRAY格式
        assert img is not None, 'Fail to read image: ' + img_path
        ok, corners = detect_chessboard(img, rows, cols)
        if not ok:
            cv2.putText(img, 'Fail to detect corners!', org=(50, 200),
                        fontFace=cv2.FONT_HERSHEY_PLAIN , fontScale=4, color=(255, 255, 0), thickness=2)
            cv2.imshow('chessboard-corners-%04d'%i, img)
            cv2.waitKey()
            cv2.destroyWindow('chessboard-corners-%04d'%i)
        else:
            if draw:
                cv2.drawChessboardCorners(img, (cols, rows), corners, ok)
                cv2.imshow('chessboard-corners-%04d'%i, img)
                cv2.waitKey()
                cv2.destroyWindow('chessboard-corners-%04d'%i)
            imgs.append(img)
            img_paths.append(img_path)
            img_points.append(corners)

    nimgs = len(imgs)
    img_wh = img.shape[1::-1]  # (W,H)图像的宽以及高，所有图片必须具有相同尺寸，标定时要用到
    print('Got {} images for camera calibration.'.format(nimgs))

    # 第三步，进行相机标定
    # 先构造世界坐标系下的角点坐标，角点应该进行正确排序：先是第一行第一列，然后第一行第二列，...
    object_points = np.zeros((rows * cols, 1, 3), dtype='float32')
    square_sz = 138  #mm    any constant (edge length of eac chess board square, in millimeters or meters)
    for r in range(rows):
        for c in range(cols):
            object_points[r * cols + c, 0, 0] = c * square_sz  # world coordinate - X 行方向
            object_points[r * cols + c, 0, 1] = r * square_sz  # world coordinate - Y 列方向
    object_points = [object_points] * nimgs #9,72,1,3
    # 标定
    camera_mat = np.eye(3, 3)  # 如果有初始相机参数，可以在这里进行设置，将提高标定准确率
    dist_coeff = np.zeros((5, 1))  # 畸变参数k1 k2 k3 p1 p2
    ok, camera_mat, dist_coeff, rvecs, tvecs = cv2.calibrateCamera(
        object_points, img_points, img_wh, camera_mat, dist_coeff,
        criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 100, 1e-6))
    #R (9,3,1) T (9,3,1)
    # 每个旋转向量 rvec 的尺寸为 (3, 1)，表示一个三维向量。这个向量通常是以列向量的形式表示的，包含三个元素，分别对应于绕 x、y、z 轴的旋转角度（以弧度为单位）
    assert ok and cv2.checkRange(camera_mat) and cv2.checkRange(dist_coeff), 'Calibration failed!' #checkRange预测是否在有效范围之内

    # 保存标定结果
    print('camera_mat:\n', camera_mat)
    print('dist_coeff:\n', dist_coeff)
    np.save(r'../output/camera_mat', camera_mat)
    np.save(r'../output/dist_coeff', dist_coeff)
    print('Saved calibration results to: camera_mat.npy and dist_coeff.npy')

    # 第四步，做一些简单的验证

    # 计算标定误差：将物理点投影到图像空间中，并与对应的图像点进行比较，计算重投影误差
    for i in range(nimgs):
        proj_points = cv2.projectPoints(object_points[i], rvecs[i], tvecs[i], camera_mat, dist_coeff)[0]  #第二个返回时雅可比矩阵，一般用不上
        average_err = cv2.norm(proj_points, img_points[i], cv2.NORM_L2) / len(img_points[i])  #单位mm
        print('Reprojection error for image \'%s\': %g' % (img_paths[i], average_err))

    # 对相机畸变进行补偿，得到去畸变的图像
    for i in range(nimgs):
        img_undistort = cv2.undistort(imgs[i], camera_mat, dist_coeff)  #校正
        cv2.imshow('original', imgs[i])
        cv2.imshow('undistort', img_undistort)
        if cv2.waitKey() == 27:
            break

7 关于标定过程注意事项

• 标定板选用长宽不一样的，以免角点检测失误；
• 拍摄角度尽量不要使横纵方向完全颠倒；
• 标定过程中保证相机的光圈和焦距参数不变；
• 图片应为10-25张。