python标定相机外参 python 相机标定

照相机标定

• 一、针孔照相机模型

• 针孔相机
• 坐标转换
• 畸变现象
• 畸变矫正
• 摄像机旋转平移 `Camera rotation and translation`

• 二、照相机标定

• 标定参数线性回归
• 最小二乘求解标定参数
• 张正友标定算法

• 基本参数变量
• 求解Homographic矩阵
• 计算内参数矩阵
• 极大似然估计
• 基本步骤

• 三、相机标定代码实现

• 3.1 运行图片集
• 3.2 运行结果

---

一、针孔照相机模型

针孔相机

针孔照相机模型 （有时称为射影照相机模型）是计算机视觉中广泛使用的照相机模型。针孔相机模型就是把相机简化成小孔成像，在这种模型下，物体的空间坐标和图像坐标之间是线性的关系，因此对相机参数的求解就归结到求解线性方程组上。而相机标定就是确定相机的内部参数和外部参数。

坐标转换

世界坐标系： 是客观三维世界的绝对坐标系，也称客观坐标系。因为数码相机安放在三维空间中，我们需要世界坐标系这个基准坐标系来描述数码相机的位置，并且用它来描述安放在此三维环境中的其它任何物体的位置。
相机坐标系（光心坐标系）： 以相机的光心为坐标原点，X 轴和Y 轴分别平行于图像坐标系的 X 轴和Y 轴，相机的光轴为Z 轴。
图像坐标系： 其是以摄像机拍摄的二维照片为基准建立的坐标系。用于指定物体在照片中的位置。

  摄像机标定(Camera calibration)简单来说是从世界坐标系转换为相机坐标系，再由相机坐标系转换为图像坐标系的过程，也就是求最终的投影矩阵$P$的过程。

以上面两张坐标图为例：左边是相机坐标系，转为右边的图像坐标，是一个小孔成像的模型。

• $C$ 点表示camera centre，即相机的中心点，也是相机坐标系的中心点
• $Z$ 轴表示principal axis，即相机的主轴
• $p$ 点所在的平面表示image plane，即相机的像平面，也就是图片坐标系所在的二维平面
• $p$ 点表示principal point，即主点，主轴与像平面相交的点
• $C$ 点到 $p$ 点的距离，也就是右边图中的 $f$ 表示focal length，即相机的焦距
• 像平面上的 $x$ 和 $y$ 坐标轴是与相机坐标系上的 $X$ 和 $Y$
• 相机坐标系是以 $X$，$Y$， $Z$（大写）三个轴组成的且原点在 $C$ 点，度量值为米（m）；
• 像平面坐标系是以 $x$，$y$（小写）两个轴组成的且原点在 $p$ 点，度量值为米（m）；
• 图像坐标系一般指图片相对坐标系，在这里可以认为和像平面坐标系在一个平面上，不过原点是在图片的角上，而且度量值为像素的个数（pixel）；

$x = \frac{fX}{Z}$$y = \frac{fY}{Z}$可以表示为矩阵计算为:$\begin{pmatrix} x \\ y \\ 1\end{pmatrix} \rightarrow \begin{pmatrix} fX \\ fY \\ Z\end{pmatrix} = \begin{bmatrix} f & 0 & 0& 0\\ 0 & f & 0 & 0\\ 0 & 0 & 1 & 0\end{bmatrix}\begin{pmatrix} X \\ Y \\ Z \\ 1\end{pmatrix}$或者$\begin{pmatrix} x \\ y \\ 1\end{pmatrix} \rightarrow \begin{pmatrix} fX \\ fY \\ Z\end{pmatrix} = \begin{bmatrix} f & 0 & 0\\ 0 & f & 0\\ 0 & 0 & f\end{bmatrix}\begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\end{bmatrix}\begin{pmatrix} X \\ Y \\ Z \\ 1\end{pmatrix}$

像主点偏移

在上面的基础上加了一个p点坐标的偏移量，同时可以表示为矩阵计算为:$\begin{pmatrix} x \\ y \\ 1\end{pmatrix} \rightarrow \begin{pmatrix} fX \\ fY \\ Z\end{pmatrix} = \begin{bmatrix} f & 0 & x_{0}\\ 0 & f & y_{0}\\ 0 & 0 & f\end{bmatrix}\begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\end{bmatrix}\begin{pmatrix} X \\ Y \\ Z \\ 1\end{pmatrix}$

