# 摄像头标定

``````cheese_size = (11, 8) # 棋盘格图片在长宽方向上包含格子的个数
corner_num = cheese_size[0] * cheese_size[1]
unit = 20  # 实际的格子间距这里是 20mm

objp = np.zeros((corner_num, 3), np.float32)
objp[:, :2] = np.mgrid[0:cheese_size[0], 0:cheese_size[1]].T.reshape(-1, 2)
objp *= unit
print("objp:", objp.shape)
stereo_right_images = []
stereo_left_images = []
objpoints = []
save = True

for fn in fns:
print(fn)
stereo_left_images.append(left_img) # opencv读取视频

stereo_right_images.append(right_img)

objpoints.append(objp)
print(len(stereo_left_images), len(stereo_right_images))

# 单目标定
x, stereo_right_corners = calibrate(
stereo_right_images, objpoints, cheese_size=cheese_size, show_img=True, fnames=fns)
stereo_right_ret, stereo_right_mtx, stereo_right_dist, stereo_right_rvecs, stereo_right_tvecs = x
# stereo_nir_dist = np.zeros_like(stereo_nir_dist)
print('right cali done...')

x, stereo_left_corners = calibrate(
stereo_left_images, objpoints, cheese_size=cheese_size, show_img=True, fnames=fns)
stereo_left_ret, stereo_left_mtx, stereo_left_dist, stereo_left_rvecs, stereo_left_tvecs = x
print('left cali done...')

if _DIST_:
print('--------------------------------------------------------------------')
stereo_right_dist = np.zeros_like(stereo_right_dist)
stereo_left_dist = np.zeros_like(stereo_left_dist)

# 双目标定
retval, stereo_left_mtx, stereo_left_dist, stereo_right_mtx, stereo_right_dist, R_left2right, T_left2right, E_left2right, F_left2right=
cv2.stereoCalibrate(np.array(objpoints), np.squeeze(np.array(stereo_left_corners)),
np.squeeze(np.array(stereo_right_corners)
), stereo_left_mtx, stereo_left_dist,
stereo_right_mtx, stereo_right_dist, (stereo_left_images[0].shape[0],
stereo_left_images[0].shape[1])
# flags=cv2.CALIB_RATIONAL_MODEL
)  # python3 transfer stereo_left -> stereo_righ tflags=cv2.CALIB_RATIONAL_MODEL
h, w, c = stereo_right_images[0].shape
print('stereo cali done...')

if _DIST_:
stereo_right_dist = np.zeros_like(stereo_right_dist)
stereo_left_dist = np.zeros_like(stereo_left_dist)

``````

``````def calibrate(images, objpoints, cheese_size, show_img=False, fnames=[]):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 50, 0.001) # 终止迭代标准，最大迭代次数或者到达精度
imgpoints = []
num = 0
for img in images:
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, cheese_size, None)# 检测输入图片是否含有棋盘图片

# If found, add object points, image points (after refining them)
if ret == True:
cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 亚像素角点检测
# print(corners)
imgpoints.append(corners)
if show_img:
# Draw and display the corners
img = np.copy(img)
cv2.drawChessboardCorners(img, cheese_size, corners, ret) # 棋盘格角点绘制
cv2.imshow('img'  , img)
cv2.imwrite("right.jpg", img)
cv2.waitKey(3)
num = num + 1
if show_img:
cv2.destroyAllWindows()

# input('回车下一步')
return cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None), imgpoints #求出内参和每一个视角的外参数

``````

# 摄像头矫正

``````# 双目矫正
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
stereo_left_mtx, stereo_left_dist, stereo_right_mtx, stereo_right_dist, (w, h), R_left2right, T_left2right, alpha=0)
R1[0, :] *= -1
R2[0, :] *= -1
print('stereo rectify done...')

# 得到映射变换
stereo_left_mapx, stereo_left_mapy = cv2.initUndistortRectifyMap(
stereo_left_mtx, stereo_left_dist, R1, P1, (w, h), 5)
stereo_right_mapx, stereo_right_mapy = cv2.initUndistortRectifyMap(
stereo_right_mtx, stereo_right_dist, R2, P2, (w, h), 5)
print('initUndistortRectifyMap done...')
``````

