相机标定原理总结和实现

相机标定就是世界坐标到像素坐标的映射，这个世界坐标是我们人为去定义的，标定就是已知标定控制点的世界坐标和像素坐标我们去解算这个映射关系，一旦这个关系解算出来了我们就可以由点的像素坐标去反推它的世界坐标。

相机原理

相机成像涉及到一个复杂的光学和电子系统。以手机拍照为例，当打开手机准备拍照，镜头（Lens）会首先把被摄景物投影在图像传感器（Sensor）上，与此同时，影像处理器（ISP）会通过测光、测距算出合适的参数并指示镜头对焦，随着你按下拍照键，图像传感器（Sensor）会完成一次曝光，并通过影像处理器（ISP）变成图片，再经手机应用的后期处理，最终呈现在屏幕上，这就是消费者看到的JPG图像。

camera

整个成像系统的主要部件：

镜头：
镜头(Lenses)是将拍摄景物在传感器上成像的器件，它通常由几片透镜、光圈叶片、对焦马达等光学元件组成。从材质上看，摄像头的镜头可分为塑胶透镜和玻璃透镜。目前很多镜头都自带了自动对焦系统(AF)，通过与相机卡口的电子对接来实现对目标的自动对焦。

传感器：
传感器(Sensor)是摄像头组成的核心，其作用是作为相机的感光元件。摄像头传感器主要有两种，一种是CCD传感器，一种是CMOS传感器，两者区别在于：CCD的优势在于成像质量好，但是由于制造工艺复杂，成本居高不下，特别是大型CCD，价格非常高昂。在相同分辨率下，CMOS价格比CCD便宜，但是CMOS器件产生的图像质量相比CCD来说要低一些。

cmos

CMOS的原理是通过光电感应原理将光信号转换为电信号。当光子通过镜头轰击在传感器，传感器能感应到光照强度的大小，这意味只能是获取黑白(0,1)照片。为了将黑白图像转换为彩色图像，相机传感器上还设置了Bayer发明的颜色滤波矩阵，只让相应颜色波长的光子通过，仿照了人眼对于颜色的特殊模式要求，到此即形成了不同模式的RAW图（即原始未经加工过的图，单反相机基本都支持RAW格式保存，其本质上并不是图片，而是一组数据）。

影像处理器：
ISP(Image Signal Processor)在相机成像的整个环节中负责接收感光元件的原始信号数据，然后对传感器输入的信号进行运算处理，最终得出经过线性纠正、噪点去除、坏点修补、颜色插值、白平衡校正、曝光校正等处理后的结果。ISP芯片能够在很大程度上决定手机相机最终的成像质量，通常它对图像质量的改善空间可达10%-15%。

相机参数

相机的参数分为内参和外参：相机内参数是与相机自身特性相关的参数，比如相机的焦距、像素大小等；相机外参数是在世界坐标系中的参数，比如相机的位置、旋转方向等。

相机的内参由相机的物理结构决定，分别为焦点、光心和畸变系数。

焦点：在物理学上指平行光线经透镜折射或曲面镜反射后的会聚点。平行光线经凸透镜折射(或凹面镜反射)后各折射线(或反射线)会聚之点叫做实焦点；经凹透镜折射(或凸面镜反射)后各折射线(或反射线)发散而不会聚于一点，这时朝反方向延长的交点叫做虚焦点。平行于主轴的平行光线经折射(或反射)后的相交点必在主轴上，在主轴上的焦点叫做主焦点。

光心：每个透镜主轴上都有一个特殊点，凡是通过该点的光，其传播方向不变，这个点叫光心。

畸变：真实的镜头还会有径向和切向畸变，透镜形状引起的畸变叫做径向畸变，透镜安装与成像平面不平行引起的畸变叫做切向畸变。

径向畸变主要分为桶形畸变和枕型畸变。在针孔模型中，一条直线投影到像素平面上还是一条直线。但在实际中，相机的透镜往往使得真实环境中的一条直线在图片中变成了曲线，越靠近图像的边缘现象越明显，由于透镜往往是中心对称的，这使得不规则畸变通常径向对称；

在相机的组装过程中由于透镜和CMOS或者CCD的安装位置误差，不能使透镜严格和成像平面平行会引入切向畸变。如果存在切向畸变，一个矩形被投影到成像平面上时，很可能会变成一个梯形。

distortion1

distortion2

相机的外参用于描述相机在静态场景下相机的运动，或者在相机固定时，运动物体的刚性运动。外参包括旋转矩阵和平移矩阵，旋转矩阵和平移矩阵共同描述了如何把点从世界坐标系转换到摄像机坐标系。旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向，平移矩阵描述了在摄像机坐标系下空间原点的位置。

