700字范文 > 张正友相机标定法原理与实现

张正友相机标定法原理与实现

时间：2018-12-22 02:26:01

相关推荐

张正友相机标定法原理与实现

张正友相机标定法是张正友教授1998年提出的单平面棋盘格的相机标定方法。传统标定法的标定板是需要三维的，需要非常精确，这很难制作，而张正友教授提出的方法介于传统标定法和自标定法之间，但克服了传统标定法需要的高精度标定物的缺点，而仅需使用一个打印出来的棋盘格就可以。同时也相对于自标定而言，提高了精度，便于操作。因此张氏标定法被广泛应用于计算机视觉方面。

传统标定法的的标定板张正友标定法的标定板

今天，我们就来讲解一下张氏标定法的原理和实现，学会之后，我们就可以自己去制作一个棋盘标定板，然后拍照，标定自己手机相机的参数啦！

一、相机标定介绍

二、算法原理

1.整体流程

2.模型假设

3.模型求解

（1）内外参数求解

（2）畸变系数求解

（3）精度优化

三、算法实现

1.main.py

2.homography.py

4.extrinsics.py

5.distortion.py

6.refine_all.py

7.结果

一、相机标定介绍

相机标定指建立相机图像像素位置与场景点位置之间的关系，根据相机成像模型，由特征点在图像中坐标与世界坐标的对应关系，求解相机模型的参数。相机需要标定的模型参数包括内部参数和外部参数。

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

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

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

枕形畸变桶状畸变

用数学公式来表示：

即

其中，为相机中的坐标；为真实世界坐标；为内参矩阵；为外参矩阵

为内参矩阵，是相机内部参数组成的一个3*3的矩阵，其中，代表焦距；为畸变参数,为中心点坐标，为纵横比例参数，我们可以默认设为1，所以。

为外参矩阵，是描述照相机方向的旋转矩阵，是描述照相机中心位置的三维平移向量。

二、算法原理

1.整体流程

2.模型假设

3.模型求解

（1）内外参数求解

我们令，则

其中，H为一个3*3的矩阵，并且有一个元素作为齐次坐标。因此，H有8个自由度。

现在有8个自由度需要求解，所以需要四个对应点。也就是四个点就可以求出图像平面到世界平面的单应性矩阵H。

我想，张氏标定法选用的棋盘格作为标定板的原因除了角点方便检测的另外一个原因可能就是这个吧。

通过4个点，我们就可以可以获得单应性矩阵H。但是，H是内参阵和外参阵的合体。我们想要最终分别获得内参和外参。所以需要想个办法，先把内参求出来。然后外参也就随之解出了。观察一下这个式子：

我们可以知道以下约束条件:

①, 正交，也就是说。其实这个不难理解，因为 ,是分别绕x轴和y轴得到的，而x轴和y轴均垂直z轴。

②旋转向量的模为1，也就是说,这是因为旋转不改变尺度。

根据这两个约束条件，经过数学变换，我们可以得到：

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

用数学公式来表达如下：

我们很容易发现B是一个对称阵，所以B的有效元素就剩下6个，即

进一步化简：

通过计算，我们可以得到

利用上面提到的两个约束条件，我们可以得到下面的方程组：

这个方程组的本质和前面那两个用h和A组成的约束条件方程组是一样的。

通过至少含一个棋盘格的三幅图像，应用上述公式我们就可以估算出B了。得到B后，我们通过cholesky分解，就可以得到摄相机机的内参阵A的六个自由度，即：

再根据化简可得外部参数，即：

（2）畸变系数求解

在文章的开始，我们就讲到真实的镜头并非理想的透视成像，而是带有不同程度的畸变。理论上镜头的畸变包括径向畸变和切向畸变，切向畸变影响较小，通常只考虑径向畸变，而且在径向畸变的求解中，仅考虑了起主导的二元泰勒级数展开的前两个系数。

具体推导，参考/onthewaysuccess/article/details/40736177

（3）精度优化

在张正友标定法中，使用了两次极大似然估计策略，第一次是在不考虑畸变的情况下求解内参和外参，第二次是求解实际的畸变系数。

极大似然参数估计，参考/onthewaysuccess/article/details/40717213

三、算法实现