``````if save:
np.save('P1', P1)
np.save('P2', P2)
np.save('stereo_right_mtx', stereo_right_mtx)
np.save('stereo_right_dist', stereo_right_dist)
np.save('stereo_left_mtx', stereo_left_mtx)
np.save('stereo_left_dist', stereo_left_dist)
np.save('R_left2right', R_left2right)
np.save('T_left2right', T_left2right)
``````

``````# 可视化验证，看网格是否对齐
def drawLine(img, num=16):
h, w, *_ = img.shape
for i in range(0, h, h // num):
cv2.line(img, (0, i), (w, i), (0, 255, 0), 1, 8)
return img

for fn in fns:

frame0 = cv2.remap(right_img, stereo_right_mapx,
stereo_right_mapy, cv2.INTER_LINEAR)
frame1 = cv2.remap(left_img, stereo_left_mapx,
stereo_left_mapy, cv2.INTER_LINEAR)

img = np.concatenate((frame0, frame1), axis=1).copy()
img = drawLine(img, 32)
cv2.imshow('img', img)
ret = cv2.waitKey(3)

``````

# 世界坐标计算

``````import numpy as np
import matplotlib.pylab as plt
import math
# [K1,K2,P1,P2,k3]
point_r=[]
point_l=[]
with open(left_path,"r") as f:
while line:
temp=line.split(' ')
point=[int(temp[0]), int(temp[1])]
point_l.append(point)
with open(right_path, "r") as f:
while line:
temp = line.split(' ')
point = [int(temp[0]), int(temp[1])]  #1280
point_r.append(point)
point_r = np.array(point_r)
point_l = np.array(point_l)
return point_l,point_r

def computexyz(point_l, point_r, M_l,M_r):
# compute XYZ
ul = point_l[0]
vl = point_l[1]
ur = point_r[0]
vr = point_r[1]
# A = np.zeros((4,3))
A = np.array([[ul * M_l[2][0] - M_l[0][0], ul * M_l[2][1] - M_l[0][1], ul * M_l[2][2] - M_l[0][2]],
[vl * M_l[2][0] - M_l[1][0], vl * M_l[2][1] - M_l[1][1], vl * M_l[2][2] - M_l[1][2]],
[ur * M_r[2][0] - M_r[0][0], ur * M_r[2][1] - M_r[0][1], ur * M_r[2][2] - M_r[0][2]],
[vr * M_r[2][0] - M_r[1][0], vr * M_r[2][1] - M_r[1][1], vr * M_r[2][2] - M_r[1][2]]])
b = np.array([M_l[0][3] - ul * M_l[2][3], M_l[1][3] - vl * M_l[2][3], M_r[0][3] - ur * M_r[2][3],
M_r[1][3] - vr * M_r[2][3]])
temp1 = np.linalg.inv(np.matmul(A.T, A))
temp2 = np.matmul(A.T, b)
point_xyz = np.matmul(temp1, temp2)
return point_xyz

left_path = "data/point_left_c.txt"
right_path = "data/point_right_c.txt"

print(distLeft)
print(distRight)
m,n=point_r.shape

R_l = np.eye(3)
t_l = np.array([0,0,0])
P_r = np.zeros((3,4))
P_r[:, 0:3] = R_r
P_r[:, 3] = t_r

M_r=np.matmul(K_r,P_r)

P_l = np.zeros((3,4))
P_l[:, 0:3] = R_l
P_l[:, 3] = t_l

M_l=np.matmul(K_l,P_l)
Point = []
for i in range(len(point_l)):
Pointxyz = computexyz(point_l[i],point_r[i],M_l,M_r)
Point.append(Pointxyz)

Point = np.array(Point)

fig = plt.figure()

ax.scatter(Point[:,0], Point[:,1], Point[:,2])

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')

plt.show()

``````

THE END