相机成像的四个坐标系

在处理相机图像成像时常常涉及到四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。

世界坐标系(world coordinate system)：用户定义的三维世界的坐标系，为了描述目标物在真实世界里的位置而被引入。单位为m。

相机坐标系(camera coordinate system)：在相机上建立的坐标系，为了从相机的角度描述物体位置而定义，作为沟通世界坐标系和图像/像素坐标系的中间一环。单位为m。

图像坐标系(image coordinate system)：为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标。单位为m。

像素坐标系(pixel coordinate system)：为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系。单位为个（像素数目）。

其中，相机坐标系的z轴与光轴重合，且垂直于图像坐标系平面并通过图像坐标系的原点，相机坐标系与图像坐标系之间的距离为焦距f（也即焦点点到光心的距离）。像素坐标系平面u-v和图像坐标系平面x-y重合，但像素坐标系原点位于图中左上角（之所以这么定义，目的是从存储信息的首地址开始读写）。

coord

从世界坐标系到相机坐标系

构建世界坐标系的目的是为了更好的描述相机的位置在哪里，在双目视觉中一般将世界坐标系原点定在左相机或者右相机或者二者X轴方向的中点。从世界坐标系到相机坐标系的变化为刚体变化，即物体不会发生形变，只需要进行旋转和平移。我们将从世界坐标系到相机坐标系涉及到旋转和平移动作通过一个旋转矩阵和平移向量组合成一个齐次坐标的变换矩阵。

仿射变换中绕z轴旋转的变换公式如下所示：

$f(x) = \left\{ \begin{array}{lr} x = x'cosθ - y'sinθ \\ y = x'sinθ + y'cosθ \\ z = z' \end{array} \right.$

转换为矩阵计算的形式如下：
$\begin{bmatrix} x \\ y \\ z \end{bmatrix} =\begin{bmatrix} cosθ & -sinθ & 0 \\ sinθ & cosθ & 0 \\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = R_z * \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix}$

这里 $R_z$ 就是是绕z轴的旋转矩阵，同理我们可以获得绕x,y轴的旋转矩阵 $R_x$ , $R_y$ 。由此可得在三维空间的旋转矩阵 $R= R_x*R_y*R_z$ 。

平移向量 $t$ 是一个对应x,y,z的(3,1)的向量，其符号表明了在对应坐标上的移动方向。

由此得到世界坐标系到相机坐标系的变化如下所示：

$\begin{bmatrix} x_c \\ y_c \\ z_c \end{bmatrix} = R * \begin{bmatrix} x_w \\ y_w \\ z_w \end{bmatrix} + t = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix}$

这里 $\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$ 就是相机的外参矩阵，之所称之为外参矩阵可以理解为只与相机外部参数有关，且外参矩阵随刚体位置的变化而变化。

从相机坐标系到理想图像坐标系（不考虑畸变）

这一过程进行了从三维坐标到二维坐标的转换，也即投影透视过程（用中心投影法将物体投射到投影面上，从而获得的一种较为接近视觉效果的单面投影图，也就是使我们人眼看到景物近大远小的一种成像方式）。

从相机坐标系到理想图像坐标系（无畸变）的变换可由相似三角形原则计算得出：

$△ABO_c \sim △oCO_c , △PBO_c \sim △pCO_c$

$\frac{AB}{oC} = \frac{AO_c}{oO_c} = \frac{PB}{pC} = \frac{X_c}{x} = \frac{Y_c}{y} = \frac{Z_c}{f}$

$x=\frac{X_c}{Z_c}*f, y=\frac{Y_c}{Z_c}*f$

$Z_c * \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = \begin{bmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} * \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix}$

其中f是焦距，属于相机的内参，一般我们都可以知道相机镜头对应的焦距。

从理想图像坐标系到实际图像坐标系（考虑畸变）

物体通过镜头投影到平面上时，受制于镜头的工艺问题，成像会产生畸变，主要就是我们上文提的径向畸变和切向畸变。

实际情况中常用r=0处的泰勒级数展开的前几项来近似描述径向畸变。矫正径向畸变前后的坐标关系为：

