摄像头标定准备工作
使用双目摄像头拍摄贴有棋盘格的平面，拍摄多组图片，要保证左右相机均能够拍摄到完整的方格。并测量棋盘格方格的实际尺寸。
摄像头标定
对步骤1拍摄的到的左右相机的照片进行处理，分别计算左右相机的参数。
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)
left_img = cv2.imread(fn)
stereo_left_images.append(left_img) # opencv读取视频
right_img = cv2.imread(fn.replace('left', 'right'))
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)
其中的calibrate函数为：
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 #求出内参和每一个视角的外参数
此过程能够自动检测图片中的棋盘格图片进行参数计算，得到两个摄像头的内外参数矩阵，以及两个摄像头之间的位置关系。
摄像头矫正
此时能够得到每一个摄像头的参数，但是实际中并不是理想的双目视觉模型，Bouguet立体校正算法，其核心原理是通过像素平面透视变换，使左右图像重投影误差最小，使双目系统最接近理想状态。也就是立体矫正。
# 双目矫正
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:
left_img = cv2.imread(fn)
right_img = cv2.imread(fn.replace('left', 'right'))
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)
矫正之后的图片棋盘格之间应该是对齐的，在同一高度。
此时就完成了相机的标定和矫正工作。
世界坐标计算
得到相机的参数之后，假如知道同一个点在左右视频中的位置，就能够计算其三维坐标位置。
利用跟踪或者其他算法得到某个固定点在左右摄像头的像素坐标，保存为txt文件，使用left_path 和right_path 代表其路径。
import numpy as np
import matplotlib.pylab as plt
import math
# [K1,K2,P1,P2,k3]
def read_point(left_path,right_path):
point_r=[]
point_l=[]
with open(left_path,"r") as f:
line = f.readline().strip('\n')
while line:
temp=line.split(' ')
point=[int(temp[0]), int(temp[1])]
point_l.append(point)
line=f.readline().strip('\n')
with open(right_path, "r") as f:
line = f.readline().strip('\n')
while line:
temp = line.split(' ')
point = [int(temp[0]), int(temp[1])] #1280
point_r.append(point)
line = f.readline().strip('\n')
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"
point_l,point_r = read_point(left_path,right_path)
distRight=np.load("stereo_right_dist.npy").squeeze()
distLeft=np.load("stereo_left_dist.npy").squeeze()
print(distLeft)
print(distRight)
m,n=point_r.shape
K_r = np.load("stereo_right_mtx.npy")
R_r = np.load("R_left2right.npy")
t_r = np.load("T_left2right.npy").squeeze()
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)
K_l = np.load("stereo_left_mtx.npy")
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 = fig.add_subplot(111, projection='3d')
ax.scatter(Point[:,0], Point[:,1], Point[:,2])
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
即可得到如下所示的三维轨迹图。
具体的原理部分可以参考其余博客，写的都挺好。