最后可以得到 $K$，也就是相机内参Intrinsic parameters:$K = \begin{bmatrix} f & & p_{x}\\ & f & p_{y}\\ 0 & 0 & 1\end{bmatrix}$$K = \begin{bmatrix} fa & s & x_{0}\\ 0 & f & y_{0}\\ 0 & 0 & 1\end{bmatrix}$

畸变现象

  摄像机校准一般采用小孔成像模型，理想的小孔模型是线性模型，但是由于存在镜头畸变等原因，线性模型通常要加上一些内部参数，变成非线性模型。

  相机的成像过程实质上是坐标系的转换。首先空间中的点由 “世界坐标系” 转换到 “像机坐标系”，然后再将其投影到成像平面 ( 图像物理坐标系 ) ，最后再将成像平面上的数据转换到 图像像素坐标系。但是由于透镜制造精度以及组装工艺的偏差会引入畸变，导致原始图像的失真。镜头的畸变分为径向畸变和切向畸变两类。

• 图像径向畸变：径向畸变是沿着透镜半径方向分布的畸变，产生原因是光线在原理透镜中心的地方比靠近中心的地方更加弯曲，这种畸变在普通廉价的镜头中表现更加明显，径向畸变主要包括桶形畸变和枕形畸变两种。
  – 透镜质量原因
  – 光线在远离透镜中心的地方比靠近中心的地方更加弯曲

	桶形畸变	枕形畸变

• 切向畸变：切向畸变是由于透镜本身与相机传感器平面（成像平面）或图像平面不平行而产生的，这种情况多是由于透镜被粘贴到镜头模组上的安装偏差导致。

畸变矫正

径向畸变可以用如下公式修正：$x_{corr} =x_{dis} (1+k_{1} r^{2} +k_{2} r^{4} +k_{3} r^{6} )$$y_{corr} =y_{dis} (1+k_{1} r^{2} +k_{2} r^{4} +k_{3} r^{6} )$

切向畸变是由于透镜与成像平面不严格的平行，其可以用如下公式修正：$x_{corr}=x_{dis} + [2p_1xy+p_2(r^2+2x^2)]$$y_{corr}=y_{dis} + [p_1(r^2+2y^2)+2p_2xy]$

其中：

• $x_{dis}$和$y_{dis}$表示有畸变的坐标；
• $x_{corr}$和$y_{corr}$表示修复后的坐标；
• $k_1,k_2,k_3$表示径向畸变参数；
• $p_1,p_2$表示切向畸变参数；

所以最终得到5个畸变参数：$D=( k_1, k_2, p_1 , p_2 , k_3)$

摄像机旋转平移 Camera rotation and translation

$P$要投影到像面上时，先要将该点的坐标转换到相机坐标系下。刚体从世界坐标系转换到相机坐标系的过程，可以通过旋转和平移来得到。因此相机的外部参数就包括了旋转、平移矩阵。

$\begin{pmatrix} x \\ y \\ 1\end{pmatrix} \rightarrow \begin{bmatrix} f & s & x_{0}\\ 0 & f & y_{0}\\ 0 & 0 & f\end{bmatrix} \begin{bmatrix} R|t\end{bmatrix}\begin{pmatrix} X \\ Y \\ Z \\ 1\end{pmatrix}$$x \rightarrow K[R|t]X$
其中 $[R|t]$

• R表示相机的旋转矩阵;
• t表示相机的位移矩阵；

相机在世界中的旋转包括三个方向，分别对应着 $X,Y,Z$

在上图中也用roll、yaw、pitch 分别表示 $X,Y,Z$ 。