$\left\{ \begin{array}{lr} x_{coor} = x_p(1+k_1r^2+k_2r^4+k_3r^6) \\ y_{coor} = y_p(1+k_1r^2+k_2r^4+k_3r^6) \end{array}\right.$

由此可知对于径向畸变，我们有3个畸变参数需要求解。

切向畸变需要两个额外的畸变参数来描述，矫正前后的坐标关系为：

$\left\{ \begin{array}{lr} x_{coor} = x_p+[2p_1x_py_p + p_2(r^2+2x_p^2)] \\ y_{coor} = y_p+ [p_1(r^2+2y_p^2) + 2p_2x_py_p] \end{array} \right.$

由此可知对于切向畸变，我们有2个畸变参数需要求解。

综上，我们一共需要5个畸变参数（k1、k2、k3、p1、p2 ）来描述透镜畸变。

从实际图像坐标系到像素坐标系

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫光心。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy表示每一列和每一行分别代表多少mm，即 $1pixel=dx mm$ 。

从实际图像坐标系到像素坐标系的变换可由下面的偏移公式计算得出：

$\left\{ \begin{array}{lr} u = \frac{x}{dx} +u_0 \\ v = \frac{y}{dy} +v_{0} \ \end{array}\right.$

转换为矩阵计算的形式如下：

$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} 1/dx & 0 & {u_0} \\ 0 & 1/dy & {v_0} \\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$

最终变换

根据四个坐标系的转换关系，在不考虑畸变的情况下，物体从世界坐标系投影到像素坐标系的过程如下：

$Z_c \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} 1/dx & 0 & {u_0} \\ 0 & 1/dy & {v_0} \\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} * \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix} = \begin{bmatrix} {f_x} & 0 & {u_0} & 0 \\ 0 & {f_y} & {v_0} & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} * \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix} = M * N * \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix}$

其中M是内参矩阵，N是外参矩阵。通过这个转换，一个三维中的坐标点，可以明确的在图像中找到一个对应的像素点。

单应矩阵

单应性（Homography）在计算机视觉领域是一个非常重要的概念。单应性变换在图像校正、图像拼接、相机位姿估计、视觉SLAM等领域有非常重要的作用。对应的变换矩阵称为单应性矩阵。本质上单应矩阵实际就是个透视变换矩阵，给定至少4个点，通过单应性矩阵可以将2D物体从一个平面投影到另外一个平面。相机标定中引入了单应性变换，可以简单的理解为它用来描述物体在世界坐标系和像素坐标系之间的位置映射关系。

相机中单应性矩阵定义为：

$H =s* \begin{bmatrix} {f_x} & 0 & {u_0} & 0 \\ 0 & {f_y} & {v_0} & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} * \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} * = s * M * N$

假设两张图像中的对应点对齐次坐标为(x',y',1)和(x,y,1)，那么这两个坐标的转换关系为：

$\begin{bmatrix} x' \\ y' \\ 1 \end{bmatrix} = H * \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} = \begin{bmatrix} h_{11} &h_{12} & h_{13} \\ h_{21} &h_{22} & h_{23} \\ h_{31} &h_{32} & h_{33} \end{bmatrix} * \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$