1.main.py

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import cv2 as cvimport numpy as npimport osfrom step.homography import get_homographyfrom step.intrinsics import get_intrinsics_paramfrom step.extrinsics import get_extrinsics_paramfrom step.distortion import get_distortionfrom step.refine_all import refinall_all_paramdef calibrate():#求单应矩阵H = get_homography(pic_points, real_points_x_y)#求内参intrinsics_param = get_intrinsics_param(H)#求对应每幅图外参extrinsics_param = get_extrinsics_param(H, intrinsics_param)#畸变矫正k = get_distortion(intrinsics_param, extrinsics_param, pic_points, real_points_x_y)#微调所有参数[new_intrinsics_param, new_k, new_extrinsics_param] = refinall_all_param(intrinsics_param,k, extrinsics_param, real_points, pic_points)print("intrinsics_parm:\t", new_intrinsics_param)print("distortionk:\t", new_k)print("extrinsics_parm:\t", new_extrinsics_param)if __name__ == "__main__":file_dir = r'..\pic'# 标定所用图像pic_name = os.listdir(file_dir)# 由于棋盘为二维平面，设定世界坐标系在棋盘上，一个单位代表一个棋盘宽度，产生世界坐标系三维坐标cross_corners = [7, 4] #棋盘方块交界点排列real_coor = np.zeros((cross_corners[0] * cross_corners[1], 3), np.float32)real_coor[:, :2] = np.mgrid[0:7, 0:4].T.reshape(-1, 2)real_points = []real_points_x_y = []pic_points = []for pic in pic_name:pic_path = os.path.join(file_dir, pic)pic_data = cv.imread(pic_path)# 寻找到棋盘角点succ, pic_coor = cv.findChessboardCorners(pic_data, (cross_corners[0], cross_corners[1]), None)if succ:# 添加每幅图的对应3D-2D坐标pic_coor = pic_coor.reshape(-1, 2)pic_points.append(pic_coor)real_points.append(real_coor)real_points_x_y.append(real_coor[:, :2])calibrate()

2.homography.py

这是用于求解单应性矩阵的文件

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import numpy as npfrom scipy import optimize as opt#求输入数据的归一化矩阵def normalizing_input_data(coor_data):x_avg = np.mean(coor_data[:, 0])y_avg = np.mean(coor_data[:, 1])sx = np.sqrt(2) / np.std(coor_data[:, 0])sy = np.sqrt(2) / np.std(coor_data[:, 1])norm_matrix = np.matrix([[sx, 0, -sx * x_avg],[0, sy, -sy * y_avg],[0, 0, 1]])return norm_matrix#求取初始估计的单应矩阵def get_initial_H(pic_coor, real_coor):# 获得归一化矩阵pic_norm_mat = normalizing_input_data(pic_coor)real_norm_mat = normalizing_input_data(real_coor)M = []for i in range(len(pic_coor)):#转换为齐次坐标single_pic_coor = np.array([pic_coor[i][0], pic_coor[i][1], 1])single_real_coor = np.array([real_coor[i][0], real_coor[i][1], 1])#坐标归一化pic_norm = np.dot(pic_norm_mat, single_pic_coor)real_norm = np.dot(real_norm_mat, single_real_coor)#构造M矩阵M.append(np.array([-real_norm.item(0), -real_norm.item(1), -1,0, 0, 0,pic_norm.item(0) * real_norm.item(0), pic_norm.item(0) * real_norm.item(1), pic_norm.item(0)]))M.append(np.array([0, 0, 0,-real_norm.item(0), -real_norm.item(1), -1,pic_norm.item(1) * real_norm.item(0), pic_norm.item(1) * real_norm.item(1), pic_norm.item(1)]))#利用SVD求解M * h = 0中h的解U, S, VT = np.linalg.svd((np.array(M, dtype='float')).reshape((-1, 9)))# 最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小H = VT[-1].reshape((3, 3))H = np.dot(np.dot(np.linalg.inv(pic_norm_mat), H), real_norm_mat)H /= H[-1, -1]return H#返回估计坐标与真实坐标偏差def value(H, pic_coor, real_coor):Y = np.array([])for i in range(len(real_coor)):single_real_coor = np.array([real_coor[i, 0], real_coor[i, 1], 1])U = np.dot(H.reshape(3, 3), single_real_coor)U /= U[-1]Y = np.append(Y, U[:2])Y_NEW = (pic_coor.reshape(-1) - Y)return Y_NEW#返回对应jacobian矩阵def jacobian(H, pic_coor, real_coor):J = []for i in range(len(real_coor)):sx = H[0]*real_coor[i][0] + H[1]*real_coor[i][1] +H[2]sy = H[3]*real_coor[i][0] + H[4]*real_coor[i][1] +H[5]w = H[6]*real_coor[i][0] + H[7]*real_coor[i][1] +H[8]w2 = w * wJ.append(np.array([real_coor[i][0]/w, real_coor[i][1]/w, 1/w,0, 0, 0,-sx*real_coor[i][0]/w2, -sx*real_coor[i][1]/w2, -sx/w2]))J.append(np.array([0, 0, 0,real_coor[i][0]/w, real_coor[i][1]/w, 1/w,-sy*real_coor[i][0]/w2, -sy*real_coor[i][1]/w2, -sy/w2]))return np.array(J)#利用Levenberg Marquart算法微调Hdef refine_H(pic_coor, real_coor, initial_H):initial_H = np.array(initial_H)final_H = opt.leastsq(value,initial_H,Dfun=jacobian,args=(pic_coor, real_coor))[0]final_H /= np.array(final_H[-1])return final_H#返回微调后的Hdef get_homography(pic_coor, real_coor):refined_homographies =[]error = []for i in range(len(pic_coor)):initial_H = get_initial_H(pic_coor[i], real_coor[i])final_H = refine_H(pic_coor[i], real_coor[i], initial_H)refined_homographies.append(final_H)return np.array(refined_homographies)

