-
Notifications
You must be signed in to change notification settings - Fork 1
/
stereo_calibration.py
124 lines (94 loc) · 4.98 KB
/
stereo_calibration.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
import os
import cv2
import glob
import numpy as np
from os.path import join as osjoin
from camera_calibration import find_board_corners
def stereo_calibrate(board_size, square_size, calibration_path,
camera_params_path):
object_pts, left_image_pts, right_image_pts = calculate_image_and_object_points(board_size, calibration_path,
square_size)
K = np.load(osjoin(camera_params_path, "K.npy"))
D = np.load(osjoin(camera_params_path, "dist.npy"))
imgs = glob.glob(osjoin(calibration_path, '*.jpg'))
image_size = cv2.imread(imgs[0]).shape[:2]
ret, K1, D1, K2, D2, R, T, E, F = cv2.stereoCalibrate(
object_pts, left_image_pts, right_image_pts, K, D, K, D, image_size)
print("RMS of stereo calibration: ", ret)
R1, R2, P1, P2, Q, left_ROI, right_ROI = cv2.stereoRectify(
K1, D1, K2, D2, image_size, R, T, flags=cv2.CALIB_ZERO_DISPARITY,
alpha=0.9)
save_stereo_calibration_params(D1, D2, E, F, K1, K2, P1, P2, Q,
R, R1, R2, T, camera_params_path)
def calculate_image_and_object_points(board_size, calibration_path,
square_size,
visualize_corners=False):
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
30, 0.001)
chessboard_finder_flag = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS
obj_pts_instance = np.zeros((np.prod(board_size), 3),
dtype=np.float32)
obj_pts_instance[:, :2] = np.mgrid[0:board_size[0],
0:board_size[1]].T.reshape(-1, 2)
obj_pts_instance *= square_size
object_pts = []
left_image_pts = []
right_image_pts = []
pair_images = read_stereo_calibration_images(calibration_path)
for left_path, right_path in pair_images:
corners_right, right_image_gray, right_image, ret_right = find_board_corners(board_size, right_path, chessboard_finder_flag)
corners_left, left_image_gray, left_image, ret_left = find_board_corners(board_size, left_path, chessboard_finder_flag)
if ret_left and ret_right:
object_pts.append(obj_pts_instance)
subpix_right = cv2.cornerSubPix(right_image_gray, corners_right,
(5, 5), (-1, -1), criteria)
right_image_pts.append(subpix_right)
subpix_left = cv2.cornerSubPix(left_image_gray, corners_left,
(5, 5), (-1, -1), criteria)
left_image_pts.append(subpix_left)
if visualize_corners:
visualize_stereo_corners(board_size, subpix_left, subpix_right,
left_image, left_path, ret_left,
ret_right, right_image,
calibration_path)
else:
print("Chessboard couldn't be detected in image pair: ", left_path,
" and ", right_path)
return object_pts, left_image_pts, right_image_pts
def read_stereo_calibration_images(calibration_path):
left_images = glob.glob(osjoin(calibration_path, '*_L.jpg'))
right_images = glob.glob(osjoin(calibration_path, '*_R.jpg'))
assert len(left_images) == len(
right_images), "Error: Unequal number of left and right images."
left_images.sort()
right_images.sort()
pair_images = zip(left_images, right_images)
return pair_images
def visualize_stereo_corners(board_size, corners2_left, corners2_right,
left, left_im, ret_left, ret_right, right,
calibration_path):
im1 = cv2.drawChessboardCorners(left, board_size, corners2_left,
ret_left)
im2 = cv2.drawChessboardCorners(right, board_size, corners2_right,
ret_right)
im = cv2.hconcat([im1, im2])
corner_path = osjoin(calibration_path, 'corners')
if not os.path.isdir(corner_path):
os.makedirs(corner_path)
cv2.imwrite(osjoin(corner_path,
f'{left_im.split("/")[-1].split("_")[0]}.jpg'), im)
def save_stereo_calibration_params(D1, D2, E, F, K1, K2, P1, P2, Q, R,
R1, R2, T, camera_params_path):
np.save(osjoin(camera_params_path, "K1"), K1)
np.save(osjoin(camera_params_path, "D1"), D1)
np.save(osjoin(camera_params_path, "K2"), K2)
np.save(osjoin(camera_params_path, "D2"), D2)
np.save(osjoin(camera_params_path, "R"), R)
np.save(osjoin(camera_params_path, "T"), T)
np.save(osjoin(camera_params_path, "E"), E)
np.save(osjoin(camera_params_path, "F"), F)
np.save(osjoin(camera_params_path, "R1"), R1)
np.save(osjoin(camera_params_path, "R2"), R2)
np.save(osjoin(camera_params_path, "P1"), P1)
np.save(osjoin(camera_params_path, "P2"), P2)
np.save(osjoin(camera_params_path, "Q"), Q)