$\left\{ \begin{array}{lr} x' = \frac{h_{11}x + h_{12}y + h_{13}}{h_{31}x + h_{32}y + h_{33}} \\ y' = \frac{h_{21}x + h_{22}y + h_{23}}{h_{31}x + h_{32}y + h_{33}} \\ \end{array}\right.$

8自由度下H计算过程有两种方法:

第一种方法：直接设置 h33=1。

$\left\{ \begin{array}{lr} x' = \frac{h_{11}x + h_{12}y + h_{13}}{h_{31}x + h_{32}y + 1} \\ y' = \frac{h_{21}x + h_{22}y + h_{23}}{h_{31}x + h_{32}y + 1 }\\ \end{array}\right.$

第二种方法：将H添加约束条件，将H矩阵模变为1。

$h_{11}^2 + h_{12}^2 + h_{13}^2 + h_{21}^2 + h_{22}^2 + h_{23}^2 + h_{31}^2 + h_{32}^2 + h_{33}^2 =1$

通过这两个条件推导，8自由度的H至少需要4对对应的点才能计算出单应矩阵。在真实的应用场景中，计算的点对中都会包含噪声。比如点的位置偏差几个像素，甚至出现特征点对误匹配的现象，如果只使用4个点对来计算单应矩阵，那会出现很大的误差。因此，为了使得计算更精确，一般都会使用远大于4个点对来计算单应矩阵。另外上述方程组采用直接线性解法通常很难得到最优解，所以实际使用中一般会用其他优化方法，如奇异值分解、Levenberg-Marquarat（LM）算法等进行求解。

OpenCV实现相机标定

棋盘标定（张氏标定法）是在Opencv中常用的标定方式，只需要两个平面，黑白格子相交的角点就可以实现标定，其流程如下：

1、打印一张棋盘格标定图纸，将其贴在平面物体的表面。
2、拍摄一组不同方向棋盘格的图片，可以通过移动相机来实现，也可以移动标定图片来实现。
3、对于每张拍摄的棋盘图片，检测图片中所有棋盘格的特征点（角点，也就是下图中黑白棋盘交叉点，中间品红色的圆圈内就是一个角点）。我们定义打印的棋盘图纸位于世界坐标系Zw=0的平面上，世界坐标系的原点位于棋盘图纸的固定一角（比如下图中黄色点）。像素坐标系原点位于图片左上角。
4、因为棋盘标定图纸中所有角点的空间坐标是已知的，这些角点对应在拍摄的标定图片中的角点的像素坐标也是已知的，如果我们得到这样的N>=4个匹配点对（越多计算结果越鲁棒），就可以根据LM等优化方法得到其单应矩阵H。

具体实现：

用一张9x6的棋盘壁纸铺满电脑屏幕充当标定板。

board.jpg

相机使用Fujifile XT4，镜头使用XF 56mmF1.4，从不同角度拍摄棋盘的照片。

3.使用OpenCV的findChessboardCorners()函数找到每个棋盘的角点并将其可视化。棋盘中每个角点对应的世界坐标为(0,0,0), (1,0,0),...(8,5,0)。

使用OpenCV的calibrateCamera()函数求解相机的参数。

输入参数为：
- 世界坐标系内角点, 图像的角点，图像的尺寸，相机内参矩阵，畸变矩阵，旋转矩阵，平移矩阵，求解方式。
输出结果为：
- 像机的内参矩阵, 3x3；
- 畸变系数(k1, k2, p1, p2, [k3, [k4, k5, k6])， 4, 5, 或8, 具体尺寸取决于flags；
- 每一张图片的旋转向量，nx3x1；
- 每一张图片的平移向量，nx3x1；
  前两个结果是相机的内参，后两个结果是相机的外参。

如果需要去畸变，使用OpenCV的undistort()函数结合求解的内参可以获得去畸变的图片。
使用OpenCV的projectPoints()函数，可以通过上述求得的内参外参将一个世界坐标投影到图像坐标上。
如果需要单应矩阵，可以使用OpenCV的findHomography()函数求出相机的单应矩阵。

代码：

import cv2
import numpy as np
from glob import glob
import matplotlib.pyplot as plt


def show_img():
    for i, image_path in enumerate(glob('board_images/*g')):
        img = cv2.imread(image_path)

        plt.subplot(2, 2, i + 1)
        plt.imshow(img)

    plt.tight_layout()
    plt.show()


def get_corners():
    # 指定棋盘上所有角点的世界坐标(x,y,z)，如： (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    obj_point = np.zeros((6 * 9, 3), np.float32)
    obj_point[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)

    word_points = []
    img_points = []

    # 图像大小
    h, w = 0, 0

    for i, image_path in enumerate(glob('board_images/*g')):

        img = cv2.imread(image_path)
        h, w, _ = img.shape
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # 寻找角点
        ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

        if ret:
            word_points.append(obj_point)
            img_points.append(corners)

            # 可视化
            cv2.drawChessboardCorners(img, (9, 6), corners, ret)

            plt.subplot(2, 2, i + 1)
            plt.imshow(img)

    plt.tight_layout()
    plt.show()

    return np.array(word_points), np.array(img_points), (w, h)


if __name__ == '__main__':
    # show_img()

    # 获取角点
    word_points, img_points, img_size = get_corners()

    # 基于角点的世界坐标和图像坐标来求解单应矩阵
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(word_points, img_points, img_size, None, None)
    if ret:
        print('fx: {}, fy: {}, cx: {}, cy: {}'.format(mtx[0][0], mtx[1][1], mtx[0][2], mtx[1][2]))

    # 畸变校正
    # img = None
    # dst_img = cv2.undistort(img, mtx, dist, None, mtx)
    
    # 3D->2D投影误差计算
    for i in range(len(word_points)):
        re_project_img_points = cv2.projectPoints(word_points[i], rvecs[i], tvecs[i], mtx, dist)
        err = np.linalg.norm(img_points[i] - re_project_img_points[0])
        print('project {} error: {}'.format(i, err))
    
    # 获得单应矩阵H
    word_points = word_points.reshape(-1, 3)
    img_points = img_points.reshape(-1, 2)
    H, _ = cv2.findHomography(word_points, img_points)
    print(H)

    test_word_p = np.array([5, 5, 0]).reshape(3, 1)
    print(H.dot(test_word_p))