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

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

注意，这里的外参是相对于标定板的2D外参，如果要求解相机相当于载体坐标系的外参，则可以将标定板与地面平行或垂直放置，通过测量求出标定板与载体坐标系的变换关系，进而将相机相对于标定板的外参转换为相对于载体坐标系的外参。

而若要直接求解相对于世界坐标系(或载体坐标系)3D外参，而不做两步转换，请参考这篇博客：
【PnP】详细公式推导，使用DLT直接线性变换法求解相机外参

---

🌔01
坐标系变换关系

设世界坐标系$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]$

---

🌔02
求解单应变换

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

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

---

🌔03
求解相机内参

由$\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}{H}_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$

---

🌔04
求解相机外参

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

由$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]$

---

🌔05
畸变的公式

径向畸变是由透镜自身引起的，不可忽略，而切向畸变一般是由安装引起的，常可以忽略。这里两种畸变都予以考虑。

在归一化相机坐标系(通过尺度变换，使得$Z_C=1$)中：

对于径向畸变，有以下畸变公式，其中$X_C^{\prime },Y_C^{\prime }$为带畸变的坐标，$X_C,Y_C$为真实坐标，$r^2=X_C^2+Y_C^2$：

$\{\begin{array}{l}{X}_C^{\prime }=X_C(1+k_1{r}^2+k_2{r}^4+k_3{r}^6),\\ Y_C^{\prime }=Y_C(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\end{array}$

如果在径向畸变的基础上考虑切向畸变，有以下畸变公式：

$\{\begin{array}{l}{X}_C^{\prime }=X_C(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+[2p_1{X}_C{Y}_C+p_2(r^2+2X_C^2)],\\ Y_C^{\prime }=Y_C(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+[2p_2{X}_C{Y}_C+p_1(r^2+2Y_C^2)]\end{array}$

在联合迭代优化开始前，可以先将畸变系数任意设置一个合理的初值。

---

🌔06
联合优化

⭐️注意在前面几个小节中求取的内参和外参都是初始值，接下来会与畸变系数联合优化求解。
即联合优化求解参数：$param=\left[\begin{array}{c}K,R_i,T_i,k_1,k_2,k_3,p_1,p_2\end{array}\right]$

设以下符号表示：

• 角点像素坐标的观测值$m$为$(u,v)$，与之对应的世界坐标系坐标$M$为$(X_W,Y_W)$；

• 图像的个数为$n$，每张图像的角点数为$m$；

• ${\hat{m}}_{ij}=g(param,M_{ij})$为在考虑畸变时，用第$i$张图像的第$j$个角点的世界坐标经重投影函数$g$后得到的像素坐标估计值${\hat{m}}_{ij}$​.

则可以通过求解最小化估计值与观测值的误差这一优化问题(可使用Levenberg-Marquardt算法)，求得参数$param$：

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

---

🌔07
标定代码

以下是封装好重要函数的相机标定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'../data'))
    draw = False
    for i in range(1, img_num+1):
        img_path = '../data/%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

---

🌔08
注意事项

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