1.针孔相机模型

针孔相机成像原理其实就是利用投影将真实的三维世界坐标转换到二维的相机坐标上去，其模型示意图如下图所示：

(X，Y，Z)为在世界坐标系下一点的物理坐标

( u , v ) 为该点对应的在像素坐标系下的像素坐标

引入齐次坐标的原因：引入齐次坐标的目的是为了升维，将坐标从二维坐标变为三维坐标。

2.相机成像过程

相机成像系统中，共包含四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。

（1）世界坐标系（ $Xw_{}$ , $Yw_{}$
描述目标在真实世界中的位置引入的参考坐标系。
（2）相机坐标系（ $Xc_{}$ , $Yc_{}$ , $Zc_{}$ ）
联系世界坐标系与图像坐标系的桥梁，一般取摄像机的光学轴为ｚ轴。
（3）图像坐标系（x,y）
根据投影关系引入，方便进一步得到像素坐标，单位为毫米，坐标原点为摄像机光轴与图像物理坐标系的交点位置。
（4）像素坐标系（u,v）
真正从相机内读到的信息，图像物理坐标的离散化，以像素为单位，坐标原点在左上角。

2.1 各个坐标系之间的转换

2.1.1 图像坐标系到像素坐标系

像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。

由于(u,v)只代表像素的列数与行数，而像素在图像中的位置并没有用物理单位表示出来，所以，我们还要建立以物理单位（如毫米）表示的图像坐标系（x,y）。将相机光轴与图像平面的交点（一般位于图像平面的中心处，也称为像主点（principal point）定义为该坐标系的原点 $O_1{}$ ，且x轴与u轴平行，y轴与v轴平行，假设( $u_0{}$ , $v_0{}$ )代表 $O_1{}$ 在(u,v)坐标系下的坐标，dx与dy分别表示每个像素在横轴x和纵轴y上的物理尺寸，则图像中的每个像素在(u,v)坐标系中的坐标和在（x,y）坐标系中的坐标之间都存在如下的关系：

$\begin{bmatrix} u\\v \\ 1 \end{bmatrix}=\begin{bmatrix} \frac{1}{dx} & 0&u_{0} \\ 0 & \frac{1}{dy} &v_{0} \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x\\ y \\1 \end{bmatrix}$

2.1.2 相机坐标系到图像坐标系

点P(Xc,Yc,Zc)由通过投影中心的光线投影到图像平面上,相应的图像点为p(x,y,f);根据相似三角形原理:

用线性矩阵描述为：

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

世界坐标系是为了描述相机的位置而被引入,上图中坐标系OwXwYwZw即为世界坐标系。平移向量t旋转矩阵R可以用来表示相机坐标系与世界坐标系的关系。所以，假设空间点P在世界坐标系下的齐次坐标是 $(Xw,Yw,Zw,1)^{T}$ ,在相机坐标下的齐次坐标是 $(Xc,Yc,Zc,1)^{T}$ ,则存在如下关系:

$\begin{bmatrix} X_{c}\\ Y_{c} \\Z_{c} \\ 1 \end{bmatrix}=\begin{bmatrix} R & t\\ 0^{T} & 1 \end{bmatrix}\begin{bmatrix} X_{w}\\Y_{w} \\Z _{w} \\ 1 \end{bmatrix}$

其中 R为3×3的旋转矩阵,t为3×1的平移矢量。

2.1.4世界坐标系到像素坐标系

世界坐标系到像素坐标系的变换可由上面的公式组合而成：

$M_{2}=\begin{bmatrix} R & t\\ 0^{T} & 1 \end{bmatrix}\begin{bmatrix} X_{w}\\Y_{w} \\Z _{w} \\ 1 \end{bmatrix}$

其中M1称为相机的内部参数矩阵,M2称为相机的外部参数矩阵,M称为投影矩阵 。

3.畸变与畸变矫正

3.1 畸变

在世界坐标中的一条直线上的点在相机上只呈现出了一个点，其中发生了非常大的变化，同时也损失和很多重要的信息，这正是我们3D重建、目标检测与识别领域的重点和难点。实际中，镜头并非理想的透视成像，带有不同程度的畸变。理论上镜头的畸变包括径向畸变和切向畸变，切向畸变影响较小，通常只考虑径向畸变。

