相机校准与姿态估计

一、相机校准

本小节中我们主要学习：

相机导致的扭曲类型
如何找到相机的内在和外在属性
如何根据这些属性使图像不失真

1. 基础概念

一些针孔相机会给图像带来明显的失真。两种主要的畸变是径向畸变和切向畸变。

径向变形会导致直线看起来是弯曲的。点离图像中心越远，径向畸变越大。例如，下图显示了一个图像，其中棋盘的两个边缘用红线标记。但是，你可以看到棋盘的边界不是一条直线，与红线不匹配。所有预期的直线都凸出。访问失真（光学）了解更多详情。

径向畸变可表示如下： $x_{distorted} = x(1+k_1r^2+k_22r^4+k_3r^6) \\ y_{distorted} = y(1+k_1r^2+k_22r^4+k_3r^6)$

类似地，由于摄像镜头没有完全平行于成像平面对齐，所以会发生切向畸变。因此，图像中的某些区域可能看起来比预期的更近。切向畸变量可以表示如下：

$x_{distorted} = x+[2p_1xy+p_2(r^2+2x^2)] \\ y_{distorted} = y+[2p_2xy+p_1(r^2+2y^2)]$

简而言之，我们需要找到五个参数，称为失真系数，由下式给出：

Distortion coefficients= $(k_1 k_2 p_1 p_2 k_3)$

除此之外，我们还需要一些其他信息，比如相机的内在和外在参数。内在参数是特定于相机的。它们包括焦距 $(f_x,f_y)$ 和光学中心 $(c_x,c_y)$ 等信息。焦距和光学中心可用于创建相机矩阵，该矩阵可用于消除由于特定相机镜头引起的失真。相机矩阵对于特定的相机是唯一的，因此一旦计算出来，它就可以在同一相机拍摄的其他图像上重复使用。它表示为一个 3x3 矩阵：

$\text { camera matrix }=\left[\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right]$

外部参数对应于将 3D 点的坐标转换为坐标系的旋转和平移向量。

对于立体声应用，首先需要纠正这些失真。为了找到这些参数，我们必须提供一些定义明确的图案的样本图像（例如棋盘）。我们找到一些我们已经知道相对位置的特定点（例如棋盘中的方角）。我们知道这些点在现实世界空间中的坐标，我们知道图像中的坐标，所以我们可以求解失真系数。为了获得更好的结果，我们至少需要 10 个测试模式。

2. 代码

如上所述，我们需要至少 10 个测试模式来进行相机校准。 OpenCV 附带了一些棋盘图像（参见 samples/data/left01.jpg – left14.jpg），因此我们将使用这些图像。考虑一个棋盘的图像。相机校准所需的重要输入数据是一组 3D 真实世界点以及图像中这些点对应的 2D 坐标。二维图像点是可以的，我们可以从图像中轻松找到。（这些图像点是棋盘中两个黑色方块相互接触的位置）

来自真实世界空间的 3D 点呢？这些图像是从静态相机拍摄的，棋盘放置在不同的位置和方向。所以我们需要知道 (X,Y,Z) 值。但为简单起见，我们可以说棋盘在 XY 平面上保持静止（因此 Z=0 始终）并且相机相应地移动。这种考虑有助于我们仅找到 X、Y 值。现在对于 X,Y 值，我们可以简单地将点传递为 (0,0), (1,0), (2,0), ... 这表示点的位置。在这种情况下，我们得到的结果将是棋盘正方形大小的比例。但是，如果我们知道正方形大小（例如 30 毫米），我们可以将值传递为 (0,0)、(30,0)、(60,0)、...。因此，我们得到以 mm 为单位的结果。（在这种情况下，我们不知道正方形大小，因为我们没有拍摄这些图像，所以我们通过正方形大小）。

3D 点称为物点，2D 像点称为像点。

1. 设置

因此，要在棋盘中查找模式，我们可以使用函数 cv.findChessboardCorners()。我们还需要传递我们正在寻找什么样的图案，比如 8x8 网格、5x5 网格等。在这个例子中，我们使用 7x6 网格。（通常棋盘有 8x8 方格和 7x7 内角）。如果获得模式，它将返回角点和 retval，这将是 True。这些角将按顺序排列（从左到右，从上到下）

用法如下：cv.findChessboardCorners( image, patternSize[, corners[, flags]] ) -> retval, corners
注意：此功能可能无法在所有图像中找到所需的图案。因此，一个不错的选择是编写代码，以便启动相机并检查每一帧是否有所需的图案。获得图案后，找到角并将其存储在列表中。另外，在阅读下一帧之前提供一些间隔，以便我们可以在不同方向调整我们的棋盘。继续此过程，直到获得所需数量的良好模式。即使在此处提供的示例中，我们也不确定给出的 14 个图像中有多少是好的。因此，我们必须阅读所有图像并只拍摄好的图像。除了棋盘，我们也可以使用圆形网格。在这种情况下，我们必须使用函数 cv.findCirclesGrid() 来查找模式。更少的图像足以使用圆形网格执行相机校准。
用法如下：