3.intrinsics.py

这是用于求解内参矩阵的文件

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import numpy as np#返回pq位置对应的v向量def create_v(p, q, H):H = H.reshape(3, 3)return np.array([H[0, p] * H[0, q],H[0, p] * H[1, q] + H[1, p] * H[0, q],H[1, p] * H[1, q],H[2, p] * H[0, q] + H[0, p] * H[2, q],H[2, p] * H[1, q] + H[1, p] * H[2, q],H[2, p] * H[2, q]])#返回相机内参矩阵Adef get_intrinsics_param(H):#构建V矩阵V = np.array([])for i in range(len(H)):V = np.append(V, np.array([create_v(0, 1, H[i]), create_v(0, 0 , H[i])- create_v(1, 1 , H[i])]))#求解V*b = 0中的bU, S, VT = np.linalg.svd((np.array(V, dtype='float')).reshape((-1, 6)))#最小的奇异值对应的奇异向量,S求出来按大小排列的，最后的最小b = VT[-1]#求取相机内参w = b[0] * b[2] * b[5] - b[1] * b[1] * b[5] - b[0] * b[4] * b[4] + 2 * b[1] * b[3] * b[4] - b[2] * b[3] * b[3]d = b[0] * b[2] - b[1] * b[1]alpha = np.sqrt(w / (d * b[0]))beta = np.sqrt(w / d**2 * b[0])gamma = np.sqrt(w / (d**2 * b[0])) * b[1]uc = (b[1] * b[4] - b[2] * b[3]) / dvc = (b[1] * b[3] - b[0] * b[4]) / dreturn np.array([[alpha, gamma, uc],[0,beta, vc],[0,0,1]])

4.extrinsics.py

这是用于求解外参矩阵的文件

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import numpy as np#返回每一幅图的外参矩阵[R|t]def get_extrinsics_param(H, intrinsics_param):extrinsics_param = []inv_intrinsics_param = np.linalg.inv(intrinsics_param)for i in range(len(H)):h0 = (H[i].reshape(3, 3))[:, 0]h1 = (H[i].reshape(3, 3))[:, 1]h2 = (H[i].reshape(3, 3))[:, 2]scale_factor = 1 / np.linalg.norm(np.dot(inv_intrinsics_param, h0))r0 = scale_factor * np.dot(inv_intrinsics_param, h0)r1 = scale_factor * np.dot(inv_intrinsics_param, h1)t = scale_factor * np.dot(inv_intrinsics_param, h2)r2 = np.cross(r0, r1)R = np.array([r0, r1, r2, t]).transpose()extrinsics_param.append(R)return extrinsics_param

5.distortion.py