径向畸变：径向畸变主要由镜头径向曲率产生（光线在远离透镜中心的地方比靠近中心的地方更加弯曲）。导致真实成像点向内或向外偏离理想成像点。其中畸变像点相对于理想像点沿径向向外偏移，远离中心的，称为枕形畸变；径向畸点相对于理想点沿径向向中心靠拢，称为桶状畸变。

桶状畸变：

枕形畸变：

3.2 畸变公式

径向畸变公式：

切向畸变公式：

其中，（ $x,y$ ），（ $\widehat{x},\widehat{y}$ ）分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标， $r$ 为图像像素点到图像中心点的距离，即 $r^{2}=x^{2}+y^{2}$

相机标定的第二个目的就是获得相机的畸变参数，如上式中的 $k_{1},k_{2},k_{3},p_{1},p_{2}$

4.相机标定原理

针对针孔相机模型，只要内参矩阵和外参矩阵就可以唯一的确定相机模型。这个过程就称为相机标定。相机标定的目的是为了获得相机的内参矩阵和外参矩阵。相机标定的内参主要包括焦距、像主点坐标、畸变参数。

通过世界坐标集( $X_{i}$ , $Y_{i}$ , $Z_{i}$ )，以及它们在图像平面上的投影坐标集( $u_{i}$ , $v_{i}$ )，计算相机投影矩阵M中的 11个未知参数。

相机模型：

线性方法：

非线性方法：

5.张正友标定法介绍

张正友标定法利用如下图所示的棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标( u , v ) 。

张正友标定法将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标W = 0 ，由于标定板的世界坐标系是人为事先定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到每一个角点在世界坐标系下的物理坐标( U , V , W = 0 ) 。

我们将利用这些信息：每一个角点的像素坐标( u , v ) 、每一个角点在世界坐标系下的物理坐标( U , V , W = 0 )，来进行相机的标定，获得相机的内外参矩阵、畸变参数。

5.1张正友标定法的整体流程

5.2 张正友标定法的模型

5.3 模型求解

张正友标定法标定相机的内外参数的思路如下：

1）、求解内参矩阵与外参矩阵的积；

2）、求解内参矩阵；

3）、求解外参矩阵。

1）求解内参与外参的积

其中，H为一个3*3的矩阵，并且有一个元素作为齐次坐标。因此，H有8个自由度。现在有8个自由度需要求解，所以需要四个对应点。也就是四个点就可以求出图像平面到世界平面的单应性矩阵H。通过4个点，我们就可以可以获得单应性矩阵H。但是，H是内参阵和外参阵的合体。

由于 $h_1$ 和 $h_2$ 是通过单应性求解出来的，所以我们要求解的参数就变成K矩阵中未知的5个参数。我们可以通过三个单应性矩阵来求解这5个参数，利用三个单应性矩阵在两个约束下可以生成6个方程。其中，三个单应性矩阵可以通过三张对同一标定板不同角度和高度的照片获得。

2）求解内参

当观测图像大于或等于三幅图像时，就可以得到b的唯一解，应用上述公式我们就可以估算出B了。

得到B后，我们通过cholesky分解，就可以得到摄相机机的内参阵A的六个自由度，即

3）求解外参

根据 $\begin{bmatrix} h_{1} & h_{2} &h_{3} \end{bmatrix}=\lambda K\begin{bmatrix} r_{1} & r_{2} &t \end{bmatrix}$ 化简可得外部参数，即：

6.相机标定的步骤

（1）准备一个张正友标定法的棋盘格，棋盘格大小已知，用相机对其进行不同角度的拍摄，得到一组图像；

（2）对图像中的特征点如标定板角点进行检测，得到标定板角点的像素坐标值，根据已知的棋盘格大小和世界坐标系原点，计算得到标定板角点的物理坐标值；

（3）求解内参矩阵与外参矩阵；

（4）求解畸变系数；

（5）利用L-M（Levenberg-Marquardt）算法对上述参数进行优化。

7.源代码

import cv2
import glob
import numpy as np