cv.findCirclesGrid( image, patternSize, flags, blobDetector, parameters[, centers] ) -> retval, centers
cv.findCirclesGrid( image, patternSize[, centers[, flags[, blobDetector]]] ) -> retval, centers

一旦我们找到角点，我们可以使用 cv.cornerSubPix() 来提高它们的准确性。我们还可以使用 cv.drawChessboardCorners() 绘制图案。所有这些步骤都包含在以下代码中：

用法如下：cv.cornerSubPix( image, corners, winSize, zeroZone, criteria ) -> corners

import numpy as np
import cv2 as cv
import matplotlib
import matplotlib.pyplot as plt
import glob
%matplotlib inline

def cv_show(name,img):
    cv.imshow(name,img)
    cv.waitKey(0)
    cv.destroyAllWindows()

# 终止标准
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 准备对象点集, 如 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
# 定义存储对象点集和存储所有图像像素的阵列
objpoints = [] # 现实空间的三维点
imgpoints = [] # 图像平面的二维点

# 读取同目录下的所有后缀名为 .jpg的图像文件
images = glob.glob('*.jpg')
for fname in images:
    img = cv.imread(fname)
    gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
    # 找出棋盘的图像角点
    ret, corners = cv.findChessboardCorners(gray, (7,6), None)
    # 若找到则在提炼之后加入阵列
    if ret == True:
        objpoints.append(objp)
        corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners)
        # 绘出角点并演示
        cv.drawChessboardCorners(img, (7,6), corners2, ret)
        cv.imshow('img', img)
        cv.waitKey(0)
cv.destroyAllWindows()

2. 校准

现在我们有了目标点和图像点，我们可以进行校准了。我们可以使用函数 cv.calibrateCamera() 返回相机矩阵、失真系数、旋转和平移向量等。

用法如下：ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
# 保存文件便于下一节使用
np.savez('B.npz', mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)

3. 修正失真

现在，我们可以拍摄图像并使其不失真。 OpenCV 有两种方法可以做到这一点。但是首先，我们可以使用 cv.getOptimalNewCameraMatrix() 基于自由缩放参数细化相机矩阵。如果缩放参数 alpha=0，则返回具有最少不需要像素的未失真图像。所以它甚至可能会删除图像角落的一些像素。如果 alpha=1，所有像素都会保留一些额外的黑色图像。此函数还返回可用于裁剪结果的图像 ROI。

所以，我们拍摄一张新图片（本例中为left12.jpg。这是本章的第一张图片）

用法如下： cv.getOptimalNewCameraMatrix( cameraMatrix, distCoeffs, imageSize, alpha[, newImgSize[, centerPrincipalPoint]] ) -> retval, validPixROI

img = cv.imread('left12.jpg')
h,  w = img.shape[:2]
newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))

使用cv.undistort()

用法如下： cv.undistort( src, cameraMatrix, distCoeffs[, dst[, newCameraMatrix]] ) -> dst

这是最简单的方法。只需调用该函数并使用上面获得的 ROI 来裁剪结果。

# undistort
dst = cv.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y+h, x:x+w]
cv_show('calibresult.png', dst)

使用重映射

这种方式有点难度。首先，找到从失真图像到未失真图像的映射函数。然后使用重映射功能。

# undistort
mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w,h), 5)
dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y+h, x:x+w]
cv_show('calibresult.png', dst)

您可以在结果中看到所有边缘都是直的。

现在，您可以使用 NumPy（np.savez、np.savetxt 等）中的写入函数存储相机矩阵和失真系数，以备将来使用。

3. 重新投影误差

重投影误差可以很好地估计找到的参数的精确程度。重投影误差越接近于零，我们找到的参数就越准确。给定固有矩阵、失真矩阵、旋转矩阵和平移矩阵，我们必须首先使用 cv.projectPoints() 将对象点转换为图像点。然后，我们可以计算通过变换得到的值与角点查找算法之间的绝对范数。为了找到平均误差，我们计算了为所有校准图像计算的误差的算术平均值。

用法如下： cv.projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs[, imagePoints[, jacobian[, aspectRatio]]] ) -> imagePoints, jacobian

mean_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2)/len(imgpoints2)
    mean_error += error
print( "total error: {}".format(mean_error/len(objpoints)) )

total error: 0.024498458465990365

二、姿态估计

本小节中我们将学习利用 calib3d 模块在图像中创建一些 3D 效果。

1. 基本概念