这是用于求解畸变矫正系数的文件

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import numpy as np#返回畸变矫正系数k0,k1def get_distortion(intrinsic_param, extrinsic_param, pic_coor, real_coor):D = []d = []for i in range(len(pic_coor)):for j in range(len(pic_coor[i])):#转换为齐次坐标single_coor = np.array([(real_coor[i])[j, 0], (real_coor[i])[j, 1], 0, 1])#利用现有内参及外参求出估计图像坐标u = np.dot(np.dot(intrinsic_param, extrinsic_param[i]), single_coor)[u_estim, v_estim] = [u[0]/u[2], u[1]/u[2]]coor_norm = np.dot(extrinsic_param[i], single_coor)coor_norm /= coor_norm[-1]#r = np.linalg.norm((real_coor[i])[j])r = np.linalg.norm(coor_norm)D.append(np.array([(u_estim - intrinsic_param[0, 2]) * r ** 2, (u_estim - intrinsic_param[0, 2]) * r ** 4]))D.append(np.array([(v_estim - intrinsic_param[1, 2]) * r ** 2, (v_estim - intrinsic_param[1, 2]) * r ** 4]))#求出估计坐标与真实坐标的残差d.append(pic_coor[i][j, 0] - u_estim)d.append(pic_coor[i][j, 1] - v_estim)'''D.append(np.array([(pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 2, (pic_coor[i][j, 0] - intrinsic_param[0, 2]) * r ** 4]))D.append(np.array([(pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 2, (pic_coor[i][j, 1] - intrinsic_param[1, 2]) * r ** 4]))#求出估计坐标与真实坐标的残差d.append(u_estim - pic_coor[i][j, 0])d.append(v_estim - pic_coor[i][j, 1])'''D = np.array(D)temp = np.dot(np.linalg.inv(np.dot(D.T, D)), D.T)k = np.dot(temp, d)'''#也可利用SVD求解D * k = d中的kU, S, Vh=np.linalg.svd(D, full_matrices=False)temp_S = np.array([[S[0], 0],[0, S[1]]])temp_res = np.dot(Vh.transpose(), np.linalg.inv(temp_S))temp_res_res = np.dot(temp_res, U.transpose())k = np.dot(temp_res_res, d)'''return k

6.refine_all.py

这是用于微调参数的文件