w = 9  # 内角点个数，内角点是和其他格子连着的点
h = 6

objp = np.zeros((w * h, 3), np.float32)#一个9*6行3列的矩阵
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)# 储存棋盘格角点的世界坐标和图像坐标对      reshape(-1, 2)-1表示不确定分几行，2表示分为2列
objp=2.6*objp #棋盘格实际大小
objpoints = []  # 在世界坐标系中的三维点
imgpoints = []  # 在图像平面的二维点

images = glob.glob('photo2/*.jpg')
# 取文件夹中所有图片
for fname in images:
    # 对每张图片，识别出角点，记录世界物体坐标和图像坐标
    img = cv2.imread(fname)#获取图像的水平方向和垂直方向的尺寸
    img = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)#立方插值法 缩放到原来的二分之一
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)  # 转灰度

    # 寻找角点，存入corners，ret是找到角点的flag
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

    # criteria:角点精准化迭代过程的终止条件
    criteria = (cv2.TERM_CRITERIA_MAX_ITER+ cv2.TERM_CRITERIA_EPS , 30, 0.001)#第一项表示迭代次数达到最大次数时停止迭代，第二项表示角点位置变化的最小值已经达到最小时停止迭代

    # 执行亚像素级角点检测
    corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # 输入图像、角点初始坐标、搜索窗口为2*winsize+1、第四个参数作用类似于winSize，但是总是具有较小的范围，通常忽略（即Size(-1, -1)）,  求角点的迭代终止条件
    objpoints.append(objp)# .append()向列表/数组添加元素
    imgpoints.append(corners2)

    # 在棋盘上绘制角点
    img = cv2.drawChessboardCorners(img, (9, 6), corners2, ret)
    cv2.namedWindow('img', cv2.WINDOW_NORMAL)
    cv2.imshow('img', img)
    cv2.waitKey(1000)

'''
传入所有图片各自角点的三维、二维坐标，相机标定。
每张图片都有自己的旋转和平移矩阵，但是相机内参和畸变系数只有一组。
mtx，相机内参；dist，畸变系数；rvecs，旋转矩阵；tvecs，平移矩阵。
'''
# 输入：世界坐标系里的位置 像素坐标 图像的像素尺寸大小 3*3矩阵，相机内参数矩阵 畸变矩阵
#使用cv2.calibrateCamera()进行标定，这个函数会返回标定结果、相机的内参数矩阵、畸变系数、旋转矩阵和平移向量
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
img = cv2.imread('photo2/5.jpg')

img = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)
h, w = img.shape[:2]

'''
使用cv.getOptimalNewCameraMatrix()优化内参数和畸变系数
参数1表示保留所有像素点，同时可能引入黑色像素，并返回一个ROI用于将其剪裁掉
设为0表示尽可能裁剪不想要的像素，这是个scale，0-1都可以取。
'''

newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# 纠正畸变
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)

# 输出纠正畸变以后的图片
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('result.png', dst)
# 输出：标定结果 相机的内参数矩阵 畸变系数 旋转矩阵 平移向量
mtx_new=mtx.tolist()
print("相机内参矩阵:\n", mtx_new)#[fx,s,x0;0,fy,y0;0,0,1],fx,fy为焦距，一般二者相等；x0、y0为主点坐标（相对于成像平面），s为坐标轴倾斜参数，理想情况下为0
print("newcameramtx:\n", newcameramtx)
print (("旋转向量rvecs:\n"),rvecs)    # 旋转向量  # 外参数，3个旋转参数
print (("平移向量tvecs:\n"),tvecs)    # 平移向量  # 外参数，3个平移参数
print("畸变系数dist:\n", dist)#5个畸变参数，径向畸变k1,k2,k3,切向畸变p1、p2

# 计算误差
# 反投影误差越接近0，说明结果越理想。
# 通过之前计算的内参数矩阵、畸变系数、旋转矩阵和平移向量，使用cv2.projectPoints()计算三维点到二维图像的投影，
# 然后计算反投影得到的点与图像上检测到的点的误差，最后计算一个对于所有标定图像的平均误差，这个值就是反投影误差。
tot_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    tot_error += error

print("total error: ", tot_error / len(objpoints))