这将是一小部分。在上一次关于相机校准的课程中，您已经找到了相机矩阵、畸变系数等。给定一个图案图像，我们可以利用上述信息来计算它的位姿，或者物体在空间中的位置，比如它是如何旋转的，它是如何位移的等等。对于一个平面物体，我们可以假设 Z=0，这样，现在的问题就变成了如何将相机放置在空间中以查看我们的图案图像。因此，如果我们知道物体在空间中的位置，我们可以在其中绘制一些 2D 图来模拟 3D 效果。让我们看看怎么做。

我们的问题是，我们想在棋盘的第一个角上绘制 3D 坐标轴（X、Y、Z 轴）。 X轴为蓝色，Y轴为绿色，Z轴为红色。所以实际上，Z 轴应该感觉它垂直于我们的棋盘平面。

首先，让我们从之前的校准结果中加载相机矩阵和畸变系数。

import numpy as np
import cv2 as cv
import glob
# 导入之前保存的数据
# 需要在上一小节中进行保存
with np.load('B.npz') as X:
    mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]

现在让我们创建一个函数draw ，用于获取棋盘中的角（使用 cv.findChessboardCorners() 获得）和轴点来绘制 3D 轴。

def draw(img, corners, imgpts):
    corner = tuple(corners[0].ravel())
    # line的两端只能传入整数，所以需要进行数据转换，一定要转换为np.uint32，np.uint8会造成损失
    corner = np.array(corner, np.uint32)
    imgpts = np.array(imgpts, np.uint32)
    img = cv.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
    img = cv.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
    img = cv.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
    return img

然后和前面的例子一样，我们创建终止标准、对象点（棋盘角的 3D 点）和轴点。轴点是 3D 空间中用于绘制轴的点。我们绘制长度为 3 的轴（单位将根据国际象棋正方形大小，因为我们根据该大小进行校准）。所以我们的 X 轴是从 (0,0,0) 到 (3,0,0) 绘制的，所以对于 Y 轴。对于 Z 轴，从 (0,0,0) 到 (0,0,-3) 绘制。负数表示它被拉向相机。

criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)

现在，像往常一样，我们加载每个图像。搜索 7x6 网格。如果找到，我们用子角像素对其进行细化。然后计算旋转和平移，我们使用函数 cv.solvePnPRansac()。一旦我们有了这些变换矩阵，我们就可以使用它们将我们的轴点投影到图像平面。简而言之，我们在 3D 空间中找到与 (3,0,0),(0,3,0),(0,0,3) 中的每一个相对应的图像平面上的点。一旦我们得到它们，我们就使用我们的 draw() 函数从第一个角到每个点画线。完毕！！！

用法如下;

cv.solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, iterationsCount[, reprojectionError[, confidence[, inliers[, flags]]]]]]]] ) -> retval, rvec, tvec, inliers
cv.solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, inliers[, params]]]]

for fname in glob.glob('left*.jpg'):
    img = cv.imread(fname)
    gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    ret, corners = cv.findChessboardCorners(gray, (7,6),None)
    if ret == True:
        corners2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        # Find the rotation and translation vectors.
        ret,rvecs, tvecs = cv.solvePnP(objp, corners2, mtx, dist)
        # project 3D points to image plane
        imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx, dist)
        img = draw(img,corners2,imgpts)
        cv.imshow('img',img)
        k = cv.waitKey(0) & 0xFF
        if k == ord('s'):
            cv.imwrite(fname[:6]+'.png', img)
cv.destroyAllWindows()

结果如下：

2. 渲染立方体

如果要绘制立方体，修改draw()函数和轴点如下。

修改 draw() 函数：

def draw(img, corners, imgpts):
    imgpts = np.int32(imgpts).reshape(-1,2)
    # draw ground floor in green
    img = cv.drawContours(img, [imgpts[:4]],-1,(0,255,0),-3)
    # draw pillars in blue color
    for i,j in zip(range(4),range(4,8)):
        img = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]),(255),3)
    # draw top layer in red color
    img = cv.drawContours(img, [imgpts[4:]],-1,(0,0,255),3)
    return img

修改轴点。它们是 3D 空间中立方体的 8 个角：

axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
                   [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])

for fname in glob.glob('left*.jpg'):
    img = cv.imread(fname)
    gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    ret, corners = cv.findChessboardCorners(gray, (7,6),None)
    if ret == True:
        corners2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        # Find the rotation and translation vectors.
        ret,rvecs, tvecs = cv.solvePnP(objp, corners2, mtx, dist)
        # project 3D points to image plane
        imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx, dist)
        img = draw(img,corners2,imgpts)
        cv.imshow('img',img)
        k = cv.waitKey(0) & 0xFF
        if k == ord('s'):
            cv.imwrite(fname[:6]+'.png', img)
cv.destroyAllWindows()