$roll \begin{bmatrix} x^{$$yaw \begin{bmatrix} x^{$$pitch \begin{bmatrix} x^{$

将三个方向的矩阵旋转相乘，可得：$R = Rot(z,\phi)Rot(y,\theta)Rot(x,\psi)$$= \begin{bmatrix} C\phi & -S\phi & 0 \\ S\phi & C\phi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} C\theta & 0 & S\theta \\ 0 & 1 & 0 \\ -S\theta & 0 & C\theta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & C\psi & -S\psi \\ 0 & S\psi & C\psi \end{bmatrix}$$= \begin{bmatrix} C\phi C\theta & -S\phi S\psi + C\phi S\theta S\psi & C\phi S\theta C\psi + S\phi S\psi \\ S\phi C\theta & C\phi S\theta S\psi + C\phi S\psi & -C\phi S\psi + S\phi S\theta C\psi \\ -S\theta & -C\theta S\psi & C\theta C\psi \end{bmatrix}$

两类参数

• 相机内部参数/内方位元素: 焦距、像主点坐标、畸变参数
• 相机外部参数/外方位元素: 旋转、平移

二、照相机标定

$X_i$ 和对应的图像坐标二维点 $x_i$，这些三维点到二维点的转换都可以通过上面提到的相机内参 $K$ ，相机外参 $R$ 和 $t$，以及畸变参数 $D$ 经过一系列的矩阵变换得到。

标定参数线性回归

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

相机模型:

可得:

变换为矩阵形式：

最小二乘求解标定参数

给定超定方程$Ax = b$ $x$的解为等式两边的误差平方和最小化$min||Ax - b||^{2}$ $\Leftrightarrow A^T(Ax - b) = 0$ $\Leftrightarrow x = (A^TA)^{-1}A^Tb$

优点:

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

缺点：

• 无法得知具体的内参数和外参数 $\longrightarrow$
• 求解出的11个未知量，比待标定参数（9-10个）更多。带来了参数不独立/相关的问题

标定参数非线性优化

  用概率的视角去看最小二乘问题

• 特征点投影方程为:
• 给定{(ui,vi)}，标定参数矩阵 M 的概率为：
• 给定{(ui,vi)}，标定参数矩阵 M 的似然函数为：
• 可以利用最小二乘求解 (但不一定要用线性最小二乘)

相机标定

$(Xi,Yi,Zi)$，以及它们在图像平面上的投影坐标集 $(ui,vi)$，计算相机投影矩阵 $M$ 中的11个未知参数。 $x \rightarrow K[R|t]X=MX$

• 线性方法:
• 非线性方法:

  标定工具需要有极高的精度，包括不同平面的角度、特征点的物理距离等。因此制作标定工具十分困难。

张正友标定算法

  ”张正友标定”是指张正友教授1998年提出的单平面棋盘格的摄像机标定方法。文中提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。

基本参数变量

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

求解Homographic矩阵

• 不妨设棋盘格位于$Z = 0$
• 定义旋转矩阵 $R$ 的第 $i$ 列为 $ri$, 则有：$s\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = A [r_1 \quad r_2 \quad r_3 \quad t] \begin{bmatrix} X \\ Y \\ 0 \\ 1 \end{bmatrix} = A [r_1 \quad r_2 \quad t] \begin{bmatrix} X \\ Y \\ 1 \end{bmatrix}$
• 于是空间到图像的映射可改为：$s\tilde{m} = H\tilde{M} \qquad H = A[r_1 \quad r_2 \quad t]$
• 其中 H 是描述Homographic矩阵
• H 矩阵可以根据特征点/棋盘格角点的空间坐标，以及其图像坐标用最小二乘法很容易求解。
• 令 H 为 $H = [h_1 \quad h_2 \quad h_3]$ $[h_1 \quad h_2 \quad h_3] = \lambda A[r_1 \quad r_2 \quad t]$
• Homography 有 8 个自由度，
• 由r1和r2正交，且r1和r2的模相等，可以得到如下约束：$h_1^TA^{-T}h_2 = 0$ $h_1^TA^{-T}A^{-1}h_1 = h_2^TA^{-T}A^{-1}h_2$

计算内参数矩阵

• 定义 $B = A^{-T}A^{-1} ≡$
• B 中的未知量可表示为6D向量$b$ $b = [B_{11} \quad B_{12} \quad B_{22} \quad B_{13} \quad B_{23} \quad B_{33}]^T$
• 设H中的第i列为 $h_i$ $h_i = [h_{i1} \quad h_{i2} \quad h_{i3}]$
• 根据b的定义，可以推导出如下公式$h_i^TBh_j = v_{ij}^Tb$ $v_{ij} = [h_{i1}h_{j1} \quad h_{i1}h_{j2} + h_{i2}h_{j1} \quad h_{i2}h_{j2} \quad h_{i3}h_{j2} + h_{i2}h_{j3} \quad h_{i3}h_{j3}]^T$ $h_1^TA^{-T}A^{-1}h_2 = 0$ $h_1^TA^{-T}A^{-1}h_1 = h_2^TA^{-T}A^{-1}h_2$
• 可以推导出$\begin{bmatrix} v_{12}^T \\ (v_{11} - v_{22})^T \end{bmatrix} b = 0$
• 如果有n组观察图像，则V 是 2n x 6 的矩阵$Vb=0$
• 根据最小二乘定义，V b = 0 的解是 VTV 最小特征值对应的特征向量。
• 因此, 可以直接估算出 b，后续可以通过b求解内参
• 当观测平面 n ≥ 3 时，可以得到b的唯一解
• 当 n = 2时, 一般可令畸变参数$γ= 0$
• 当 n = 1时, 仅能估算出$α$与$β$, 此时一般可假定像主点坐标 $u_0$ 与 $v_0$
• $B = A^TA^{-1}$
• $B$
• 内部参数可通过如下公式计算(cholesky分解)：
• 外部参数可通过Homography求解，由$H = [h_1 \quad h_2 \quad h_3] = \lambda A[r_1 \quad r_2 \quad t]$，可推出$r_1=\lambda A^{-1}h_1$$r_2=\lambda A^{-1}h_2$$r_3=r_1\times r_2\quad t=\lambda A^{-1}h_3$$\lambda=1/||A^{-1}h_1||=1/||A^{-1}h_2||$
• 一般而言，求解出的$R = [r_1\quad r_2\quad r_3]$
• 在实际操作中，R可以通过SVD分解实现规范化

极大似然估计

• 给定 n 张棋盘格图像，每张图像有 m 个角点
• 最小化下述公式等同于极大似然估计$\sum_{i=1}^n\sum_{j=1}^m||m_{ij} - \hat{m}(A,R_i,t_i,M_j)||^2$
• 上述非线性优化问题可以利用Levenberg-Marquardt 算法求解
• 需要初值$A,\lbrace R_i,t_i|i=1..n\rbrace$

基本步骤

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

三、相机标定代码实现

import cv2
import numpy as np
import glob

# 找棋盘格角点
# 阈值
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 棋盘格模板规格
# w = 6  # 内角点个数，内角点是和其他格子连着的点
# h = 4
w = 28
h = 20

# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = []  # 在世界坐标系中的三维点
imgpoints = []  # 在图像平面的二维点

images = glob.glob('picture/*.jpg')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘格角点
    # 棋盘图像(8位灰度或彩色图像)  棋盘尺寸  存放角点的位置
    ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
    # 如果找到足够点对，将其存储起来
    if ret == True:
        # 角点精确检测
        # 输入图像 角点初始坐标 搜索窗口为2*winsize+1 死区 求角点的迭代终止条件
        cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        objpoints.append(objp)
        imgpoints.append(corners)
        # 将角点在图像上显示
        cv2.drawChessboardCorners(img, (w, h), corners, ret)
        cv2.imshow('findCorners', img)
        cv2.waitKey(1000)
cv2.destroyAllWindows()
# 标定、去畸变
# 输入：世界坐标系里的位置 像素坐标 图像的像素尺寸大小 3*3矩阵，相机内参数矩阵 畸变矩阵
# 输出：标定结果 相机的内参数矩阵 畸变系数 旋转矩阵 平移向量
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
# mtx：内参数矩阵
# dist：畸变系数
# rvecs：旋转向量 （外参数）
# tvecs ：平移向量 （外参数）
print(("ret:"), ret)
print(("mtx:\n"), mtx)  # 内参数矩阵
print(("dist:\n"), dist)  # 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print(("rvecs:\n"), rvecs)  # 旋转向量  # 外参数
print(("tvecs:\n"), tvecs)  # 平移向量  # 外参数
# 去畸变
img2 = cv2.imread('picture/5_d.jpg')
h, w = img2.shape[:2]
# 我们已经得到了相机内参和畸变系数，在将图像去畸变之前，
# 我们还可以使用cv.getOptimalNewCameraMatrix()优化内参数和畸变系数，
# 通过设定自由自由比例因子alpha。当alpha设为0的时候，
# 将会返回一个剪裁过的将去畸变后不想要的像素去掉的内参数和畸变系数；
# 当alpha设为1的时候，将会返回一个包含额外黑色像素点的内参数和畸变系数，并返回一个ROI用于将其剪裁掉
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0, (w, h))  # 自由比例参数

dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
# 根据前面ROI区域裁剪图片
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.jpg', dst)

# 反投影误差
# 通过反投影误差，我们可以来评估结果的好坏。越接近0，说明结果越理想。
# 通过之前计算的内参数矩阵、畸变系数、旋转矩阵和平移向量，使用cv2.projectPoints()计算三维点到二维图像的投影，
# 然后计算反投影得到的点与图像上检测到的点的误差，最后计算一个对于所有标定图像的平均误差，这个值就是反投影误差。
total_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)
    total_error += error