#!usr/bin/env/ python# _*_ coding:utf-8 _*_import numpy as npimport mathfrom scipy import optimize as opt#微调所有参数def refinall_all_param(A, k, W, real_coor, pic_coor):#整合参数P_init = compose_paramter_vector(A, k, W)#复制一份真实坐标X_double = np.zeros((2 * len(real_coor) * len(real_coor[0]), 3))Y = np.zeros((2 * len(real_coor) * len(real_coor[0])))M = len(real_coor)N = len(real_coor[0])for i in range(M):for j in range(N):X_double[(i * N + j) * 2] = (real_coor[i])[j]X_double[(i * N + j) * 2 + 1] = (real_coor[i])[j]Y[(i * N + j) * 2] = (pic_coor[i])[j, 0]Y[(i * N + j) * 2 + 1] = (pic_coor[i])[j, 1]#微调所有参数P = opt.leastsq(value,P_init,args=(W, real_coor, pic_coor),Dfun=jacobian)[0]#raial_error表示利用标定后的参数计算得到的图像坐标与真实图像坐标点的平均像素距离error = value(P, W, real_coor, pic_coor)raial_error = [np.sqrt(error[2 * i]**2 + error[2 * i + 1]**2) for i in range(len(error) // 2)]print("total max error:\t", np.max(raial_error))#返回拆解后参数，分别为内参矩阵，畸变矫正系数，每幅图对应外参矩阵return decompose_paramter_vector(P)#把所有参数整合到一个数组内def compose_paramter_vector(A, k, W):alpha = np.array([A[0, 0], A[1, 1], A[0, 1], A[0, 2], A[1, 2], k[0], k[1]])P = alphafor i in range(len(W)):R, t = (W[i])[:, :3], (W[i])[:, 3]#旋转矩阵转换为一维向量形式zrou = to_rodrigues_vector(R)w = np.append(zrou, t)P = np.append(P, w)return P#分解参数集合，得到对应的内参，外参，畸变矫正系数def decompose_paramter_vector(P):[alpha, beta, gamma, uc, vc, k0, k1] = P[0:7]A = np.array([[alpha, gamma, uc],[0, beta, vc],[0, 0, 1]])k = np.array([k0, k1])W = []M = (len(P) - 7) // 6for i in range(M):m = 7 + 6 * izrou = P[m:m+3]t = (P[m+3:m+6]).reshape(3, -1)#将旋转矩阵一维向量形式还原为矩阵形式R = to_rotation_matrix(zrou)#依次拼接每幅图的外参w = np.concatenate((R, t), axis=1)W.append(w)W = np.array(W)return A, k, W#返回从真实世界坐标映射的图像坐标def get_single_project_coor(A, W, k, coor):single_coor = np.array([coor[0], coor[1], coor[2], 1])#'''coor_norm = np.dot(W, single_coor)coor_norm /= coor_norm[-1]#r = np.linalg.norm(coor)r = np.linalg.norm(coor_norm)uv = np.dot(np.dot(A, W), single_coor)uv /= uv[-1]#畸变u0 = uv[0]v0 = uv[1]uc = A[0, 2]vc = A[1, 2]#u = (uc * r**2 * k[0] + uc * r**4 * k[1] - u0) / (r**2 * k[0] + r**4 * k[1] - 1)#v = (vc * r**2 * k[0] + vc * r**4 * k[1] - v0) / (r**2 * k[0] + r**4 * k[1] - 1)u = u0 + (u0 - uc) * r**2 * k[0] + (u0 - uc) * r**4 * k[1]v = v0 + (v0 - vc) * r**2 * k[0] + (v0 - vc) * r**4 * k[1]'''uv = np.dot(W, single_coor)uv /= uv[-1]# 透镜矫正x0 = uv[0]y0 = uv[1]r = np.linalg.norm(np.array([x0, y0]))k0 = 0k1 = 0x = x0 * (1 + r ** 2 * k0 + r ** 4 * k1)y = y0 * (1 + r ** 2 * k0 + r ** 4 * k1)#u = A[0, 0] * x + A[0, 2]#v = A[1, 1] * y + A[1, 2][u, v, _] = np.dot(A, np.array([x, y, 1]))'''return np.array([u, v])#返回所有点的真实世界坐标映射到的图像坐标与真实图像坐标的残差def value(P, org_W, X, Y_real):M = (len(P) - 7) // 6N = len(X[0])A = np.array([[P[0], P[2], P[3]],[0, P[1], P[4]],[0, 0, 1]])Y = np.array([])for i in range(M):m = 7 + 6 * i#取出当前图像对应的外参w = P[m:m + 6]# 不用旋转矩阵的变换是因为会有精度损失'''R = to_rotation_matrix(w[:3])t = w[3:].reshape(3, 1)W = np.concatenate((R, t), axis=1)'''W = org_W[i]#计算每幅图的坐标残差for j in range(N):Y = np.append(Y, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))error_Y = np.array(Y_real).reshape(-1) - Yreturn error_Y#计算对应jacobian矩阵def jacobian(P, WW, X, Y_real):M = (len(P) - 7) // 6N = len(X[0])K = len(P)A = np.array([[P[0], P[2], P[3]],[0, P[1], P[4]],[0, 0, 1]])res = np.array([])for i in range(M):m = 7 + 6 * iw = P[m:m + 6]R = to_rotation_matrix(w[:3])t = w[3:].reshape(3, 1)W = np.concatenate((R, t), axis=1)for j in range(N):res = np.append(res, get_single_project_coor(A, W, np.array([P[5], P[6]]), (X[i])[j]))#求得x, y方向对P[k]的偏导J = np.zeros((K, 2 * M * N))for k in range(K):J[k] = np.gradient(res, P[k])return J.T#将旋转矩阵分解为一个向量并返回，Rodrigues旋转向量与矩阵的变换,最后计算坐标时并未用到，因为会有精度损失def to_rodrigues_vector(R):p = 0.5 * np.array([[R[2, 1] - R[1, 2]],[R[0, 2] - R[2, 0]],[R[1, 0] - R[0, 1]]])c = 0.5 * (np.trace(R) - 1)if np.linalg.norm(p) == 0:if c == 1:zrou = np.array([0, 0, 0])elif c == -1:R_plus = R + np.eye(3, dtype='float')norm_array = np.array([np.linalg.norm(R_plus[:, 0]),np.linalg.norm(R_plus[:, 1]),np.linalg.norm(R_plus[:, 2])])v = R_plus[:, np.where(norm_array == max(norm_array))]u = v / np.linalg.norm(v)if u[0] < 0 or (u[0] == 0 and u[1] < 0) or (u[0] == u[1] and u[0] == 0 and u[2] < 0):u = -uzrou = math.pi * uelse:zrou = []else:u = p / np.linalg.norm(p)theata = math.atan2(np.linalg.norm(p), c)zrou = theata * ureturn zrou#把旋转矩阵的一维向量形式还原为旋转矩阵并返回def to_rotation_matrix(zrou):theta = np.linalg.norm(zrou)zrou_prime = zrou / thetaW = np.array([[0, -zrou_prime[2], zrou_prime[1]],[zrou_prime[2], 0, -zrou_prime[0]],[-zrou_prime[1], zrou_prime[0], 0]])R = np.eye(3, dtype='float') + W * math.sin(theta) + np.dot(W, W) * (1 - math.cos(theta))return R