-
Notifications
You must be signed in to change notification settings - Fork 6
/
zed_calibration.py
176 lines (126 loc) · 5.6 KB
/
zed_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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
################################################################################
# native stereo camera calibration using the StereoLabs ZED camera in Python
# Copyright (c) 2018 Toby Breckon, Durham University, UK
# License: MIT License (MIT)
################################################################################
import cv2
import numpy as np
################################################################################
# read zed stereo camera calibration based on camera calibration data read
# from manufacturer provided calibration file
# return set of values
# as (fx, fy, B, Kl,Kr) which are:
# fx - lens focal length in x-axis
# fy - lens focal length in x-axis
# B - Baseline
# Kl - camera matrix K for left camera
# Kr - camera matrix R for right camera
# parts based on:
# https://github.com/stereolabs/zed-opencv-native/blob/master/include/calibration.hpp
# https://github.com/LarkinLabs/SRAAS/blob/master/ZED_CV2.py
def zed_camera_calibration(camera_calibration, camera_mode, full_width, height):
try:
left = camera_calibration['LEFT_CAM_'+camera_mode]
right = camera_calibration['RIGHT_CAM_'+camera_mode]
common = camera_calibration['STEREO']
except:
print("Error - specified config file does not contain valid ZED config.")
exit(1)
#[LEFT_CAM_xxx] - intrinsics
Lfx = float(left['fx'])
Lfy = float(left['fy'])
Lcx = float(left['cx'])
Lcy = float(left['cy'])
Lk1 = float(left['k1'])
Lk2 = float(left['k2'])
Lk3 = float(left['k3'])
try:
# handle newer ZED model conf formats
Lk4 = float(left['k4'])
Lk5 = 0
distCoeffsL = np.array([[Lk1], [Lk2], [Lk3], [Lk4], [Lk5]])
except BaseException:
# handle older ZED model conf formats
Lp1 = float(left['p1'])
Lp2 = float(left['p2'])
distCoeffsL = np.array([[Lk1], [Lk2], [Lk3], [Lp1], [Lp2]])
#[RIGHT_CAM_xxx] - intrinsics
Rfx = float(right['fx'])
Rfy = float(right['fy'])
Rcx = float(right['cx'])
Rcy = float(right['cy'])
Rk1 = float(right['k1'])
Rk2 = float(right['k2'])
Rk3 = float(left['k3'])
try:
# handle newer ZED model conf formats
Rk4 = float(left['k4'])
Rk5 = 0
distCoeffsR = np.array([[Rk1], [Rk2], [Rk3], [Rk4], [Rk5]])
except BaseException:
# handle older ZED model conf formats
Rp1 = float(left['p1'])
Rp2 = float(left['p2'])
distCoeffsR = np.array([[Rk1], [Rk2], [Rk3], [Rp1], [Rp2]])
# define intrinsic camera matrices, K for {left, right} caneras
K_CameraMatrix_left = np.array([[Lfx, 0, Lcx],[ 0, Lfy, Lcy],[0, 0, 1]])
K_CameraMatrix_right = np.array([[Rfx, 0, Rcx],[ 0, Rfy, Rcy],[0, 0, 1]])
# camera - extrinsics
Baseline = float(common['Baseline'])
CV = float(common['CV_'+camera_mode])
RX = float(common['RX_'+camera_mode])
RZ = float(common['RZ_'+camera_mode])
# define rotation matrix, R
R_xyz_vector = np.array([[RX], [CV], [RZ]])
R,_ = cv2.Rodrigues(R_xyz_vector)
# define translation vector, T (baseline is x-axis)
T = np.array([[Baseline],[0],[0]])
# define the Q matrix for disparity map (2D) to depth map projection (3D)
# (reference: Learning OpenCV - Gary Bradski, Adrian Kaehler, 2008)
Q = np.array([ [1, 0, 0, -1*Lcx],
[0, 1, 0, -1*Lcy],
[0, 0, 0, Lfx],
[0, 0, -1.0/Baseline, ((Lcx - Rcx) / Baseline)]]
)
# depth image is registered against left image so return set of values
# as (fx, fy, B, Kl, Kr, R, T, Q) which are:
# fx - lens focal length in x-axis
# fy - lens focal length in y-axis
# B - Baseline
# Kl - camera matrix K for left camera (3 x 3)
# Kr - camera matrix R for right camera (3 x 3)
# R - inter-camera rotation matrix (3 x 3)
# T - inter-camera translation vector (3 x 1)
# Q - disparity map (2D) to depth map projection (3D) projection matrix
# N.B. ZED camera are pre-rectified
return Lfx, Lfy, Baseline, K_CameraMatrix_left, K_CameraMatrix_right, R, T, Q
################################################################################
def read_manual_calibration(calibration_file):
# read and return (fx, fy, B, Kl, Kr, R, T, Q) which are:
# fx - lens focal length in x-axis from camera martix K of left cam
# fy - lens focal length in y-axis from camera martix K of left cam
# B - Baseline from x-axis value of translation vector T
# Kl - camera matrix K for left camera (3 x 3)
# Kr - camera matrix R for right camera (3 x 3)
# R - inter-camera rotation matrix (3 x 3)
# T - inter-camera translation vector (3 x 1)
# Q - disparity map (2D) to depth map projection (3D) projection matrix
f= cv2.FileStorage(calibration_file,cv2.FILE_STORAGE_READ)
if (f.isOpened()):
try:
K_CameraMatrix_left = f.getNode("K_l").mat()
K_CameraMatrix_right = f.getNode("K_r").mat()
Lfx = K_CameraMatrix_left[0][0]
Lfy = K_CameraMatrix_left[1][1]
R = f.getNode("R").mat()
T = f.getNode("T").mat()
Baseline = -1 * T[0][0] # i.e. x-axis of T vector
Q = f.getNode("Q").mat()
except:
print("Error - specified XML config file does not contain valid camera config.")
exit(1)
return Lfx, Lfy, Baseline, K_CameraMatrix_left, K_CameraMatrix_right, R, T, Q
else:
print("Error - cannot open specified XML config file.")
exit(1)
################################################################################