print(("total error: "), total_error / len(objpoints))

3.1 运行图片集

3.2 运行结果

内角点检测

内参数矩阵mtx：

mtx:
 [[695.46078737   0.         361.26248515]
 [  0.         703.07481477 305.45527163]
 [  0.           0.           1.        ]]

畸变系数dist：

dist:
 [[ 0.06118909 -0.8255912  -0.00749209  0.00908976  1.36032972]]

旋转向量rvecs：

rvecs:
 [array([[-0.28152072],
       [ 0.21872207],
       [ 1.32096679]]), array([[-0.01244095],
       [ 0.21787424],
       [ 0.14952902]]), array([[0.23462844],
       [0.17417217],
       [0.08981067]]), array([[-0.17656058],
       [ 0.09925635],
       [ 1.45843756]]), array([[-0.38556037],
       [-0.25101835],
       [-1.19642961]]), array([[-0.31670867],
       [ 0.07609003],
       [-1.09961636]]), array([[-0.37124041],
       [ 0.24359793],
       [-0.30102883]]), array([[-0.12212548],
       [ 0.00836443],
       [ 1.55452212]]), array([[-0.12393844],
       [ 0.02639644],
       [ 1.21673188]]), array([[-0.26644145],
       [-0.08728083],
       [ 1.23815146]]), array([[-0.1706968 ],
       [ 0.02590786],
       [ 0.70550959]]), array([[-0.32447378],
       [-0.01749435],
       [ 0.61196365]])]

