张正友标定法

相片是三维世界在二维平面上的投射，故而其深度信息是损失掉了的。但是，如果把拍照看作理想的小孔成像过程，那么相片中的每个像素，都将通过一个锥体与世界中真实的点一一对应，这时如果再来一条参考光线，那么理论上就可以实现二维图像的三维重构了。

然而，实际相机并不理想，从真实世界到图片的映射过程，实则是四个坐标系之间的变换过程，即世界坐标系、相机坐标系、图像坐标系以及像素坐标系，这些坐标系之间的关系，可以通过矩阵来表示，相机校准，目的就是求解出这些矩阵。

一个非常直观是思路是，用相机拍下一组平面，并根据平面之间点的变换关系，来拟合相机矩阵。所以，一个比较关键的问题是，如何提取平面中一一对应的点，换言之，平面中什么样的点最好提取？

答案是棋盘格的黑白交界处，这便是大名鼎鼎的张正友棋盘格标定法。

opencv提供了一系列函数以实现这个方法，其标定流程和用到的主要函数为

1. 角点检测 findChessboardCorners
2. 相机校准 calibrateCamera
3. 参数优化 getOptimalNewCameraMatrix
4. 去畸变 cv_undistort

【opencv】内置了张正友的棋盘格标定法，通过一些姿态各异的棋盘格图像，就能标定相机的内外参数。

角点检测

角点检测的目的，就是获取棋盘格上黑白相交的点的位置，直观一点，就是实现下面的结果。

为了得到上面的图像，需分三步走，第一步自然是准备好棋盘格数据。

import numpy as np
import cv2
import os

#  数据准备，path是图像文件夹的路径
path = 'imgs'
fs = os.listdir(path)
grays = []
for f in fs:
    fName = os.path.join(path, f)
    img = cv2.imread(fName)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    grays.append(gray)

第二步是核心步骤，即完成亚像素角点检测。opencv提供了【findChessboardCorners】函数用于角点检测，其输入参数包括棋盘格图像、角点个数以及标志位。在提取角点之后，可通过【cornerSubPix】函数来进一步进行亚像素角点检测，以提高精度。

# 角点检测
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 
    30, 0.001)
pImgs = []
for g in grays:
   ret, cs = cv2.findChessboardCorners(g, (11, 8), None)
   # 亚像素角点检测
   pImg = cv2.cornerSubPix(g, cs.astype(np.float32), (5, 5), (-1,   -1), criteria)
   pImgs.append(np.squeeze(pImg))

其中，pImg用于存放像素坐标中的二维点。

最后一步，画图，opencv自带的工具就像下面这样就可以，

cv2.drawChessboardCorners(grays[0], (w, h), pImgs[0], None)
cv2.imshow('findCorners', grays[0])
cv2.waitKey(1000)

考虑到此前已经学习过【matplotlib】模块，下面是【plt】的绘制流程。

import matplotlib.pyplot as plt

pts = pImgs[0].squeeze().reshape(-1,2).T
plt.imshow(grays[0])
plt.scatter(pts[0], pts[1], marker='*', c='red')
plt.show()

标定

【calibrateCamera】函数可用于图像标定，只需将现实世界的点和相机坐标系中的角点的一一对应关系输入，便能得到相应的相机矩阵。其中，现实世界中的三维点，一般成为对象点，由于棋盘格中每个方块都是等距的，故可直接建立为类似$(1,0,0),(2,0,0)\cdots$即可

objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

pObj = np.zeros((w*h, 3), np.float32)
pObj[:,:2] = np.mgrid[0:w, 0:h].T.reshape(-1,2)
pObjs = [pObj for _ in range(len(pImgs))]

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(pObjs, pImgs, 
    grays[0].shape[::-1], None, None)

其中，rec为成功标志，为True时表示标定成功。

mtx为内参矩阵，差不多是

$$\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}5572.47& 0& 1314.18\\ 0& 5573.04& 1008.16\\ 0& 0& 1\end{array}\right]$$

dist为畸变参数，最多有8个，分别表示$k_1,k_2,p_1,p_2,k_3,k_4,k_5,k_6$，本次标定得到的结果为

>>> print(dist)
[[-8.36577030e-02 -1.68977185e-01 -1.12233478e-03  9.45685802e-04
  -2.04246147e+01]]

这些畸变参数的物理意义如下

$$\begin{array}{rl}{x}^{\prime }& =\frac{x}{z},y^{\prime }=\frac{y}{z},r=\sqrt{x^{\prime 2}+y^{\prime 2}}\\ K& =\frac{1+k_1{r}^2+k{2}_r^4+k_3{r}^6}{1+k_4{r}^2+k_5{r}^4+k_6{r}^6}\\ x^{\prime \prime }& =K{x}^{\prime }+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})\\ u& =f_x{x}^{\prime \prime }+c_x\\ v& =f_y{y}^{\prime \prime }+c_y\end{array}$$

rvecs和tvecs分别表示每个标定板对应的旋转和平移向量。

去畸变

【getOptimalNewCameraMatrix】函数可以进一步优化相机参数，然后通过【undistort】函数可以修正图像畸变。

mat, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, size, 0, size)
dst = cv2.undistort(grays[0], mtx, dist, None, mat)

fig = plt.figure()
ax = fig.add_subplot(121)
ax.imshow(grays[0])
ax = fig.add_subplot(122)
ax.imshow(dst)
plt.show()

对比效果如下

最后，可通过对比反投影误差，来评估标定结果

errs = []
for i in range(len(pObjs)):
    pIm2, _ = cv2.projectPoints(pObjs[i], rvecs[i], tvecs[i], mtx, dist)
    err = cv2.norm(pImgs[i], pIm2, cv2.NORM_L2) / len(pIm2)
    errs.append(err)

plt.bar(np.arange(len(errs)), errs)
plt.show()

结果如下

# 去畸变
img2 = cv2.imread('03.jpg')
h, w = img2.shape[:2]

# 反投影误差
# 通过反投影误差，我们可以来评估结果的好坏。越接近0，说明结果越理想。
total_error = 0
for i in range(len(pObjs)):
    pIm2, _ = cv2.projectPoints(pObjs[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(pImgs[i], pIm2, cv2.NORM_L2) / len(pIm2)
    total_error += error

print("total error: ", total_error / len(pObjs))