平移向量tvecs：

tvecs:
 [array([[  8.42013706],
       [-18.58848148],
       [ 49.3615118 ]]), array([[-11.62767829],
       [-12.7051669 ],
       [ 50.19081004]]), array([[-11.84316275],
       [ -7.9137423 ],
       [ 45.99876045]]), array([[ 10.70395822],
       [-17.35058377],
       [ 45.92503071]]), array([[-15.54681312],
       [  4.17892362],
       [ 43.81156256]]), array([[-15.23511308],
       [  5.68054873],
       [ 46.83019933]]), array([[-21.32216151],
       [  1.52882323],
       [ 48.66147181]]), array([[ 7.2603346 ],
       [-8.46323449],
       [35.73347263]]), array([[  1.27019774],
       [-10.19171491],
       [ 37.63155241]]), array([[ 1.14166363],
       [-8.892467  ],
       [42.07266019]]), array([[ -3.82632485],
       [-18.15833361],
       [ 46.35969298]]), array([[ -4.31180968],
       [-17.85840848],
       [ 47.76591882]])]

反投影误差

total error:  0.09624845126795724

  通过反投影误差，我们可以来评估结果的好坏。越接近0，说明结果越理想。
  通过之前计算的内参数矩阵、畸变系数、旋转矩阵和平移向量，使用cv2.projectPoints()计算三维点到二维图像的投影，然后计算反投影得到的点与图像上检测到的点的误差，最后计算一个对于所有标定图像的平均误差，这个值就是反投影误差。

畸变矫正结果

畸变矫正前	畸变矫正后

从畸变矫正前后的图片对比情况，发现从我们的肉眼观察，并不能观察出太大的区别，说明我们自己的相机镜头不存在太大的畸变。