def cal_fromcorners(self, good):
# Perform monocular calibrations
lcorners = [(l, b) for (l, r, b) in good]
rcorners = [(r, b) for (l, r, b) in good]
self.l.cal_fromcorners(lcorners)
self.r.cal_fromcorners(rcorners)
lipts = [ l for (l, _, _) in good ]
ripts = [ r for (_, r, _) in good ]
boards = [ b for (_, _, b) in good ]
opts = self.mk_object_points(boards, True)
flags = cv2.CALIB_FIX_INTRINSIC
self.T = numpy.zeros((3, 1), dtype=numpy.float64)
self.R = numpy.eye(3, dtype=numpy.float64)
cv2.stereoCalibrate(opts, lipts, ripts, self.size,
self.l.intrinsics, self.l.distortion,
self.r.intrinsics, self.r.distortion,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
self.set_alpha(0.0)
def cal_fromcorners(self):
# , do_mono_calib = False):
#if do_mono_calib:
# print
# TODO Perform monocular calibrations
# self.l.cal_fromcorners(cam1_corners)
# self.r.cal_fromcorners(cam2_corners)
cam1_ipts, cam2_ipts, b = self.get_calib_data()
print "Calibration data info:"
print " CAM1 data array shape:\t %s" % str(cam1_ipts.shape)
print " CAM2 data array shape:\t %s" % str(cam2_ipts.shape)
print " BOARD data array shape:\t %s" % str(b.shape)
flags = cv2.CALIB_FIX_INTRINSIC
self.T = numpy.zeros((3, 1), dtype=numpy.float64)
self.R = numpy.eye(3, dtype=numpy.float64)
self.cam2_D = np.zeros((5,1), np.float64)
if LooseVersion(cv2.__version__).version[0] == 2:
cv2.stereoCalibrate(b, cam1_ipts, cam2_ipts,
self.size,
self.cam1_K, self.cam1_D,
self.cam2_K, self.cam2_D,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
else:
cv2.stereoCalibrate(self.CHESSBOARD, cam1_ipts, cam2_ipts,
self.cam1_K, self.cam1_D,
self.cam2_K, self.cam2_D,
self.size,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
print "RESULTS (R, T):"
print self.R, "\n", self.T
homog = np.hstack((self.R, np.array([[0],[0],[0]])))
homog = np.vstack((homog, np.array([0,0,0,1])))
print "R AS A QUATERNION:"
q = tf.transformations.quaternion_from_matrix(homog)
print q
# NOTE quat_from_mat() doesn't return a quaternion object!
# But the Pose msg requires one!
# Fun times if ya like debugging cryptic errors!
q_msg = Quaternion(q[0], q[1], q[2], q[3])
t_msg = Point(self.T[0], self.T[1], self.T[2])
# Send the calculated tf to the tf handler node:
posemsg = Pose(t_msg, q_msg)
try:
self.publish_stereo_tf(posemsg)
except rospy.service.ServiceException:
print "Service request was successful!"
print "Shutting down calibrator node %s" % rospy.get_name()
sys.exit([0])
def calibrate_stereo(config):
term_crit = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = 0
#flags |= cv2.CALIB_FIX_INTRINSIC
#flags |= cv2.CALIB_USE_INTRINSIC_GUESS
#flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
#flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_FIX_ASPECT_RATIO
flags |= cv2.CALIB_ZERO_TANGENT_DIST
flags |= cv2.CALIB_SAME_FOCAL_LENGTH
flags |= cv2.CALIB_RATIONAL_MODEL
flags |= cv2.CALIB_FIX_K3
flags |= cv2.CALIB_FIX_K4
flags |= cv2.CALIB_FIX_K5
(error, config.camera.left_intrinsics, config.camera.left_distortion,
config.camera.right_intrinsics, config.camera.right_distorition,
R, T, E, F) = cv2.stereoCalibrate(
config.calibration.object_points,
config.calibration.left_points,
config.calibration.right_points,
config.camera.size, criteria=term_crit, flags=flags)
return error, R, T
def StereoCalibrate(self, leftCorners, rightCorners, objectPoints):
"""Calibrates the stereo camera."""
# <009> Prepares the external parameters.
imagePointsLeft = Configuration.Configuration.Instance.Pattern.LeftCorners
imagePointsRight = Configuration.Configuration.Instance.Pattern.RightCorners
imageSize = StereoCameras.Instance.Size
cameraMatrixLeft = StereoCameras.Instance.Parameters.CameraMatrix1
cameraMatrixRight = StereoCameras.Instance.Parameters.CameraMatrix2
distCoeffLeft = StereoCameras.Instance.Parameters.DistCoeffs1
distCoeffRight = StereoCameras.Instance.Parameters.DistCoeffs2
# Defines the criterias used by stereoCalibrate() function.
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_SAME_FOCAL_LENGTH
flags |= cv2.CALIB_RATIONAL_MODEL | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5
# <010> Calibrates a stereo camera setup.
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(objectPoints, imagePointsLeft, imagePointsRight, imageSize, cameraMatrixLeft, cameraMatrixRight, distCoeffLeft, distCoeffRight)
E2 = self.EssentialMatrix(R, T)
F2 = self.FundamentalMatrix(cameraMatrix1, cameraMatrix2, E2)
# TODO: Store other values?
# <010> Records the external parameters.
StereoCameras.Instance.Parameters.CameraMatrix1 = cameraMatrix1
StereoCameras.Instance.Parameters.CameraMatrix2 = cameraMatrix2
StereoCameras.Instance.Parameters.DistCoeffs1 = distCoeffs1
StereoCameras.Instance.Parameters.DistCoeffs2 = distCoeffs2
# <014> Return the final result.
return R, T
def calibrate_rgb_and_tv(objpoints, img_points_rgb, img_points_tv, image_size,
rgb_camera_matrix, rgb_dist_coeffs, tv_camera_matrix, tv_dist_coeffs):
# matrix will be FROM tv TO rgb
object_points = [objpoints for i in range(len(img_points_rgb))]
return cv2.stereoCalibrate(
objectPoints=object_points, imagePoints2=img_points_rgb, imagePoints1=img_points_tv, imageSize=image_size,
cameraMatrix2=rgb_camera_matrix, distCoeffs2=rgb_dist_coeffs, cameraMatrix1=tv_camera_matrix, distCoeffs1=tv_dist_coeffs)
def calibrate_cameras(self):
"""Calibrate cameras based on found chessboard corners."""
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_SAME_FOCAL_LENGTH
calib = StereoCalibration()
(
calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
calib.rot_mat,
calib.trans_vec,
calib.e_mat,
calib.f_mat,
) = cv2.stereoCalibrate(
self.object_points,
self.image_points["left"],
self.image_points["right"],
self.image_size,
criteria=criteria,
flags=flags,
)[
1:
]
(
calib.rect_trans["left"],
calib.rect_trans["right"],
calib.proj_mats["left"],
calib.proj_mats["right"],
calib.disp_to_depth_mat,
calib.valid_boxes["left"],
calib.valid_boxes["right"],
) = cv2.stereoRectify(
calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
self.image_size,
calib.rot_mat,
calib.trans_vec,
flags=0,
)
for side in ("left", "right"):
(calib.undistortion_map[side], calib.rectification_map[side]) = cv2.initUndistortRectifyMap(
calib.cam_mats[side],
calib.dist_coefs[side],
calib.rect_trans[side],
calib.proj_mats[side],
self.image_size,
cv2.CV_32FC1,
)
# This is replaced because my results were always bad. Estimates are
# taken from the OpenCV samples.
width, height = self.image_size
focal_length = 0.8 * width
calib.disp_to_depth_mat = np.float32(
[[1, 0, 0, -0.5 * width], [0, -1, 0, 0.5 * height], [0, 0, 0, -focal_length], [0, 0, 1, 0]]
)
return calib
def calibrate_pair(pattern_keys, data_1, data_2, min_matches):
[cam1, keys1, matches1, frame_indices1, cam_ind1] = data_1
[cam2, keys2, matches2, frame_indices2, cam_ind2] = data_2
if frame_indices1 is None:
assert frame_indices2 is None
else:
# filter frames where both views have data
I = dict((x, i) for i, x in enumerate(frame_indices1))
J = dict((x, i) for i, x in enumerate(frame_indices2))
K = set(I.keys()).intersection(J.keys())
matches1 = [matches1[I[k]] for k in K]
matches2 = [matches2[J[k]] for k in K]
keys1 = [keys1[I[k]] for k in K]
keys2 = [keys2[J[k]] for k in K]
if len(matches1) == 0:
print "Error: No overlapping frames between views %d and %d -- double check 'order' parameter" % (
cam_ind1 + 1,
cam_ind2 + 1,
)
exit(1)
# prune non-mutual matches
tmp = [intersect_matches(m1, m2) for (m1, m2) in zip(matches1, matches2)]
[matches1, matches2] = zip(*tmp)
I = [i for i, x in enumerate(matches1) if len(x) >= min_matches]
# prune frames with insufficient matches
matches1 = [matches1[i] for i in I]
matches2 = [matches2[i] for i in I]
keys1 = [keys1[i] for i in I]
keys2 = [keys2[i] for i in I]
if len(matches1) == 0:
print "Error: pair (%d, %d) have no shared keypoints" % (cam_ind1 + 1, cam_ind2 + 1)
exit(1)
print "pair (%d, %d): %d matches" % (cam_ind1 + 1, cam_ind2 + 1, len(matches1))
to_data = calib.keypoint_matches_to_calibration_data
tmp = [to_data(m, pattern_keys, k) for (m, k) in zip(matches1, keys1)]
[obj_pts, img_pts1] = zip(*tmp)
tmp = [to_data(m, pattern_keys, k) for (m, k) in zip(matches2, keys2)]
[obj_pts2, img_pts2] = zip(*tmp)
assert all([(a == b).all() for (a, b) in zip(obj_pts, obj_pts2)])
flags = cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_INTRINSIC
error, _, _, _, _, R, T, _, _ = cv2.stereoCalibrate(
obj_pts, img_pts1, img_pts2, (0, 0), cam1.K, cam1.distortion, cam2.K, cam2.distortion, flags=flags
)
return calib.Calibrated_camera(R=R, T=T, intr=cam2)
def stereoCalibration(objpoints, imgpoints1, imgpoints2, camera_matrix1, dist_coefs1, camera_matrix2, dist_coefs2,shape): T=None R= None retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F=cv2.stereoCalibrate(objpoints, imgpoints1, imgpoints1, camera_matrix1, dist_coefs1, camera_matrix1, dist_coefs1,shape, R, T, E=None, F=None, flags=cv2.CALIB_FIX_INTRINSIC,criteria=(cv2.TermCriteria_MAX_ITER+cv2.TermCriteria_EPS, 30, 1e-6)) R1=None R2=None P1=None P2=None Q=None R1,R2,P1,P2,Q,unKnown1,unKnown2=cv2.stereoRectify(camera_matrix1,dist_coefs1,camera_matrix2,dist_coefs2,shape,R,T,R1,R2,P1,P2,Q,flags=cv2.CALIB_ZERO_DISPARITY,alpha=1,newImageSize= None) return Q
def process(controller):
map_initialized = False
num_images = 0
num_samples = 0
while True:
frame = controller.frame()
images = frame.images
if images[0].is_valid and images[1].is_valid:
if not map_initialized:
left_x_map, left_y_map = initDistortionMap(frame.images[0])
right_x_map, right_y_map = initDistortionMap(frame.images[1])
map_initialized = True
undistorted_left = interpolate(get_image_as_array(images[0]), left_x_map, left_y_map)
undistorted_right = interpolate(get_image_as_array(images[1]), left_x_map, left_y_map)
left_bw = cv2.adaptiveThreshold(undistorted_left, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 13, 3)
right_bw = cv2.adaptiveThreshold(undistorted_right, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 13, 3)
corners_left = cv2.findChessboardCorners(left_bw, board_size, flags=cv2.CALIB_CB_FILTER_QUADS)
corners_right = cv2.findChessboardCorners(right_bw, board_size, flags=cv2.CALIB_CB_FILTER_QUADS)
if corners_left[0] and corners_right[0]:
cv2.cornerSubPix(left_bw, corners_left[1], (3, 3), (-1, -1), stereocalib_criteria)
cv2.cornerSubPix(right_bw, corners_right[1], (3, 3), (-1, -1), stereocalib_criteria)
cv2.drawChessboardCorners(undistorted_left, board_size, corners_left[1], corners_left[0])
cv2.drawChessboardCorners(undistorted_right, board_size, corners_right[1], corners_right[0])
image_points1.append(corners_left[1])
image_points2.append(corners_right[1])
object_points.append(obj)
num_samples += 1
print(num_samples)
if num_samples > num_boards:
break
cv2.imshow('left', undistorted_left)
cv2.imshow('right', undistorted_right)
cv2.imshow('left_bw', left_bw)
cv2.imshow('right_bw', right_bw)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.imwrite(f'left{num_images}.jpg', left_bw)
cv2.imwrite(f'right{num_images}.jpg', right_bw)
num_images += 1
break
retval,cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
object_points, image_points1, image_points2, C1, D1, C2, D2, imageSize=(640, 240), flags=stereocalib_flags, criteria=stereocalib_criteria
)
print("params")
print(retval,cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F)
print("reprojection matrix")
a = cv2.stereoRectify(C1, D1, C2, D2, (640, 240), R, T)
print(a[-3])
def getCalibratefromStereoImage(objpoints, l_imgpoints, r_imgpoints):
# from OpenCV docs: if any of CV_CALIB_FIX_ASPECT_RATIO... are specified, the matrix components must be initialized.
cameraMatrix1 = cv2.initCameraMatrix2D(objpoints, l_imgpoints, (640, 480), 0);
cameraMatrix2 = cv2.initCameraMatrix2D(objpoints, r_imgpoints, (640, 480), 0);
distCoeffs1 = None
distCoeffs2 = None
# directly call stereCalibrate from OpenCV library
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, l_imgpoints, r_imgpoints, \
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (640, 480), \
criteria=stereocalib_criteria, flags=stereocalib_flags)
return retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
def stereo_calibrate(self, dims):
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_K3
# flags |= cv2.CALIB_FIX_K4
# flags |= cv2.CALIB_FIX_K5
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 100, 1e-5)
ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
self.objpoints, self.imgpoints_l,
self.imgpoints_r, self.M1, self.d1, self.M2,
self.d2, dims,
criteria=stereocalib_criteria, flags=flags)
print('Intrinsic_mtx_1', M1)
print('dist_1', d1)
print('Intrinsic_mtx_2', M2)
print('dist_2', d2)
print('R', R)
print('T', T)
print('E', E)
print('F', F)
# for i in range(len(self.r1)):
# print("--- pose[", i+1, "] ---")
# self.ext1, _ = cv2.Rodrigues(self.r1[i])
# self.ext2, _ = cv2.Rodrigues(self.r2[i])
# print('Ext1', self.ext1)
# print('Ext2', self.ext2)
print('')
camera_model = dict([('M1', M1), ('M2', M2), ('dist1', d1),
('dist2', d2), ('rvecs1', self.r1),
('rvecs2', self.r2), ('R', R), ('T', T),
('E', E), ('F', F)])
cv2.destroyAllWindows()
return camera_model
def calibrate(folder, name1, name2):
im1 = cv2.imread(folder + '/' + name1 +'1.jpg')
h1 = im1.shape[0]
w1 = im1.shape[1]
im2 = cv2.imread(folder + '/' + name2 + '1.jpg')
if im2 is None:
im2 = cv2.imread(folder + '/' + name2 + '1.JPG')
h2 = im2.shape[0]
w2 = im2.shape[1]
img_points1, obj_points1, valid1 = detect_points_from_pattern(folder+'/'+name1+ '*.jpg')
if name2 == 'thermal':
img_points2, obj_points2, valid2 = detect_points_from_pattern(folder+'/'+name2+ '*.jpg', is_thermal=True)
else:
img_points2, obj_points2, valid2 = detect_points_from_pattern(folder+'/'+name2+ '*.jpg')
print 'valid1',valid1
print 'valid2',valid2
rms1, K1, d1, rvecs1, tvecs1 = cv2.calibrateCamera(obj_points1, img_points1, (w1, h1),
flags=cv2.CALIB_RATIONAL_MODEL)
rms2, K2, d2, rvecs2, tvecs2 = cv2.calibrateCamera(obj_points2, img_points2, (w2, h2),
flags=cv2.CALIB_RATIONAL_MODEL)
print 'rms1:',rms1, 'rms2:',rms2
obj_points = [obj_points1[j] for j,i in enumerate(valid1) if i in valid2]
img_points1 = [img_points1[j] for j,i in enumerate(valid1) if i in valid2]
img_points2 = [img_points2[valid2.index(i)] for j,i in enumerate(valid1) if i in valid2]
# imageSize - Size of the image used only to initialize intrinsic camera matrix. Since we fix intrinsic matrix, (w,h) below can be anything.
rms, _, _, _, _, R,T,E,F= cv2.stereoCalibrate(obj_points, img_points1, img_points2, (w1,h1), K1,d1,K2,d2,
flags=cv2.CALIB_FIX_INTRINSIC|cv2.CALIB_RATIONAL_MODEL)
print 'rms:',rms
print 'R:',R
print 'T:',T
out_file_name = '/home/yuncong/Documents/dataset/calib3/'+name1+'_'+name2+'_calib_info.txt'
out_file = open(out_file_name, 'w')
np.savez(out_file, K1=K1,d1=d1,K2=K2,d2=d2,R=R,T=T)
return K1,d1,K2,d2,R,T
def stereoCalibrate(imageDir, K, D, width, height, debug=False, flags=cv2.CALIB_FIX_INTRINSIC):
"""
width：棋牌的宽
height：棋牌的高
debug：输出调试信息
flags：参考opencv的stereoCalibrate函数
"""
imagePoints, nimages, imageSize = loadImagePoints(imageDir, width, height, debug)
objectPoints = []
patternPoints = cvtools.createPatternPoints(width, height)
imagePoints1 = []
imagePoints2 = []
for i in range(0, len(imagePoints)):
if i % 2 == 0:
imagePoints1.append(imagePoints[i])
else:
imagePoints2.append(imagePoints[i])
for i in range(0, nimages / 2):
objectPoints.append(patternPoints)
return cv2.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize,K,D,K,D,criteria=(cv2.TERM_CRITERIA_COUNT+cv2.TERM_CRITERIA_EPS, 100, 1e-5),flags=flags)
def calibrate(self):
left = self.detect_squares(self.left_images)
right = self.detect_squares(self.right_images)
print "Sweet camera calibration is done. \n now for the stereo data. please wait..."
(retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
R, T, E, F) = cv2.stereoCalibrate(left[0], left[1], right[1], left[2],
left[3], left[4], right[3], right[4])
self.retval = retval
self.size = left[3]
self.rvects = (left[5], right[5])
self.tvects = (left[6], right[6])
self.camera = (left[1], right[1])#(cameraMatrix1, cameraMatrix2)
self.distortion = (left[2], right[2])#(distCoeffs1, distCoeffs2)
self.r = R
self.t = T
self.e = E
self.f = F
self.save()
print "running stereo rectify in case it is needed later"
R1, R2, P1, P2, Q, valid1, valid2 = cv2.stereoRectify(self.camera[0],
self.distortion[0], self.camera[1], self.distortion[1],
left[3], self.r, self.t)
imagePoints1.append(corners1)
imagePoints2.append(corners2)
object_points.append(obj)
print "Corners stored\n"
success += 1
if success >= numBoards:
break
cv2.destroyAllWindows()
print "Starting Calibration\n"
cameraMatrix1 = np.array((3, 3), np.float32)
cameraMatrix2 = np.array((3, 3), np.float32)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
object_points, imagePoints1, imagePoints2, (width, height)
)
## , cv2.cvTermCriteria(cv2.CV_TERMCRIT_ITER+cv2.CV_TERMCRIT_EPS, 100, 1e-5), cv2.CV_CALIB_SAME_FOCAL_LENGTH | cv2.CV_CALIB_ZERO_TANGENT_DIST)
# cv2.cv.StereoCalibrate(object_points, imagePoints1, imagePoints2, pointCounts, cv.fromarray(K1), cv.fromarray(distcoeffs1), cv.fromarray(K2), cv.fromarray(distcoeffs2), imageSize, cv.fromarray(R), cv.fromarray(T), cv.fromarray(E), cv.fromarray(F), flags = cv.CV_CALIB_FIX_INTRINSIC)
# FileStorage fs1("mystereocalib.yml", FileStorage::WRITE);
# fs1 << "CM1" << CM1;
# fs1 << "CM2" << CM2;
# #fs1 << "D1" << D1;
# fs1 << "D2" << D2;
# fs1 << "R" << R;
# fs1 << "T" << T;
# fs1 << "E" << E;
# fs1 << "F" << F;
print "Done Calibration\n"
print "Starting Rectification\n"
R1 = np.zeros(shape=(3, 3))
def calibrate(self, imgs_l, imgs_r, board, flags=None):
"""
This will save the found markers for camera_2 (right) only in
self.save_cal_imgs array
"""
# so we know a little bit about the camera, so
# start off the algorithm with a simple guess
# h,w = imgs_l[0].shape[:2]
# f = max(h,w)*0.8 # focal length is a function of image size in pixels
# K = np.array([
# [f,0,w//2],
# [0,f,h//2],
# [0,0,1]
# ])
cc = CameraCalibration()
# rms1, M1, d1, r1, t1, objpoints, imgpoints_l = cc.calibrate(imgs_l, board)
# rms2, M2, d2, r2, t2, objpoints, imgpoints_r = cc.calibrate(imgs_r, board)
data = cc.calibrate(imgs_l, board)
K1 = data["cameraMatrix"]
d1 = data["distCoeffs"]
objpoints = data["objpoints"]
imgpoints_l = data["imgpoints"]
data = cc.calibrate(imgs_r, board)
K2 = data["cameraMatrix"]
d2 = data["distCoeffs"]
imgpoints_r = data["imgpoints"]
print(d1, d2)
self.save_cal_imgs = cc.save_cal_imgs
"""
CALIB_ZERO_DISPARITY: horizontal shift, cx1 == cx2
"""
if flags is None:
flags = 0
# flags |= cv2.CALIB_FIX_INTRINSIC
flags |= cv2.CALIB_ZERO_DISPARITY
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_K3
# flags |= cv2.CALIB_FIX_K4
# flags |= cv2.CALIB_FIX_K5
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 100, 1e-5)
h, w = imgs_l[0].shape[:2]
ret, K1, d1, K2, d2, R, T, E, F = cv2.stereoCalibrate(
objpoints,
imgpoints_l,
imgpoints_r,
K1,
d1,
K2,
d2,
# (w,h),
(h, w),
# R=self.R,
# T=self.t,
criteria=stereocalib_criteria,
flags=flags)
# d1 = d1.T[0]
# d2 = d2.T[0]
# print('')
camera_model = {
'date': time.strftime("%a, %d %b %Y %H:%M:%S", time.localtime()),
'markerType': board.type,
'markerSize': board.marker_size,
'imageSize': imgs_l[0].shape[:2],
# 'cameraMatrix1': K1,
# 'cameraMatrix2': K2,
# 'distCoeffs1': d1,
# 'distCoeffs2': d2,
# 'rvecs1': r1,
# 'rvecs2': r2,
# 'R': R,
# 'T': T,
# 'E': E,
# 'F': F,
"objpoints": objpoints,
"imgpointsL": imgpoints_l,
"imgpointsR": imgpoints_r,
}
sc = StereoCamera()
sc.R = R
sc.E = E
sc.F = F
sc.T = T
sc.K1 = K1
sc.K2 = K2
sc.d1 = d1
sc.d2 = d2
return ret, camera_model, sc
def calibrate(self, load_images=False, keep_chessboard_preview_imgs=False):
"""Perform a complete calibration process."""
if load_images:
self.load_images()
if len(self.left_imgs) == 0:
logging.warning("No calibration images loaded.")
self.load_images()
if len(self.left_imgs) != len(self.right_imgs):
raise CalibrationImagesNotMatch()
self.find_chessboards(keep_chessboard_preview_imgs)
logging.info("Calibrating left camera.")
_, left_matrix, left_dist_coeff, left_rot_vec, left_trans_vec = \
cv.calibrateCamera(self.obj_pts, self.left_img_pts,
(self.width, self.height), None, None
)
self.left_matrix = left_matrix
self.left_dist_coeff = left_dist_coeff
self.left_rot_vec = left_rot_vec
self.left_trans_vec = left_trans_vec
logging.info("Calibrating right camera.")
_, right_matrix, right_dist_coeff, right_rot_vec, right_trans_vec = \
cv.calibrateCamera(self.obj_pts, self.right_img_pts,
(self.width, self.height), None, None
)
self.right_matrix = right_matrix
self.right_dist_coeff = right_dist_coeff
self.right_rot_vec = right_rot_vec
self.right_trans_vec = right_trans_vec
logging.info("Calibrating both cameras.")
_, _, _, _, _, rot_matrix, trans_vec, _, _ = cv.stereoCalibrate(
self.obj_pts, self.left_img_pts, self.right_img_pts,
self.left_matrix, self.left_dist_coeff, self.right_matrix,
self.right_dist_coeff, (self.width, self.height), None, None, None,
None, cv.CALIB_FIX_INTRINSIC,
(cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001))
self.both_rot_matrix = rot_matrix
self.both_trans_vec = trans_vec
logging.info("Rectifying cameras.")
self.left_rectif, self.right_rectif, self.left_proj, self.right_proj, \
_, self.left_roi, self.right_roi = cv.stereoRectify(
self.left_matrix, self.left_dist_coeff,
self.right_matrix, self.right_dist_coeff,
(self.width, self.height),
self.both_rot_matrix, self.both_trans_vec,
None, None, None, None, None,
cv.CALIB_ZERO_DISPARITY, 0.25
)
self.calibrated = True
# Set re-projection error
self.left_reprojection_error, self.right_reprojection_error = \
self.calculate_reprojection_error()
logging.info(
"Calibration finished. Re-projection errors: {}, {}.".format(
self.left_reprojection_error, self.right_reprojection_error))
if (chessboard_pattern_detections_paired > 0): # i.e. if did not load a calib.
print()
print("START - extrinsic calibration ...")
(rms_stereo,
camera_matrix_l,
dist_coeffs_l,
camera_matrix_r,
dist_coeffs_r,
R,
T,
E,
F) = cv2.stereoCalibrate(objpoints_pairs,
imgpoints_left_paired,
imgpoints_right_paired,
mtxL,
distL,
mtxR,
distR,
grayL.shape[::-1],
criteria=termination_criteria_extrinsics,
flags=0)
print("FINISHED - extrinsic calibration")
print()
print("Intrinsic Camera Calibration:")
print()
print("Intrinsic Camera Calibration Matrix, K - from \
intrinsic calibration:")
print("(format as follows: fx, fy - focal lengths / cx, \
cy - optical centers)")
print("[fx, 0, cx]\n[0, fy, cy]\n[0, 0, 1]")
print("Stereo calibration .....")
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# Here we fix the intrinsic camara matrixes so that only Rot, Trns, Emat and Fmat are calculated.
# Hence intrinsic parameters are the same
criteria_stereo= (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
retS, new_mtxL, distL, new_mtxR, distR, Rot, Trns, Emat, Fmat = cv2.stereoCalibrate(obj_pts,
img_ptsL,
img_ptsR,
new_mtxL,
distL,
new_mtxR,
distR,
imgL_gray.shape[::-1],
criteria_stereo,
flags)
# Once we know the transformation between the two cameras we can perform stereo rectification
# StereoRectify function
rectify_scale= 1 # if 0 image croped, if 1 image not croped
rect_l, rect_r, proj_mat_l, proj_mat_r, Q, roiL, roiR= cv2.stereoRectify(new_mtxL, distL, new_mtxR, distR,
imgL_gray.shape[::-1], Rot, Trns,
rectify_scale,(0,0))
# Use the rotation matrixes for stereo rectification and camera intrinsics for undistorting the image
# Compute the rectification map (mapping between the original image pixels and
# their transformed values after applying rectification and undistortion) for left and right camera frames
flags = 0
if args.fixint:
print "fix intrinsics"
flags |= cv2.CALIB_FIX_INTRINSIC
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
elif cam1calib["camera_matrix"] is not None and cam2calib["camera_matrix"] is not None:
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
#flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
#flags |= cv2.CALIB_FIX_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_ASPECT_RATIO
#flags |= cv2.CALIB_ZERO_TANGENT_DIST
#flags |= cv2.CALIB_SAME_FOCAL_LENGTH
#flags |= cv2.CALIB_RATIONAL_MODEL
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(obj_points,img_points1,img_points2,cam1size,cam1calib["camera_matrix"],cam1calib["dist"],cam2calib["camera_matrix"],cam2calib["dist"],criteria=term_crit,flags=flags)
print "error is:",retval
print "orig cameraMatrix1\n",cam1calib["camera_matrix"]
print "cameraMatrix1\n",cameraMatrix1
print "orig cameraMatrix2\n",cam2calib["camera_matrix"]
print "cameraMatrix2\n",cameraMatrix2
print "orig distCoeffs1",cam1calib["dist"]
print "distCoeffs1",distCoeffs1.transpose()
print "orig distCoeffs1",cam2calib["dist"]
print "distCoeffs1",distCoeffs2.transpose()
print "T\n",T
print "R\n",R
#cv2.destroyAllWindows()
# read in intrinsic params
fs_read_l = cv.FileStorage('params/left_cam.yml',cv.FILE_STORAGE_READ)
mat_l = fs_read_l.getNode("intrinsic").mat()
dist_l = fs_read_l.getNode("distortion").mat()
fs_read_l.release()
fs_read_r = cv.FileStorage('params/right_cam.yml',cv.FILE_STORAGE_READ)
mat_r = fs_read_r.getNode("intrinsic").mat()
dist_r = fs_read_r.getNode("distortion").mat()
fs_read_r.release()
print('Calibrating stereo system ...')
retval, mat1, dist1, mat2, dist2, R, T, E, F = cv.stereoCalibrate(obj_pts, \
img_pts_l,img_pts_r,mat_l,dist_l,mat_r, dist_r,gray_l.shape[::-1], \
flags=cv.CALIB_FIX_INTRINSIC)
print('R: \n',R)
print('T: \n',T)
print('E: \n',E)
print('F: \n',F)
#store the files
store_location = 'params/stereo.yml'
fs = cv.FileStorage(store_location,cv.FILE_STORAGE_WRITE)
fs.write("R",R)
fs.write("T",T)
fs.write("E",E)
fs.write("F",F)
fs.release()
cameraMatrix2 = None
distCoeffs1 = None
distCoeffs2 = None
R = None
T = None
E = None
F = None
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
stereocalib_flags = cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_SAME_FOCAL_LENGTH
stereocalib_retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
obj_points,
img_left_points,
img_right_points,
cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2,
image_size,
criteria=stereocalib_criteria,
flags=stereocalib_flags)
np.savez('cameracalibmat_data/stereo_camera_mat_and_dist_coeff.npz',
cameraMatrix1=cameraMatrix1,
distCoeffs1=distCoeffs1,
cameraMatrix2=cameraMatrix2,
distCoeffs2=distCoeffs2,
R=R,
T=T,
E=E,
F=F)
# Loading camera calibration data
print("Loading camera calibration data...")
fileName = cs.getFileName(cs.camera, prefix=cs.getCamera(1))
file = cs.getCalibDataDir(cs.root) + fileName
dataSet = msc.readData(file)
mtx1, dist1, rvecs, tvecs = dataSet
fileName = cs.getFileName(cs.camera, prefix=cs.getCamera(2))
file = cs.getCalibDataDir(cs.root) + fileName
dataSet = msc.readData(file)
mtx2, dist2, rvecs, tvecs = dataSet
# Performing stereo calibration
result = cv2.stereoCalibrate(objectPoints=objpoints, imagePoints1=imgpoints1,
imagePoints2=imgpoints2, cameraMatrix1=mtx1,
distCoeffs1=dist1, cameraMatrix2=mtx2,
distCoeffs2=dist2, imageSize=(w, h),
flags=cv2.CALIB_FIX_INTRINSIC)
# Final stereo calibration dataset
dataSet = (result[5], result[6], result[7], result[8])
while True:
q = input("Would you like to test the stereo calibration " +
"parameters before proceeding? (y/n): ")
if q.lower() == 'y':
srcImage1 = ct.takePic()
srcImage2 = ct.takeRemotePic()
rectImage = sr.stereoRectify((mtx1, dist1, mtx2, dist2, result[5], result[6]),
(srcImage1, srcImage2), cs.stream_mode)
heightR, widthR, channelsR = imgR.shape
newCameraMatrixR, roi_R = cv.getOptimalNewCameraMatrix(cameraMatrixR, distR, (widthR, heightR), 1, (widthR, heightR))
########## Stereo Vision Calibration #############################################
flags = 0
flags |= cv.CALIB_FIX_INTRINSIC
# Here we fix the intrinsic camara matrixes so that only Rot, Trns, Emat and Fmat are calculated.
# Hence intrinsic parameters are the same
criteria_stereo= (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
retStereo, newCameraMatrixL, distL, newCameraMatrixR, distR, rot, trans, essentialMatrix, fundamentalMatrix = cv.stereoCalibrate(objpoints, imgpointsL, imgpointsR, newCameraMatrixL, distL, newCameraMatrixR, distR, grayL.shape[::-1], criteria_stereo, flags)
#print(newCameraMatrixL)
#print(newCameraMatrixR)
########## Stereo Rectification #################################################
rectifyScale= 1
rectL, rectR, projMatrixL, projMatrixR, Q, roi_L, roi_R= cv.stereoRectify(newCameraMatrixL, distL, newCameraMatrixR, distR, grayL.shape[::-1], rot, trans, rectifyScale,(0,0))
stereoMapL = cv.initUndistortRectifyMap(newCameraMatrixL, distL, rectL, projMatrixL, grayL.shape[::-1], cv.CV_16SC2)
stereoMapR = cv.initUndistortRectifyMap(newCameraMatrixR, distR, rectR, projMatrixR, grayR.shape[::-1], cv.CV_16SC2)
print("Saving parameters!")
cv_file = cv.FileStorage('stereoMap.xml', cv.FILE_STORAGE_WRITE)
def getStereoCalibration(phonecamPara, webcamPara):
phonecam = {}
webcam = {}
ret, phonecam['camMtx'], phonecam['distMtx'], phonecam['rotMtx'], phonecam[
'trnsMrx'] = phonecamPara
ret, webcam['camMtx'], webcam['distMtx'], webcam['rotMtx'], webcam[
'trnsMrx'] = webcamPara
phonecam['optCamMtx'], phonecam['roi'] = cv.getOptimalNewCameraMatrix(
phonecam['camMtx'], phonecam['distMtx'], IMAGE_SIZE, alpha=ALPHA)
webcam['optCamMtx'], webcam['roi'] = cv.getOptimalNewCameraMatrix(
webcam['camMtx'], webcam['distMtx'], IMAGE_SIZE, alpha=ALPHA)
recalc = str(input("Calculate New Stereo Calibration Points? Y/N: "))
if ANS[recalc]:
objpoints, phonecamImgPoints, webcamImgPoints = getChessPatternPoints(
"stereoCamCalibPoints")
ret, _, _, _, _, rotMtx, trnsMtx, _, _ = cv.stereoCalibrate(
objpoints,
webcamImgPoints,
phonecamImgPoints,
webcam['optCamMtx'],
webcam['distMtx'],
phonecam['optCamMtx'],
phonecam['distMtx'],
IMAGE_SIZE,
flags=cv.CALIB_FIX_INTRINSIC + cv.CALIB_FIX_FOCAL_LENGTH +
cv.CALIB_FIX_ASPECT_RATIO)
webcam['stereoRectRotMtx'], phonecam['stereoRectRotMtx'], webcam[
'projMtx'], phonecam['projMtx'], Q, webcam[
'stereoROI'], _ = cv.stereoRectify(
webcam['camMtx'],
webcam['distMtx'],
phonecam['camMtx'],
phonecam['distMtx'],
IMAGE_SIZE,
rotMtx,
trnsMtx,
# flags= cv.CALIB_ZERO_DISPARITY,
alpha=ALPHA)
phoneCamStereoMap = cv.initUndistortRectifyMap(
phonecam['camMtx'], phonecam['distMtx'],
phonecam['stereoRectRotMtx'], phonecam['projMtx'], IMAGE_SIZE,
cv.CV_16SC2)
webCamStereoMap = cv.initUndistortRectifyMap(
webcam['camMtx'], webcam['distMtx'], webcam['stereoRectRotMtx'],
webcam['projMtx'], IMAGE_SIZE, cv.CV_16SC2)
saveCamPara = input("Save Stereo Camera Parameters? Y/N: ")
if ANS[saveCamPara]:
with open("./camera_params/phoneCamStereoMap", "wb") as file:
pickle.dump(phoneCamStereoMap, file)
file.close()
with open("./camera_params/webCamStereoMap", "wb") as file:
pickle.dump(webCamStereoMap, file)
file.close()
return (phoneCamStereoMap, webCamStereoMap)
else:
with open("./camera_params/phoneCamStereoMap", "rb") as file:
phoneCamStereoMap = pickle.load(file)
file.close()
with open("./camera_params/webCamStereoMap", "rb") as file:
webCamStereoMap = pickle.load(file)
file.close()
return (phoneCamStereoMap, webCamStereoMap)
def stereoCalibrate(self):
print "If you calibrate from files, make sure stereo files are"
print "in the left-right-left-right order"
# Get settings & files
self.stereo = True
self.chooseCalibrationSettings()
# Get patterns
self.recordPatterns()
# Compute the intrisic parameters first :
rvecs = [
np.zeros(3, dtype=np.float32) for i in xrange(self.max_frames_i)
]
tvecs = [
np.zeros(3, dtype=np.float32) for i in xrange(self.max_frames_i)
]
# Call OpenCV routines to do the dirty work
print "Computing intrisic parameters for the first camera"
res = cv2.calibrateCamera(self.obj_points_l, self.img_points_l,
self.frame_size, self.intrinsics_l,
self.distorsion_l, rvecs, tvecs)
rms, self.intrinsics_l, self.distorsion_l, rvecs, tvecs = res
print "Computing intrisic parameters for the second camera"
res = cv2.calibrateCamera(self.obj_points_r, self.img_points_r,
self.frame_size, self.intrinsics_r,
self.distorsion_r, rvecs, tvecs)
rms, self.intrinsics_r, self.distorsion_r, rvecs, tvecs = res
# Allocate arrays for the two camera matrix and distorsion matrices
R = np.zeros((3, 3), dtype=np.float32)
T = np.zeros((3, 1), dtype=np.float32)
E = np.zeros((3, 3), dtype=np.float32)
F = np.zeros((3, 3), dtype=np.float32)
# Compute calibration parameters :
print "Calibrating cameras.."
print "Frame size : {}".format(self.frame_size)
# set stereo flags
stereo_flags = 0
# stereo_flags |= cv2.CALIB_FIX_INTRINSIC
stereo_flags |= cv2.CALIB_USE_INTRINSIC_GUESS # Refine intrinsic parameters
# stereo_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT # Fix the principal points during the optimization.
# stereo_flags |= cv2.CALIB_FIX_FOCAL_LENGTH # Fix focal length
# stereo_flags |= cv2.CALIB_FIX_ASPECT_RATIO # fix aspect ratio
stereo_flags |= cv2.CALIB_SAME_FOCAL_LENGTH # Use same focal length
# stereo_flags |= cv2.CALIB_ZERO_TANGENT_DIST # Set tangential distortion to zero
# stereo_flags |= cv2.CALIB_RATIONAL_MODEL # Use 8 param rational distortion model instead of 5 param plumb bob model
res = cv2.stereoCalibrate(self.obj_points_l, self.img_points_l,
self.img_points_r, self.frame_size,
self.intrinsics_l, self.distorsion_l,
self.intrinsics_r, self.distorsion_r)
# flags=stereo_flags
(rms, int_left, dist_left, int_right, dist_right, R, T, E, F) = res
# Output
print "Calibration done"
print "Residual RMS : {}".format(rms)
if (rms > 1.0):
print "Calibration looks faulty, please re-run"
print "\nCalibration parameters : \n Intrinsics -left \n {} \n\n Distorsion -left\n {}".format(\
int_left, dist_left)
print "\nCalibration parameters : \n Intrinsics -right \n {} \n\n Distorsion -right\n {}".format(\
int_right, dist_right)
print "\nRotation : \n{}\n".format(R)
print "\nTranslation : \n{}\n".format(T)
print "\nEssential matrix : \n{}\n".format(E) # Essential matrix
print "\nFundamental matrix : \n{}\n".format(F) # Fundamental matrix
# TODO : Compute perspective matrix !
# Save calibration parameters
save_file = utils.getAnswer(
"Would you like to save the results ? (y/n) ", 'yn')
b_write_success = False
if save_file == "y":
while (b_write_success == False):
save_XML = utils.getAnswer("Save in XML format ? (y/n) ", "yn")
filepath = raw_input(
"Where do you want to save the file ? (enter file path) ")
try:
if (save_XML):
file = utils.handlePath(filepath,
"camera_calibration.xml")
utils.saveParametersXML(int_left, dist_left, int_right,
dist_right, R, T,
self.frame_size, file)
else:
file_left = utils.handlePath(filepath, "_left.txt")
utils.saveParameters(int_left, dist_left, R, T,
file_left)
file_right = utils.handlePath(filepath, "_right.txt")
utils.saveParameters(int_right, dist_right, R, T,
file_right)
print "Parameters file written"
b_write_success = True
except:
print "Wrong path, please correct"
time.sleep(2)
return
#####################################################################
# STAGE 3: perform extrinsic calibration (recovery of relative camera positions)
# this takes the existing calibration parameters used to undistort the individual images as
# well as calculated the relative camera positions - represented via the fundamental matrix, F
# alter termination criteria to (perhaps) improve solution - ?
termination_criteria_extrinsics = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
if (chessboard_pattern_detections > 0): # i.e. if we did not load a calibration
print()
print("START - extrinsic calibration ...")
(rms_stereo, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, R, T, E, F) = \
cv2.stereoCalibrate(objpoints, imgpointsL, imgpointsR, mtxL, distL, mtxR, distR, grayL.shape[::-1], criteria=termination_criteria_extrinsics, flags=0);
print("START - extrinsic calibration ...")
print("STEREO: RMS left to right re-projection error: ", rms_stereo)
#####################################################################
# STAGE 4: rectification of images (make scan lines align left <-> right
# N.B. "alpha=0 means that the rectified images are zoomed and shifted so that
# only valid pixels are visible (no black areas after rectification). alpha=1 means
# that the rectified image is decimated and shifted so that all the pixels from the original images
# from the cameras are retained in the rectified images (no source image pixels are lost)." - ?
if (chessboard_pattern_detections > 0): # i.e. if we did not load a calibration
print("camera matrix:\n", camera_matrix)
print("distortion coefficients: ", dist_coefs.ravel())
return img_points, camera_matrix, dist_coefs
if __name__ == '__main__':
pls, cml, dcl = preprocess('left')
prs, cmr, dcr = preprocess('right')
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
obj_points = [pattern_points] * len(pls)
print("\nstereo calibrate:")
# cmr, roiR = cv2.getOptimalNewCameraMatrix(cmr, dcr,(w, h), 1, (w, h))
# cml, roiR = cv2.getOptimalNewCameraMatrix(cml, dcl,(w, h), 1, (w, h))
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
obj_points, pls, prs, cml, dcl, cmr, dcr, (w, h))
np.save('cameraMatrix1', cameraMatrix1)
np.save('distCoeffs1', distCoeffs1)
np.save('cameraMatrix2', cameraMatrix2)
np.save('distCoeffs2', distCoeffs2)
np.save('R', R)
np.save('T', T)
print('cameraMatrix1\n', cameraMatrix1)
print('distCoeffs1\n', distCoeffs1)
print('cameraMatrix2\n', cameraMatrix2)
print('distCoeffs2\n', distCoeffs2)
print('R\n', R)
print('T\n', T)
# print(help(cv2.stereoCalibrate))
print("Dl")
print(Dl)
print("Dr")
print(Dr)
pickle.dump([err, Mr, Dr, rr, tr, roi_r], open(matrices_file % "der",
"wb"))
else:
err, Ml, Dl, rl, tl, roi_l = pickle.load(open(matrices_file % "izq", "br"))
errores.append(err)
err, Mr, Dr, rr, tr, roi_r = pickle.load(open(matrices_file % "der", "br"))
errores.append(err)
# Calibracion estereo
print(Ml)
err, Ml2, Dl2, Mr2, Dr2, R, T, E, F = cv.stereoCalibrate(
objpoints, puntosl, puntosr, Ml, Dl, Mr, Dr, img_shape)
errores.append(err)
print(Ml2)
print(err)
print("R")
print(R)
print("T")
print(T)
# Rectificacion y generacion de mapas para revertir las distorsiones (directamente sobre el par estereo)
rectification_l, rectification_r, projection_l, projection_r, disparityToDepthMap, ROI_l, ROI_r = cv.stereoRectify(
Ml2,
Dl2,
Mr2,
Dr2,
distortionCoeffsRight = np.zeros((1,5))
pattern_points = np.zeros((np.prod(boardSize), 3), np.float32)
pattern_points[:, :2] = np.indices(boardSize).T.reshape(-1, 2)
pattern_points *= squareSize
objPoints = []
for i in range (0,requiredSuccessfulDetections):
objPoints.append(pattern_points)
returnCalibrate, cameraMatrixLeft, distortionCoeffsLeft, cameraMatrixRight, distortionCoeffsRight, calib_R, calib_T, calib_E, calib_F \
= cv2.stereoCalibrate(objPoints, leftImagePoints, rightImagePoints, imageSize, cameraMatrixLeft, distortionCoeffsLeft, cameraMatrixRight, distortionCoeffsRight, \
criteria=(cv2.TERM_CRITERIA_MAX_ITER|cv2.TERM_CRITERIA_EPS, 100, 1e-5), \
flags=(cv2.CALIB_FIX_ASPECT_RATIO|cv2.CALIB_ZERO_TANGENT_DIST|cv2.CALIB_SAME_FOCAL_LENGTH) \
)
print "Found intrinsinc calibration parameters, rmi {}".format(returnCalibrate)
print "Press 'q' to continue. Showing undistorted using instrinsinc params"
print cameraMatrixLeft
print distortionCoeffsLeft
print cameraMatrixRight
print distortionCoeffsRight
while True:
#Grab frames
ch.grabFrames()
frameLeft = ch.imageLeft
frameRight = ch.imageRight
x0, y0 = map(int, [0, -l[2] / l[1]])
x1, y1 = map(int, [c, -(l[2] + l[0] * c) / l[1]])
cv2.line(img, (x0, y0), (x1, y1), (0, 255, 0), 1, cv2.LINE_AA)
return img
img1, img2 = stereo_read(c1, c2)
chessboard_found1, corners1 = cv2.findChessboardCorners(img1, (9, 6), None)
chessboard_found2, corners2 = cv2.findChessboardCorners(img2, (9, 6), None)
object_points = chessboard_points_3d()
_, _, _, _, _, R, T, E, F = cv2.stereoCalibrate([object_points], [corners1],
[corners2],
K1,
dist1,
K2,
dist2,
None,
flags=cv2.CALIB_FIX_INTRINSIC)
print(np.linalg.norm(T))
epipolar_loss = []
frames2 = []
for i in range(int(20 / 2 - subvideo_length / 2) * 30):
stereo_read(c1, c2)
for i in range(subvideo_length * 30):
i1, i2 = stereo_read(c1, c2)
frames2.append(i2)
if i == 2 * 30: # the minimum of the epipolar loss should be after 2 seconds of playback (x=60 in the plot)
def calibration(undistort=False, selfCoor=False):
"""
主要完成以下功能:
1. 分别计算单目的投影矩阵并持久化。为计算3d坐标做准备
2. 计算双目的投影矩阵及R1 R2 Q 矩阵。为行对准做准备
"""
global imgpoints_r_selfcoor, imgpoints_selfcoor, objpoints_selfcoor
global cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((7 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:7].T.reshape(-1, 2) * 25
#print(objp.shape)
#objp[:, -1] = 0
#print(objp)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
objpoints_r = []
imgpoints_r = []
images = glob.glob('stereo512/left.bmp')
images_r = glob.glob('stereo512/right.bmp')
images.sort()
images_r.sort()
for fname, fname_r in zip(images, images_r):
img = cv2.imread(fname)
img_r = cv2.imread(fname_r)
if undistort:
img = undistortImage(img, cameraMatrix1, distCoeffs1)
img_r = undistortImage(img_r, cameraMatrix2, distCoeffs2)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (9, 7), None)
#print('corners',corners)
ret_r, corners_r = cv2.findChessboardCorners(gray_r, (9, 7), None)
# If found, add object points, image points (after refining them)
if ret == True and ret_r == True:
objpoints.append(objp)
objpoints_r.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
imgpoints_r.append(corners2_r)
# 分别计算投影矩阵并持久化。
if not selfCoor:
objpoints_selfcoor = objpoints[0]
imgpoints_selfcoor = imgpoints[0]
imgpoints_r_selfcoor = imgpoints_r[0]
else:
if objpoints_selfcoor == None or imgpoints_selfcoor == None or imgpoints_r_selfcoor == None:
print("Initial the self-defined coordinate first")
return
ret, rotation, translation = cv2.solvePnP(objpoints_selfcoor,
imgpoints_selfcoor,
cameraMatrix1, distCoeffs1)
ret, rotation_r, translation_r = cv2.solvePnP(objpoints_selfcoor,
imgpoints_r_selfcoor,
cameraMatrix2, distCoeffs2)
rt1 = getrtMtx(rotation, translation)
rt2 = getrtMtx(rotation_r, translation_r)
P1_own = np.dot(cameraMatrix1, rt1)
P2_own = np.dot(cameraMatrix2, rt2)
save_camera_matrix_own_proj(P1_own)
save_camera_matrix_own_proj_r(P2_own)
# 双目计算 R1 R2 P1 P2 Q并持久化。
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, imgpoints, imgpoints_r, cameraMatrix1,
distCoeffs1, cameraMatrix2, distCoeffs2, gray.shape[::-1], flags = cv2.CALIB_FIX_INTRINSIC )
#print("OPENCV R-T\n",R, "\n", T)
R1, R2, P1_stereo, P2_stereo, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2,
gray.shape[::-1],
R,
T,
flags=0)
#print("P1,P2\n",P1_stereo,"\n", P2_stereo)
save_camera_matrix_rot(R1)
save_camera_matrix_rot_r(R2)
save_camera_matrix_stereo_proj(P1_stereo)
save_camera_matrix_stereo_proj_r(P2_stereo)
save_camera_matrix_q(Q)
objectPoints = leftObjectPoints
print("Calibrating left camera...")
_, leftCameraMatrix, leftDistortionCoefficients, _, _ = cv2.calibrateCamera(
objectPoints, leftImagePoints, imageSize, None, None)
print("Calibrating right camera...")
_, rightCameraMatrix, rightDistortionCoefficients, _, _ = cv2.calibrateCamera(
objectPoints, rightImagePoints, imageSize, None, None)
print("Calibrating stereo camera...")
(_, _, _, _, _, rotationMatrix, translationVector, _,
_) = cv2.stereoCalibrate(objectPoints, leftImagePoints, rightImagePoints,
leftCameraMatrix, leftDistortionCoefficients,
rightCameraMatrix, rightDistortionCoefficients,
imageSize, None, None, None, None,
cv2.CALIB_FIX_INTRINSIC, TERMINATION_CRITERIA)
print("Rectifying cameras...")
(leftRectification, rightRectification, leftProjection, rightProjection,
disparityToDepthMap, leftROI, rightROI) = cv2.stereoRectify(
leftCameraMatrix, leftDistortionCoefficients, rightCameraMatrix,
rightDistortionCoefficients, imageSize, rotationMatrix, translationVector,
None, None, None, None, None, cv2.CALIB_ZERO_DISPARITY, OPTIMIZE_ALPHA)
print "Q matrix:", disparityToDepthMap
print("Saving calibration...")
leftMapX, leftMapY = cv2.initUndistortRectifyMap(leftCameraMatrix,
leftDistortionCoefficients,
leftRectification,
leftProjection, imageSize, 5)
for i in range(1, n):
object_points.append(corner_coordinates)
# Size of the camera images
image_size = (640, 480)
# Set criteria and flags for camera calibration
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = (cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST +
cv2.CALIB_SAME_FOCAL_LENGTH)
# Calibrate the cameras
(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E,
F) = cv2.stereoCalibrate(object_points,
left_image_points,
right_image_points,
image_size,
criteria=criteria,
flags=flags)[1:]
# Compute the rectification transforms for each head of the stereo camera pair
(left_rect_trans, right_rect_trans, left_proj_mats, right_proj_mats, Q,
left_valid_boxes, right_valid_boxes) = cv2.stereoRectify(cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2,
image_size,
R,
T,
flags=0)
# Compute the undistortion and rectification transformation map
if keyPress == 27 or keyPress == 1048603:
print("Pressed ESC, process stopped")
break
cv2.destroyAllWindows()
print("\nRunning stereo calibration... ", end="")
rms, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, R, T, E, F = cv2.stereoCalibrate(
objectPoints=stereo_objpoints,
imagePoints1=stereo_imgpoints_l,
imagePoints2=stereo_imgpoints_r,
cameraMatrix1=cameraMatrixL,
distCoeffs1=distCoeffsL,
cameraMatrix2=cameraMatrixR,
distCoeffs2=distCoeffsR,
imageSize=stereo_imgsize,
R=None,
T=None,
E=None,
F=None,
flags=(cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_FIX_INTRINSIC),
criteria=(cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-6),
)
print("calibrated with RMS error =", rms)
print("\nCamera matrices (L and R respectively):\n", cameraMatrixL, "\n \n", cameraMatrixR)
print("\nDistortion coefficients:\n", distCoeffsL.ravel(), "\n \n", distCoeffsR.ravel(), "\n")
print("Rotational matrix between camera 1 and camera 2 \n", R, "\n")
print("Translational matrix between camera 1 and camera 2 \n", T, "\n")
print("Writing intrinsics to /data/intrinsics.yml... ", end="")
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
flags |= cv2.CALIB_ZERO_TANGENT_DIST
flags = cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST +cv2.CALIB_USE_INTRINSIC_GUESS +cv2.CALIB_SAME_FOCAL_LENGTH + \
cv2.CALIB_RATIONAL_MODEL +cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5
"""-------------------------------------------------------------------------"""
"""Stereo calibration"""
"""-------------------------------------------------------------------------"""
rms_stereo, M0, d0, M1, d1, R, T, E, F = cv2.stereoCalibrate(
object_points,
image_points0,
image_points1,
camera_matrix0,
dist_coeffs0,
camera_matrix1,
dist_coeffs1,
img0.shape[0:2][::-1],
criteria=criteria,
flags=flags)
print("STEREO: RMS left to right re-projection error: ", rms_stereo)
"""-------------------------------------------------------------------------"""
"""Stereo rectification"""
"""-------------------------------------------------------------------------"""
# Q holds the quintessence of the algorithm, the reprojection matrix
R0, R1, P0, P1, Q, _, _ = cv2.stereoRectify(M0,
d0,
M1,
d1,
def calibrate_cameras(self):
"""Calibrate cameras based on found chessboard corners."""
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
flags = (cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST +
cv2.CALIB_SAME_FOCAL_LENGTH)
calib = StereoCalibration()
print(self.object_points)
print(self.image_points["left"])
print(self.image_points["right"])
print(self.image_size)
(calib.cam_mats["left"], calib.dist_coefs["left"],
calib.cam_mats["right"], calib.dist_coefs["right"],
calib.rot_mat, calib.trans_vec, calib.e_mat,
calib.f_mat) = cv2.stereoCalibrate(objectPoints=self.object_points,
imagePoints1=self.image_points["left"],
imagePoints2=self.image_points["right"],
imageSize=self.image_size,
cameraMatrix1=calib.cam_mats["left"],
distCoeffs1=calib.dist_coefs["left"],
cameraMatrix2=calib.cam_mats["right"],
distCoeffs2=calib.dist_coefs["right"],
R=calib.rot_mat,
T=calib.trans_vec,
E=calib.e_mat,
F=calib.f_mat,
criteria=criteria,
flags=flags)[1:]
(calib.rect_trans["left"], calib.rect_trans["right"],
calib.proj_mats["left"], calib.proj_mats["right"],
calib.disp_to_depth_mat, calib.valid_boxes["left"],
calib.valid_boxes["right"]) = cv2.stereoRectify(calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
self.image_size,
calib.rot_mat,
calib.trans_vec,
flags=0)
for side in ("left", "right"):
(calib.undistortion_map[side],
calib.rectification_map[side]) = cv2.initUndistortRectifyMap(
calib.cam_mats[side],
calib.dist_coefs[side],
calib.rect_trans[side],
calib.proj_mats[side],
self.image_size,
cv2.CV_32FC1)
# This is replaced because my results were always bad. Estimates are
# taken from the OpenCV samples.
width, height = self.image_size
focal_length = 0.8 * width
calib.disp_to_depth_mat = np.float32([[1, 0, 0, -0.5 * width],
[0, -1, 0, 0.5 * height],
[0, 0, 0, -focal_length],
[0, 0, 1, 0]])
return calib
# StereoCalibrate function
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_K3
# flags |= cv2.CALIB_FIX_K4
# flags |= cv2.CALIB_FIX_K5
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpointsL, imgpointsR, mtxL, distL, mtxR, distR,
ChessImaR.shape[::-1], criteria_stereo, flags)
# StereoRectify function
rectify_scale = 0 # if 0 image croped, if 1 image nor croped
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(
MLS, dLS, MRS, dRS, ChessImaR.shape[::-1], R, T, rectify_scale,
(0, 0)) # last paramater is alpha, if 0= croped, if 1= not croped
# initUndistortRectifyMap function
Left_Stereo_Map = cv2.initUndistortRectifyMap(
MLS, dLS, RL, PL, ChessImaR.shape[::-1], cv2.CV_16SC2
) # cv2.CV_16SC2 this format enables us the programme to work faster
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
ChessImaR.shape[::-1],
cv2.CV_16SC2)
imagePoints1.append(corners1)
imagePoints2.append(corners2)
object_points.append(obj)
print "Corners stored\n"
success += 1
if (success >= numBoards):
break
cv2.destroyAllWindows()
print "Starting Calibration\n"
cameraMatrix1 = np.array((3, 3), np.float32)
cameraMatrix2 = np.array((3, 3), np.float32)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
object_points, imagePoints1, imagePoints2, (width, height))
## , cv2.cvTermCriteria(cv2.CV_TERMCRIT_ITER+cv2.CV_TERMCRIT_EPS, 100, 1e-5), cv2.CV_CALIB_SAME_FOCAL_LENGTH | cv2.CV_CALIB_ZERO_TANGENT_DIST)
#cv2.cv.StereoCalibrate(object_points, imagePoints1, imagePoints2, pointCounts, cv.fromarray(K1), cv.fromarray(distcoeffs1), cv.fromarray(K2), cv.fromarray(distcoeffs2), imageSize, cv.fromarray(R), cv.fromarray(T), cv.fromarray(E), cv.fromarray(F), flags = cv.CV_CALIB_FIX_INTRINSIC)
#FileStorage fs1("mystereocalib.yml", FileStorage::WRITE);
# fs1 << "CM1" << CM1;
#fs1 << "CM2" << CM2;
# #fs1 << "D1" << D1;
#fs1 << "D2" << D2;
#fs1 << "R" << R;
#fs1 << "T" << T;
#fs1 << "E" << E;
#fs1 << "F" << F;
print "Done Calibration\n"
print "Starting Rectification\n"
R1 = np.zeros(shape=(3, 3))
R2 = np.zeros(shape=(3, 3))
if __name__ == '__main__':
#标定所用图片文件夹
left_pic_dir = '../pic/left'
right_pic_dir = '../pic/right'
#单目标定
real_points, left_pic_points, left_cameraMatrix, left_distCoeffs = calibrate_single(
left_pic_dir)
_, right_pic_points, right_cameraMatrix, right_distCoeffs = calibrate_single(
right_pic_dir)
size = (752, 480) #图片尺寸
#进行双目标定
_, _, _, _, _, R, T, E, F = cv.stereoCalibrate(
real_points, left_pic_points, right_pic_points, left_cameraMatrix,
left_distCoeffs, right_cameraMatrix, right_distCoeffs, size)
#相机坐标系转换
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv.stereoRectify(
left_cameraMatrix, left_distCoeffs, right_cameraMatrix,
right_distCoeffs, size, R, T)
#减小畸变
left_map1, leftmap2 = cv.initUndistortRectifyMap(left_cameraMatrix,
left_distCoeffs,
R1,
P1,
size=size,
m1type=cv.CV_16SC2)
right_map1, right_map2 = cv.initUndistortRectifyMap(right_cameraMatrix,
right_distCoeffs,
def StereoCameraCalibration() :
# Calibrate the left camera
cam1 = CameraCalibration( sorted( glob.glob( '{}/left*.png'.format(calibration_directory) ) ) )
# Calibrate the right camera
cam2 = CameraCalibration( sorted( glob.glob( '{}/right*.png'.format(calibration_directory) ) ) )
# Stereo calibration termination criteria
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
# Stereo calibration flags
flags = 0
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
flags |= cv2.CALIB_RATIONAL_MODEL
flags |= cv2.CALIB_FIX_K3
flags |= cv2.CALIB_FIX_K4
flags |= cv2.CALIB_FIX_K5
# Stereo calibration
calibration = cv2.stereoCalibrate( cam1['obj_points'], cam1['img_points'], cam2['img_points'],
cam1['camera_matrix'], cam1['dist_coefs'], cam2['camera_matrix'], cam2['dist_coefs'], cam1['img_size'],
flags=flags, criteria=criteria )
# Store the stereo calibration results in a dictionary
parameter_names = ( 'calib_error', 'camera_matrix_l', 'dist_coefs_l', 'camera_matrix_r', 'dist_coefs_r', 'R', 'T', 'E', 'F' )
calibration = dict( zip( parameter_names, calibration ) )
# Stereo rectification
rectification = cv2.stereoRectify(
calibration['camera_matrix_l'], calibration['dist_coefs_l'],
calibration['camera_matrix_r'], calibration['dist_coefs_r'],
cam1['img_size'], calibration['R'], calibration['T'], flags=0 )
# Store the stereo rectification results in the dictionary
parameter_names = ( 'R1', 'R2', 'P1', 'P2', 'Q', 'ROI1', 'ROI2' )
calibration.update( zip( parameter_names, rectification ) )
# Undistortion maps
calibration['left_map'] = cv2.initUndistortRectifyMap(
calibration['camera_matrix_l'], calibration['dist_coefs_l'],
calibration['R1'], calibration['P1'], cam1['img_size'], cv2.CV_32FC1 )
calibration['right_map'] = cv2.initUndistortRectifyMap(
calibration['camera_matrix_r'], calibration['dist_coefs_r'],
calibration['R2'], calibration['P2'], cam2['img_size'], cv2.CV_32FC1 )
# Compute reprojection error
undistorted_l = cv2.undistortPoints( np.concatenate( cam1['img_points'] ).reshape(-1, 1, 2),
calibration['camera_matrix_l'], calibration['dist_coefs_l'], P=calibration['camera_matrix_l'] )
undistorted_r = cv2.undistortPoints( np.concatenate( cam2['img_points'] ).reshape(-1, 1, 2),
calibration['camera_matrix_r'], calibration['dist_coefs_r'], P=calibration['camera_matrix_r'] )
lines_l = cv2.computeCorrespondEpilines( undistorted_l, 1, calibration['F'] )
lines_r = cv2.computeCorrespondEpilines( undistorted_r, 2, calibration['F'] )
calibration['reproject_error'] = 0
for i in range( len( undistorted_l ) ) :
calibration['reproject_error'] += abs( undistorted_l[i][0][0] * lines_r[i][0][0] +
undistorted_l[i][0][1] * lines_r[i][0][1] + lines_r[i][0][2] ) + abs( undistorted_r[i][0][0] * lines_l[i][0][0] +
undistorted_r[i][0][1] * lines_l[i][0][1] + lines_l[i][0][2] )
calibration['reproject_error'] /= len( undistorted_r )
# Write calibration results
with open( '{}/calibration.log'.format(calibration_directory) , 'w') as output_file :
output_file.write( '\n~~~ Left camera calibration ~~~\n\n' )
output_file.write( 'Calibration error : {}\n'.format( cam1['calib_error'] ) )
output_file.write( 'Reprojection error : {}\n'.format( cam1['reproject_error'] ) )
output_file.write( 'Camera matrix :\n{}\n'.format( cam1['camera_matrix'] ) )
output_file.write( 'Distortion coefficients :\n{}\n'.format( cam1['dist_coefs'].ravel() ) )
output_file.write( '\n~~~ Right camera calibration ~~~\n\n' )
output_file.write( 'Calibration error : {}\n'.format( cam2['calib_error'] ) )
output_file.write( 'Reprojection error : {}\n'.format( cam2['reproject_error'] ) )
output_file.write( 'Camera matrix :\n{}\n'.format( cam2['camera_matrix'] ) )
output_file.write( 'Distortion coefficients :\n{}\n'.format( cam2['dist_coefs'].ravel() ) )
output_file.write( '\n~~~ Stereo camera calibration ~~~\n\n' )
output_file.write( 'Stereo calibration error : {}\n'.format( calibration['calib_error'] ) )
output_file.write( 'Reprojection error : {}\n'.format( calibration['reproject_error'] ) )
output_file.write( 'Left camera matrix :\n{}\n'.format( calibration['camera_matrix_l'] ) )
output_file.write( 'Left distortion coefficients :\n{}\n'.format( calibration['dist_coefs_l'].ravel() ) )
output_file.write( 'Right camera matrix :\n{}\n'.format( calibration['camera_matrix_r'] ) )
output_file.write( 'Right distortion coefficients :\n{}\n'.format( calibration['dist_coefs_r'].ravel() ) )
output_file.write( 'Rotation matrix :\n{}\n'.format( calibration['R'] ) )
output_file.write( 'Translation vector :\n{}\n'.format( calibration['T'].ravel() ) )
output_file.write( 'Essential matrix :\n{}\n'.format( calibration['E'] ) )
output_file.write( 'Fundamental matrix :\n{}\n'.format( calibration['F'] ) )
output_file.write( 'Rotation matrix for the first camera :\n{}\n'.format( calibration['R1'] ) )
output_file.write( 'Rotation matrix for the second camera :\n{}\n'.format( calibration['R2'] ) )
output_file.write( 'Projection matrix for the first camera :\n{}\n'.format( calibration['P1'] ) )
output_file.write( 'Projection matrix for the second camera :\n{}\n'.format( calibration['P2'] ) )
output_file.write( 'Disparity-to-depth mapping matrix :\n{}\n'.format( calibration['Q'] ) )
output_file.write( 'ROI for the left camera : {}\n'.format( calibration['ROI1'] ) )
output_file.write( 'ROI for the right camera : {}\n'.format( calibration['ROI2'] ) )
# Write the calibration object with all the parameters
with open( '{}/calibration.pkl'.format(calibration_directory) , 'wb') as output_file :
pickle.dump( calibration, output_file, pickle.HIGHEST_PROTOCOL )
# Return the calibration
return calibration
def calibrate_cameras(self):
"""Calibrate cameras based on found chessboard corners."""
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
stereo_flag = (cv2.CALIB_FIX_INTRINSIC+cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_FIX_FOCAL_LENGTH
+cv2.CALIB_FIX_PRINCIPAL_POINT)
#stereo_flag = cv2.CALIB_USE_INTRINSIC_GUESS
calib = StereoCalibration()
'''
calib.cam_mats["left"] = []
calib.cam_mats["right"] = []
calib.cam_mats["left"].append(np.array([6761, 0, 0]).astype('float'))
calib.cam_mats["left"].append(np.array([0, 6700, 0]).astype('float'))
calib.cam_mats["left"].append(np.array([1818, 1007, 1]).astype('float'))
calib.cam_mats["right"].append(np.array([6888, 0, 0]).astype('float'))
calib.cam_mats["right"].append(np.array([0, 6821, 0]).astype('float'))
calib.cam_mats["right"].append(np.array([1664, 1096, 1]).astype('float'))
'''
calib.cam_mats['left'] = np.zeros((3,3))
calib.cam_mats['left'][0,0] = 6761
calib.cam_mats['left'][0,1] = 0
calib.cam_mats['left'][0,2] = 0
calib.cam_mats['left'][1,0] = 0
calib.cam_mats['left'][1,1] = 6700
calib.cam_mats['left'][1,2] = 0
calib.cam_mats['left'][2,0] = 1818
calib.cam_mats['left'][2,1] = 1007
calib.cam_mats['left'][2,2] = 1
calib.cam_mats['right'] = np.zeros((3,3))
calib.cam_mats['right'][0,0] = 6888
calib.cam_mats['right'][0,1] = 0
calib.cam_mats['right'][0,2] = 0
calib.cam_mats['right'][1,0] = 0
calib.cam_mats['right'][1,1] = 6821
calib.cam_mats['right'][1,2] = 0
calib.cam_mats['right'][2,0] = 1664
calib.cam_mats['right'][2,1] = 1096
calib.cam_mats['right'][2,2] = 1
(calib.cam_mats["left"], calib.dist_coefs["left"],
calib.cam_mats["right"], calib.dist_coefs["right"],
calib.rot_mat, calib.trans_vec, calib.e_mat,
calib.f_mat) = cv2.stereoCalibrate(self.object_points,
self.image_points["left"],
self.image_points["right"],
self.image_size,
calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
#self.image_size,
calib.rot_mat,
calib.trans_vec,
calib.e_mat,
calib.f_mat,
criteria=criteria,
flags=stereo_flag)[1:]
(calib.rect_trans["left"], calib.rect_trans["right"],
calib.proj_mats["left"], calib.proj_mats["right"],
calib.disp_to_depth_mat, calib.valid_boxes["left"],
calib.valid_boxes["right"]) = cv2.stereoRectify(calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
self.image_size,
calib.rot_mat,
calib.trans_vec,
flags=0)
for side in ("left", "right"):
(calib.undistortion_map[side],
calib.rectification_map[side]) = cv2.initUndistortRectifyMap(
calib.cam_mats[side],
calib.dist_coefs[side],
calib.rect_trans[side],
calib.proj_mats[side],
self.image_size,
cv2.CV_32FC1)
'''
# This is replaced because my results were always bad. Estimates are
# taken from the OpenCV samples.
width, height = self.image_size
focal_length = 0.8 * width
calib.disp_to_depth_mat = np.float32([[1, 0, 0, -0.5 * width],
[0, -1, 0, 0.5 * height],
[0, 0, 0, -focal_length],
[0, 0, 1, 0]])
'''
return calib
# cv2.imshow('img_left',l_tagged_img)
# cv2.imshow('img_right',r_tagged_img)
# cv2.waitKey(2000)
cv2.imwrite(f'../imgs/check/{i+1}_l.jpg', l_tagged_img)
cv2.imwrite(f'../imgs/check/{i + 1}_r.jpg', r_tagged_img)
print('逐个标定单目摄像机...')
l_ret, l_mtx, l_dist, l_rvecs, l_tvecs = cv2.calibrateCamera(objpoints, left_imgpoints, l_img_size, None, None, criteria=criteria)
r_ret, r_mtx, r_dist, r_rvecs, r_tvecs = cv2.calibrateCamera(objpoints, right_imgpoints, r_img_size, None, None, criteria=criteria)
# print('左Camera标定的RMS:', l_ret)
# print('右Camera标定的RMS:', r_ret)
print('标定双目摄像机...')
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints,
left_imgpoints,
right_imgpoints,
l_mtx,
l_dist,
r_mtx,
r_dist,
l_img_size,
criteria=criteria)
print('*'*50)
print('左摄像机到右摄像机坐标系的旋转矩阵:','\n',R)
print('左摄像机到右摄像机坐标系的位移矩阵:','\n',T)
print('左摄像机到右摄像机坐标系的本征矩阵:','\n',E)
print('左摄像机到右摄像机坐标系的基础矩阵:','\n',F)
print('重建损失:', retval)
h, w = image_lists[0].shape
camera_matrix = list()
camera_matrix.append(
cv2.initCameraMatrix2D(object_points, image_points[:image_number], (w, h),
0))
camera_matrix.append(
cv2.initCameraMatrix2D(object_points, image_points[image_number:], (w, h),
0))
# 4. 双目视觉进行标定
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 100, 1e-5)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(object_points, image_points[:image_number], image_points[image_number:], camera_matrix[0],
None, camera_matrix[1], None, (w, h),
flags=cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_USE_INTRINSIC_GUESS |
cv2.CALIB_SAME_FOCAL_LENGTH | cv2.CALIB_RATIONAL_MODEL | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5,
criteria=term)
# 5. 矫正一张图像看看，是否完成了极线矫正
start_time = time.time()
fname1 = './images/left/left01.jpg'
fname2 = './images/right/right01.jpg'
img1 = cv2.imread(fname1)
img2 = cv2.imread(fname2)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = \
cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (w, h), R, T)
# If found, add object points, image points (after refining them)
if ret == True:
corners22 = cv2.cornerSubPix(gray2,corners21,(11,11),(-1,-1),criteria)
imgpoints2.append(corners22)
# Draw and display the corners
img2 = cv2.drawChessboardCorners(img2, (8,6), corners22,ret)
cv2.imshow('img',img2)
cv2.waitKey(500)
i=i+1
cv2.destroyAllWindows()
ret, cameramatrix2, distcoeffs2, rvecs2, tvecs2 = cv2.calibrateCamera(objpoints, imgpoints2, (320,240),None,None)
ret, cameramatrix1, distcoeffs1, rvecs1, tvecs1 = cv2.calibrateCamera(objpoints, imgpoints1, (320,240),None,None)
R1,Jacobian1=cv2.Rodrigues(rvecs1[1])
R2,Jacobian2=cv2.Rodrigues(rvecs2[1])
ret,cameraMatrix1,distCoeffs1,cameraMatrix2,distCoeffs2,R,T,E,F=cv2.stereoCalibrate(objpoints,imgpoints1,imgpoints2,(320,240),None,None,None,None)#cameramatrix1,distcoeffs1,cameramatrix2,distcoeffs2)
#cv2.StereoRectify(cameramatrix1,distcoeffs1,cameramatrix2,distcoeffs2,(320,240))#,R,T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY, -1, (0, 0))
print str('Camera Matrix 1:') + str(cameramatrix1)
print str('Camera Matrix 2:')+str(cameramatrix2)
print str('Distortion Coefficients 1:')+str(distcoeffs1)
print str('Distortion Coefficients 2:')+str(distcoeffs2)
print str('Relative Rotational Matrix:')+str(R)
print str('Relative Translational Matrix:')+str(T)
#print Q
#stereo = cv2.createStereoBM(numDisparities=16, blockSize=15)
#disparity = stereo.compute(img1,img2)
#imshow('disparity',disparity)
#cv2.ReprojectImageto3D(disparity,_3DImage,Q,handleMissingValues=0)
#load img_points
rgbImages = glob(const.rgbFolder + '*.dat')
irImages = glob(const.irFolder+ '*.dat')
rgb_img_points = getImagePoints(rgbImages)
ir_img_points = getImagePoints(irImages)
#create object points for all image pairs
pattern_points = np.zeros((irImages.__len__(),np.prod(const.pattern_size), 3), np.float32)
pattern_points[:,:, :2] = np.indices(const.pattern_size).T.reshape(-1, 2)
pattern_points *= const.square_size
#load calibration results
rgbCamera = shelve.open(const.rgbCameraIntrinsic)
irCamera = shelve.open(const.irCameraIntrinsic)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(pattern_points,
ir_img_points,
rgb_img_points,
irCamera['camera_matrix'],
irCamera['dist_coefs'],
rgbCamera['camera_matrix'],
rgbCamera['dist_coefs'],
const.ir_image_size)
print("error:"+ str(retval))
#save calibration results
camera_file = shelve.open(const.rgbToIR, 'n')
camera_file['R'] = R
camera_file['T'] = T
camera_file.close()
def calibrate_stereo_camera(left_video, right_video, cameraMatrix_left,
distCoeffs_left, cameraMatrix_right,
distCoeffs_right):
"""
Calibrate a stereo camera pair using data
previously recorded.
"""
# Prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp_left = np.zeros((6 * 7, 3), np.float32)
objp_left[:, :2] = np.mgrid[0:6, 0:7].T.reshape(-1, 2)
objp_right = np.zeros((6 * 7, 3), np.float32)
objp_right[:, :2] = np.mgrid[0:6, 0:7].T.reshape(-1, 2)
objp_left *= 0.024
objp_right *= 0.024
# Arrays to store object points and image points from all the images.
objpoints_left = [] # 3d point in real world space
imgpoints_left = [] # 2d points in image plane.
objpoints_right = [] # 3d point in real world space
imgpoints_right = [] # 2d points in image plane.
# Get the first pair of frames
ret_left, frame_left = left_video.read()
ret_right, frame_right = right_video.read()
# Get the shape
shape = frame_left.shape
# While there are stil images to read
i = 0
while ret_left is True and ret_right is True:
gray_left = cv2.cvtColor(frame_left, cv2.COLOR_BGR2GRAY)
gray_right = cv2.cvtColor(frame_right, cv2.COLOR_BGR2GRAY)
ret_left, corners_left = cv2.findChessboardCorners(gray_left, (6, 7), None)
ret_right, corners_right = cv2.findChessboardCorners(gray_right, (6, 7), None)
# If found, add object points, image points (after refining them)
if ret_left is True and ret_right is True:
# Append object points
objpoints_left.append(objp_left)
objpoints_right.append(objp_right)
# Refine chessboard corner locations
cv2.cornerSubPix(gray_left, corners_left, (11, 11), (-1, -1), CRITERIA)
cv2.cornerSubPix(gray_right, corners_right, (11, 11), (-1, -1), CRITERIA)
# Append image points
imgpoints_left.append(corners_left)
imgpoints_right.append(corners_right)
# Draw and display the corners
cv2.drawChessboardCorners(frame_left, (6, 7), corners_left, ret_left)
cv2.drawChessboardCorners(frame_right, (6, 7), corners_right, ret_right)
print i
i = i + 1
# Display the images
cv2.imshow('frame_left', frame_left)
cv2.imshow('frame_right', frame_right)
k = cv2.waitKey(1) & 0xFF
if k == 27:
break
# Advance to the next pair of frames
ret_left, frame_left = left_video.read()
ret_right, frame_right = right_video.read()
# Calibrate the stereo camera pair
print "Calculating calibration..."
stereo_calibrate_args = {
'objectPoints': objpoints_left,
'imagePoints1': imgpoints_left,
'imagePoints2': imgpoints_right,
'imageSize': (shape[1], shape[0]),
'criteria': CRITERIA,
'flags': cv2.cv.CV_CALIB_USE_INTRINSIC_GUESS,
'cameraMatrix1': cameraMatrix_left,
'cameraMatrix2': cameraMatrix_right,
'distCoeffs1': distCoeffs_left,
'distCoeffs2': distCoeffs_right
}
_, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(**stereo_calibrate_args)
print "Calculating rectification..."
stereo_rectify_args = {
'cameraMatrix1': cameraMatrix1,
'cameraMatrix2': cameraMatrix2,
'distCoeffs1': distCoeffs1,
'distCoeffs2': distCoeffs2,
'imageSize': (shape[1], shape[0]),
'R': R,
'T': T
}
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = \
cv2.stereoRectify(**stereo_rectify_args)
print "Calculating mappings..."
map_1_left, map_2_left = cv2.initUndistortRectifyMap(cameraMatrix1,
distCoeffs1, R1, P1,
(shape[1], shape[0]),
cv2.CV_32FC1)
map_1_right, map_2_right = cv2.initUndistortRectifyMap(cameraMatrix2,
distCoeffs2, R2, P2,
(shape[1], shape[0]),
cv2.CV_32FC1)
# Save the calibration mappings for future use
np.save("test_data/map_1_left.npy", map_1_left)
np.save("test_data/map_2_left.npy", map_2_left)
np.save("test_data/map_1_right.npy", map_1_right)
np.save("test_data/map_2_right.npy", map_2_right)
def calibrate_stereo(left_filenames, right_filenames):
left_images = sorted(glob.glob(left_filenames))
right_images = sorted(glob.glob(right_filenames))
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*8,3), np.float32)
objp[:,:2] = np.mgrid[0:8,0:6].T.reshape(-1,2)
obj_points = []
img_pointsL = []
img_pointsR = []
for i in range(len(left_images)):
nameL = left_images[i]
nameR = right_images[i]
imgL = cv2.imread(nameL, 0)
imgR = cv2.imread(nameR, 0)
retL, cornersL = cv2.findChessboardCorners(imgL, (8,6), None)
retR, cornersR = cv2.findChessboardCorners(imgR, (8,6), None)
if retL and retR:
obj_points.append(objp)
cv2.cornerSubPix(imgL, cornersL, (11,11), (-1,-1), criteria)
img_pointsL.append(cornersL)
cv2.cornerSubPix(imgR, cornersR, (11,11), (-1,-1), criteria)
img_pointsR.append(cornersR)
yield False, True, (nameL,cornersL), (nameR,cornersR)
else:
yield False, False, (nameL,None), (nameR,None)
image_size = imgL.shape[::-1]
# Find individual intrinsic parameters first
retL, matrixL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
obj_points, img_pointsL, image_size, None, None)
retR, matrixR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
obj_points, img_pointsR, image_size, None, None)
# Calibrate stereo camera, with calculated intrinsic parameters
ret, matrixL, distL, matrixR, distR, R, T, E, F = cv2.stereoCalibrate(
obj_points, img_pointsL, img_pointsR, image_size,
matrixL, distL, matrixR, distR, flags=cv2.CALIB_FIX_INTRINSIC)
# Calculate rectification transforms for each camera
R1 = np.zeros((3,3), np.float32)
R2 = np.zeros((3,3), np.float32)
P1 = np.zeros((3,4), np.float32)
P2 = np.zeros((3,4), np.float32)
Q = np.zeros((4,4), np.float32)
R1, R2, P1, P2, Q, roiL, roiR = cv2.stereoRectify(
matrixL, distL, matrixR, distR, image_size, R, T)
# Create undistortion/rectification map for each camera
mapsL = cv2.initUndistortRectifyMap(matrixL, distL, R1, P1,
image_size, cv2.CV_32FC1)
mapsR = cv2.initUndistortRectifyMap(matrixR, distR, R2, P2,
image_size, cv2.CV_32FC1)
# Convert maps to fixed-point representation (faster)
mapsL = cv2.convertMaps(mapsL[0], mapsL[1], cv2.CV_32FC1)
mapsR = cv2.convertMaps(mapsR[0], mapsR[1], cv2.CV_32FC1)
calib = {
"intrinsicL": matrixL, "intrinsicR": matrixR,
"mapsL": mapsL, "mapsR": mapsR,
"R1": R1, "R2": R2, "P1": P1, "P2": P2, "Q": Q,
"roiL": roiL, "roiR": roiR
}
yield True, calib, None, None
print("rvecsL: ", rvecsL) # 旋转向量
print("tvecsL: ", tvecsL) # 平移向量
'''
# Right Side
print("Calibrating right camera...")
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objPoints, imagePointsR, imageSize, None, None)
# Calibrate the Camera for Stereo
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
print("Calibrating cameras together...")
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objPoints, imagePointsL, imagePointsR, mtxL, distL, mtxR, distR, imageSize,
None, None, None, None, flags, criteriaStereo)
print("Rectifying cameras...")
rectifyScale = 0
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
imageSize, R, T, None, None,
None, None, None,
cv2.CALIB_ZERO_DISPARITY,
rectifyScale)
print("Saving calibration...")
mapXL, mapYL = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL, imageSize,
cv2.CV_32FC1)
mapXR, mapYR = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR, imageSize,
cv2.CV_32FC1)
def calibrate_stereo_camera(self, alpha=0.0):
'''
Run stereo calibration
Returns calibration rms error (float), image height, width
and camera_matrix, dist_coeffs, projection_matrix and rectification_matrix
for right and left camera (as np.ndarray() type) as well as baseline R and T
@param alpha: Set zoom, alpha = 0.0 results in cropped image with all pixels valid in undistorted image, alpha = 1.0 preserves all pixels from original image
@type alpha: float (0.0 < alpha < 1.0)
'''
# load images
print "--> loading left images from folder '%s' with prefix '%s'" % (self.folder, self.image_prefix_left)
print "--> loading right images from folder '%s' with prefix '%s'" % (self.folder, self.image_prefix_right)
images_l = self._load_images(self.folder, self.image_prefix_left, grayscale=True)
images_r = self._load_images(self.folder, self.image_prefix_right, grayscale=True)
print "--> loaded %i left images" % len(images_l)
print "--> loaded %i right images" % len(images_r)
# get width and height of images and check all
(h, w) = images_l[0].shape[:2]
for l in images_l: assert l.shape[:2] == (h, w)
for r in images_r: assert r.shape[:2] == (h, w)
# detect image and object points in all images
print "--> detecting calibration object in all images..."
(image_points_l, object_points_l) = self._detect_points(images_l, is_grayscale=True)
(image_points_r, object_points_r) = self._detect_points(images_r, is_grayscale=True)
# sanity checks for image and object points
print "image_points_l = " + str(len(image_points_l)) + ", image_points_r = " + str(len(image_points_r))
print "object_points_l = " + str(len(object_points_l)) + ", object_points_r = " + str(len(object_points_r))
assert (len(image_points_l) == len(image_points_r))
assert (len(object_points_l) == len(object_points_r))
object_points = object_points_l
# prepare matrices
camera_matrix_l = np.zeros((3,3), np.float32)
dist_coeffs_l = np.zeros((1,5), np.float32)
camera_matrix_r = np.zeros((3,3), np.float32)
dist_coeffs_r = np.zeros((1,5), np.float32)
# mono flags
mono_flags = 0
#mono_flags |= cv2.CALIB_FIX_K3
# run monocular calibration on each camera to get intrinsic parameters
(rms_l, camera_matrix_l, dist_coeffs_l, _, _) = cv2.calibrateCamera(object_points, image_points_l, (w, h), camera_matrix_l, dist_coeffs_l, flags=mono_flags)
(rms_r, camera_matrix_r, dist_coeffs_r, _, _) = cv2.calibrateCamera(object_points, image_points_r, (w, h), camera_matrix_r, dist_coeffs_r, flags=mono_flags)
#(camera_matrix_l, _) = cv2.getOptimalNewCameraMatrix(camera_matrix_l, dist_coeffs_l, (w, h), alpha)
#(camera_matrix_r, _) = cv2.getOptimalNewCameraMatrix(camera_matrix_r, dist_coeffs_r, (w, h), alpha)
# set stereo flags
stereo_flags = 0
stereo_flags |= cv2.CALIB_FIX_INTRINSIC
# # More availabe flags...
# stereo_flags |= cv2.CALIB_USE_INTRINSIC_GUESS # Refine intrinsic parameters
# stereo_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT # Fix the principal points during the optimization.
# stereo_flags |= cv2.CALIB_FIX_FOCAL_LENGTH # Fix focal length
# stereo_flags |= cv2.CALIB_FIX_ASPECT_RATIO # fix aspect ratio
# stereo_flags |= cv2.CALIB_SAME_FOCAL_LENGTH # Use same focal length
# stereo_flags |= cv2.CALIB_ZERO_TANGENT_DIST # Set tangential distortion to zero
# stereo_flags |= cv2.CALIB_RATIONAL_MODEL # Use 8 param rational distortion model instead of 5 param plumb bob model
# run stereo calibration
res = cv2.stereoCalibrate(object_points, image_points_l, image_points_r, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (w, h), flags=stereo_flags)
(rms_stereo, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, R, T, E, F) = res
# run stereo rectification
res = self._rectify(camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (h, w), R, T, alpha)
(rectification_matrix_l, rectification_matrix_r, projection_matrix_l, projection_matrix_r) = res
# # DEBUG: check types -> should all be np.ndarray
# print type(camera_matrix_l)
# print type(dist_coeffs_l)
# print type(rectification_matrix_l)
# print type(projection_matrix_l)
# print type(camera_matrix_r)
# print type(dist_coeffs_r)
# print type(rectification_matrix_r)
# print type(projection_matrix_r)
# print type(R)
# print type(T)
return ((rms_l, rms_r, rms_stereo),
camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l,
camera_matrix_r, dist_coeffs_r, rectification_matrix_r, projection_matrix_r,
(h, w), R, T)
continue
cv2.destroyAllWindows()
print("calibrating camera")
print(50 * "=")
(_ , cameraMatrixRight, distortionRight, _, _) = cv2.calibrateCamera(points3DList, cornersListRight, \
rightFrame.shape[::-1], None, None)
(_, cameraMatrixLeft, distortionLeft, _, _) = cv2.calibrateCamera(points3DList, cornersListLeft, \
leftFrame.shape[::-1], None, None)
TERMINATION_CRITERIA = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_MAX_ITER,
30, 0.001)
(_, _, _, _, _, rotationMatrix, translationVector, _,
_) = cv2.stereoCalibrate(points3DList, cornersListLeft, cornersListRight,
cameraMatrixLeft, distortionLeft,
cameraMatrixRight, distortionRight,
rightFrame.shape[::-1], None, None, None, None,
cv2.CALIB_FIX_INTRINSIC, TERMINATION_CRITERIA)
print("saving numpy files")
print(50 * "=")
np.save("cameraMatrixRight.npy", cameraMatrixRight)
np.save("cameraMatrixLeft.npy", cameraMatrixLeft)
np.save("distortionLeft.npy", distortionLeft)
np.save("distortionRight.npy", distortionRight)
np.save("rotationMatrix.npy", rotationMatrix)
np.save("translationVector.npy", translationVector)
cameraProjectionMatrixLeft = np.dot(cameraMatrixLeft, np.c_[np.eye(3),
np.zeros(
def stop_recording(self):
self.chb_dimensions_spin.setEnabled(True)
self.chb_size_x_spin.setEnabled(True)
self.chb_size_y_spin.setEnabled(True)
self.start_calib_btn.setText('Start recording')
self.recording = False
min_frames = 10
if len(self.corners['left']) < min_frames:
QMessageBox.error(
self, "Not enough images",
"""You have not recorded enough images of the checkerboard. Please record at least {} images to calibrate"""
.format(min_frames), QMessageBox.Ok)
return
self.status_bar_update.emit('Calibrating left camera intrinsics')
rmsL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
self.object_points,
self.corners['left'], (self.res_width, self.res_height),
None,
None,
flags=cv2.CALIB_ZERO_TANGENT_DIST)
print('Left camera RMS reprojection error {}'.format(rmsL))
if rmsL > 1:
rospy.logwarn(
'The reprojection error is very large. Take more images from different view points. Make sure That the checkerboard covers all parts of the image in some frames. Take images from close up, far away and shallow angles of attack.'
)
self.status_bar_update.emit('Calibrating right camera intrinsics')
rmsR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
self.object_points,
self.corners['right'], (self.res_width, self.res_height),
None,
None,
flags=cv2.CALIB_ZERO_TANGENT_DIST)
print('Right camera RMS reprojection error {}'.format(rmsR))
if rmsR > 1:
rospy.logwarn(
'The reprojection error is very large. Take more images from different view points. Make sure That the checkerboard covers all parts of the image in some frames. Take images from close up, far away and shallow angles of attack.'
)
self.status_bar_update.emit('Calibrating extrinsics')
rmsStereo, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
self.object_points,
self.corners['left'],
self.corners['right'],
mtxL,
distL,
mtxR,
distR, (self.res_width, self.res_height),
flags=cv2.CALIB_FIX_INTRINSIC + cv2.CALIB_ZERO_TANGENT_DIST)
print('Stereo RMS reprojection error {}'.format(rmsStereo))
if rmsStereo > 1:
rospy.logwarn(
'The reprojection error is very large. Take more images from different view points. Make sure That the checkerboard covers all parts of the image in some frames. Take images from close up, far away and shallow angles of attack.'
)
thresh = 1 # threshold for RMS warning
if rmsL > thresh or rmsR > thresh or rmsStereo > thresh:
reply = QMessageBox.warning(
self, "High reprojection error",
"""The reprojection error is very large.\nRMS reprojection error left: {:.2f}, right: {:.2f}, stereo: {:.2f}.\
\nTake more images from different view points.\
Make sure That the checkerboard covers all parts of the image in some frames.\
\nTake images from close up, far away and shallow angles of attack.\
\n\nWould you like to save this calibration anyway?"""
.format(rmsL, rmsR, rmsStereo),
QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
if reply == QMessageBox.No:
return
self.open_calibration_file()
self.calib['R_lr'] = R.T.tolist()
self.calib['R_rl'] = R.tolist()
self.calib['CameraParameters1'][
'DistortionModel'] = 'plumb_bob' # opencv parameters are the same as matlab
self.calib['CameraParameters1']['FocalLength'][0] = float(mtxL[0][0])
self.calib['CameraParameters1']['FocalLength'][1] = float(mtxL[1][1])
self.calib['CameraParameters1']['PrincipalPoint'][0] = float(
mtxL[0][2])
self.calib['CameraParameters1']['PrincipalPoint'][1] = float(
mtxL[1][2])
self.calib['CameraParameters1']['RadialDistortion'][0] = float(
distL[0][0])
self.calib['CameraParameters1']['RadialDistortion'][1] = float(
distL[0][1])
self.calib['CameraParameters1']['RadialDistortion'][2] = float(
distL[0][4])
self.calib['CameraParameters2']['DistortionModel'] = 'plumb_bob'
self.calib['CameraParameters2']['FocalLength'][0] = float(mtxR[0][0])
self.calib['CameraParameters2']['FocalLength'][1] = float(mtxR[1][1])
self.calib['CameraParameters2']['PrincipalPoint'][0] = float(
mtxR[0][2])
self.calib['CameraParameters2']['PrincipalPoint'][1] = float(
mtxR[1][2])
self.calib['CameraParameters2']['RadialDistortion'][0] = float(
distR[0][0])
self.calib['CameraParameters2']['RadialDistortion'][1] = float(
distR[0][1])
self.calib['CameraParameters2']['RadialDistortion'][2] = float(
distR[0][4])
with open(self.calib_path, 'w') as outfile:
outfile.write(yaml.dump(self.calib, default_flow_style=None))
self.status_bar_update.emit('Wrote calibration to {}'.format(
self.calib_path))
rospy.loginfo('Wrote calibration to {}'.format(self.calib_path))
def stereo_calibration(check_img_folder, nimages, display=False, dims=(4, 11), size=(640, 480)):
"""reads files from a directory of stereo images of opencv circle grid. calibrates intrinsics of each camera, then extrinsics of stereo rig
"""
# grab calbration frames directory
for dir in os.listdir(check_img_folder):
if fnmatch.fnmatch(dir, "calibration_frames*"):
check_img_folder = check_img_folder + dir + "/"
break
# Number of points in circle grid
num_pts = dims[0] * dims[1]
if not os.path.exists(check_img_folder + "images_used/"):
os.mkdir(check_img_folder + "images_used/")
# evaluate image points
nimg = 0 # number of images with found corners
iptsF1 = [] # image point arrays to fill up
iptsF2 = []
random_images = random.sample(range(500), nimages)
# for n in range(0,nimages,2):
for n in random_images:
filename1 = check_img_folder + "cam1_frame_" + str(n + 1) + ".bmp"
filename2 = check_img_folder + "cam2_frame_" + str(n + 1) + ".bmp"
if os.path.exists(filename1) and os.path.exists(filename2):
img1 = cv2.imread(filename1, 0)
img2 = cv2.imread(filename2, 0)
# find center points in circle grid
[found1, points1] = cv2.findCirclesGridDefault(img1, dims, flags=(cv2.CALIB_CB_ASYMMETRIC_GRID))
[found2, points2] = cv2.findCirclesGridDefault(img2, dims, flags=(cv2.CALIB_CB_ASYMMETRIC_GRID))
# copy the found points into the ipts matrices
temp1 = np.zeros((num_pts, 2))
temp2 = np.zeros((num_pts, 2))
if found1 and found2:
for i in range(num_pts):
temp1[i, 0] = points1[i, 0, 0]
temp1[i, 1] = points1[i, 0, 1]
temp2[i, 0] = points2[i, 0, 0]
temp2[i, 1] = points2[i, 0, 1]
iptsF1.append(temp1)
iptsF2.append(temp2)
nimg = nimg + 1 # increment image counter
# save images with points identified
drawn_boards_1 = img1.copy()
drawn_boards_2 = img2.copy()
cv2.drawChessboardCorners(drawn_boards_1, dims, points1, found1)
cv2.drawChessboardCorners(drawn_boards_2, dims, points2, found2)
cv2.imwrite(check_img_folder + "images_used/" + "cam1_frame_" + str(n + 1) + ".bmp", drawn_boards_1)
cv2.imwrite(check_img_folder + "images_used/" + "cam2_frame_" + str(n + 1) + ".bmp", drawn_boards_2)
print "\n Usable stereo pairs: " + str(nimg)
# convert image points to numpy
iptsF1 = np.array(iptsF1, dtype=np.float32)
iptsF2 = np.array(iptsF2, dtype=np.float32)
# evaluate object points
opts = object_points(dims, nimg, 4.35)
# initialize camera parameters
intrinsics1 = np.zeros((3, 3))
intrinsics2 = np.zeros((3, 3))
distortion1 = np.zeros((8, 1))
distortion2 = np.zeros((8, 1))
# Set initial guess for intrinsic camera parameters (focal length = 0.35cm)
intrinsics1[0, 0] = 583.3
intrinsics1[1, 1] = 583.3
intrinsics1[0, 2] = 320
intrinsics1[1, 2] = 240
intrinsics1[2, 2] = 1.0
intrinsics2[0, 0] = 583.3
intrinsics2[1, 1] = 583.3
intrinsics2[0, 2] = 320
intrinsics2[1, 2] = 240
intrinsics2[2, 2] = 1.0
# calibrate cameras
print "Calibrating camera 1..."
(cam1rms, intrinsics1, distortion1, rotv1, trav1) = cv2.calibrateCamera(
opts,
iptsF1,
size,
intrinsics1,
distortion1,
flags=int(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL),
)
print "\nEstimated intrinsic parameters for camera 1:"
for i in range(3):
print [intrinsics1[i, j] for j in range(3)]
print "\nEstimated distortion parameters for camera 1:"
print distortion1
print "Calibrating camera 2..."
(cam2rms, intrinsics2, distortion2, rotv2, trav2) = cv2.calibrateCamera(
opts,
iptsF2,
size,
intrinsics2,
distortion2,
flags=int(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL),
)
print "\nEstimated intrinsic parameters for camera 2:"
for i in range(3):
print [intrinsics2[i, j] for j in range(3)]
print "\nEstimated distortion parameters for camera 2:"
print distortion2
print "\n rms pixel error:"
print "cam1 orig: " + str(cam1rms)
print "cam2 orig: " + str(cam2rms)
# Estimate extrinsic parameters from stereo point correspondences
print "\n Stereo estimating..."
# (stereorms, intrinsics1, distortion1, intrinsics2, distortion2, R, T, E, F) = cv2.stereoCalibrate(opts, iptsF1, iptsF2, intrinsics1, distortion1, intrinsics2, distortion2, size,criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 300, 1e-7), flags=(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL))
(stereorms, intrinsics1, distortion1, intrinsics2, distortion2, R, T, E, F) = cv2.stereoCalibrate(
opts,
iptsF1,
iptsF2,
size,
intrinsics1,
distortion1,
intrinsics2,
distortion2,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 300, 1e-7),
flags=(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL),
)
print "\nEstimated extrinsic parameters between cameras 1 and 2:\nRotation:"
for i in range(3):
print [R[i, j] for j in range(3)]
print "\nTranslation:"
print [T[i, 0] for i in range(3)]
print("prev")
print(front_cam.intrinsic, front_cam.dist)
print(back_cam.intrinsic, back_cam.dist)
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
# ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
# objpoints, imgpoints_front,
# imgpoints_back, front_cam.intrinsic, front_cam.dist, back_cam.intrinsic,
# back_cam.dist, (1920, 1080),
# criteria=stereocalib_criteria, flags=flags)
ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
objpoints,
imgpoints_front,
imgpoints_back,
front_cam.intrinsic,
np.array([0, 0, 0, 0, 0]),
back_cam.intrinsic,
np.array([0, 0, 0, 0, 0]), (1920, 1080),
criteria=stereocalib_criteria,
flags=flags)
print("back")
print(front_cam.intrinsic, front_cam.dist)
print(back_cam.intrinsic, back_cam.dist)
print('Intrinsic_mtx_1', M1)
print('dist_1', d1)
print('Intrinsic_mtx_2', M2)
print('dist_2', d2)
print('R', R)
print('T', T)
def calibrate(self):
##
undisorted = False
cheesboard_found = False
possib_cho = ['N', 'Y']
user_choice = ""
# object points, depend on cheesboard size
objp = np.zeros((np.prod(self.cheesboard_size),3)
, np.float32)
objp[:,:2] = np.mgrid[0:self.cheesboard_size[0],0:self.cheesboard_size[1]]\
.T.reshape(-1,2)
objp = objp*self.cheesboard_scale
raw_input("Press any key to start calibration...")
while True:
# Read single frame from camera
for i in range(0,5):
_, frame_l = self.cam_l.read()
_, frame_r = self.cam_r.read()
# Frame sizes
rows,cols,dim = frame_l.shape
M = cv2.getRotationMatrix2D((cols/2,rows/2),-90,1)
frame_l = cv2.warpAffine(frame_l,M,(cols,rows))
frame_r = cv2.warpAffine(frame_r,M,(cols,rows))
# Convert to grayscale
gray_l = cv2.cvtColor(frame_l,cv2.COLOR_BGR2GRAY)
gray_r = cv2.cvtColor(frame_r,cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret_l, corners_l = cv2.findChessboardCorners(gray_l,
self.cheesboard_size, None)
ret_r, corners_r = cv2.findChessboardCorners(gray_r,
self.cheesboard_size, None)
# If chessboard corners found
if ret_l and ret_r and not undisorted:
print "Left Camera: Cheesboard Found"
print "Right Camera: Cheesboard Found"
self.objpoints.append(objp)
cv2.cornerSubPix(gray_l, corners_l,(5, 5),(-1,-1), self.criteria)
cv2.cornerSubPix(gray_r, corners_r,(5, 5),(-1,-1), self.criteria)
# Add image points to buffer
self.imgpoints_l.append(corners_l)
self.imgpoints_r.append(corners_r)
# Draw and display the corners
cv2.drawChessboardCorners(frame_l, self.cheesboard_size, corners_l, ret_l)
cv2.drawChessboardCorners(frame_r, self.cheesboard_size, corners_r, ret_r)
if self.lowres:
# Put text on image
cv2.putText(frame_l,"Found!!!", (100,rows/2), cv2.FONT_HERSHEY_SIMPLEX,
2,(255,255,255),5, 1)
cv2.putText(frame_r,"Found!!!", (100,rows/2), cv2.FONT_HERSHEY_SIMPLEX,
2,(255,255,255),5, 1)
else:
# Put text on image
cv2.putText(frame_l,"Found!!!", (100,rows/2), cv2.FONT_HERSHEY_SIMPLEX,
4,(255,255,255),5, 1)
cv2.putText(frame_r,"Found!!!", (100,rows/2), cv2.FONT_HERSHEY_SIMPLEX,
4,(255,255,255),5, 1)
self.img_counter = self.img_counter + 1
cheesboard_found = True
elif undisorted:
frames = [frame_l, frame_r]
maps = [left_maps, right_maps]
rect_frames = [0,0]
for src in self.cam_sources:
rect_frames[src] = cv2.remap(frames[src], maps[src][0], maps[src][1], cv2.INTER_LANCZOS4)
# Undistorted images
frame_l = rect_frames[0]
frame_r = rect_frames[1]
# show image
cv2.imshow('img_left',frame_l)
cv2.imshow('img_right',frame_r)
k = cv2.waitKey(5) & 0xFF
# Founded?, What next
if cheesboard_found:
while user_choice not in possib_cho:
user_choice = raw_input("Found: %d , continue (Y/N) : " % (self.img_counter,))
cheesboard_found = False
# If user chose No
if user_choice == possib_cho[0]:
print "Using calculated parameters to remove disortion..."
if not self.isOpen3():
# Calculating disortion params for stereo camera
ret_val,camera_mat_l, dist_l, camera_mat_r, dist_r, R, T, E, F = cv2.stereoCalibrate(
self.objpoints, self.imgpoints_l,
self.imgpoints_r, gray_l.shape[::-1]
)
# distortion params
dist = [dist_l, dist_r]
# Remove translation
for src in self.cam_sources:
dist[src][0][-1] = 0.0
else:
# Calculating disortion params for each camera
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(self.objpoints,
self.imgpoints_l,
gray_l.shape[::-1],
None,None)
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(self.objpoints,
self.imgpoints_r,
gray_r.shape[::-1],
None,None)
print "Calibration Error for left, right camera : " + str(ret_l) + ", " + str(ret_r)
# distortion params
dist = [dist_l, dist_r]
# Remove translation
for src in self.cam_sources:
dist[src][0][-1] = 0.0
## find new camera and remove translation params
camera_mat_l, roi_l=cv2.getOptimalNewCameraMatrix(mtx_l,dist_l,(cols,rows),1,(cols,rows))
camera_mat_r, roi_r=cv2.getOptimalNewCameraMatrix(mtx_r,dist_r,(cols,rows),1,(cols,rows))
stereorms, mtx_l, dist_l, mtx_r,\
dist_r, R, T, E, F = cv2.stereoCalibrate(
objectPoints=self.objpoints, imagePoints1=self.imgpoints_l,
imagePoints2=self.imgpoints_r, cameraMatrix1=camera_mat_l,
distCoeffs1=dist_l, cameraMatrix2=camera_mat_r,
distCoeffs2=dist_r, imageSize=gray_l.shape[::-1],
flags=(cv2.CALIB_FIX_INTRINSIC | cv2.CALIB_RATIONAL_MODEL)
)
print "Calibration Error for both cameras : " + str(stereorms)
# camera matrixes
cam_mats = [camera_mat_l, camera_mat_r]
# Crop option
rectify_scale = 0
# Rectification
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
camera_mat_l, dist_l,
camera_mat_r, dist_r,
gray_l.shape[::-1], R, T, alpha = rectify_scale
)
left_maps = cv2.initUndistortRectifyMap(camera_mat_l, dist_l, R1, P1, gray_l.shape[::-1], cv2.CV_16SC2)
right_maps = cv2.initUndistortRectifyMap(camera_mat_r, dist_r, R2, P2, gray_l.shape[::-1], cv2.CV_16SC2)
## Save result
np.savez(
"disortion_params.npz", camera_mat_l=camera_mat_l, camera_mat_r=camera_mat_r, dist_l=dist_l,
dist_r=dist_r, R=R, T=T, E=E,
F=F, left_maps=left_maps, right_maps=right_maps
)
undisorted = True
user_choice = ""
if k == 27:
break
cv2.destroyAllWindows()
def stereoCalibrate():
board_n = board_w * board_h
board_sz = (board_w, board_h)
# cv2.namedWindow('Stereo Calibration', cv2.WINDOW_AUTOSIZE)
board_pts = inverted_seq(board_sz, square_sz)
image1, image2, image1_pts, image2_pts, object_pts, imgSize = getMatchedPtsByChessboard(
[3], board_sz, board_pts)
#image1, image2, image1_pts, image2_pts, object_pts, imgSize = getMatchedPts()
# print image1_pts, image2_pts, object_pts, imgSize
cameraMatrix1 = np.identity(3, np.float32); cameraMatrix2 = np.identity(3, np.float32)
distCoeffs1 = np.zeros((5), np.float32); distCoeffs2 = np.zeros((5), np.float32)
#CALIBRATE THE CAMERA
flags = cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_SAME_FOCAL_LENGTH
criteria = (1 + 2, 100, 1e-5) # 1 for MAX_ITER and 2 for EPS |= =|
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
object_pts, image1_pts, image2_pts, cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2, imgSize, flags = flags, criteria = criteria)
print 'Re-projection error', retval
print 'Intrinsic matrix 1\n', cameraMatrix1
print 'Intrinsic matrix 2\n', cameraMatrix2
print 'Distortion coeffs 1\n', distCoeffs1
print 'Distortion coeffs 2\n', distCoeffs2
print 'Rotation matrix\n', R
print 'Translation vector\n', T
print 'Eigen matrix\n', E
print 'Fundamental matrix\n', F
cameraMatrix = cameraMatrix1
cameraMatrix[:2,2] = (imgSize[0] / 2, imgSize[1] / 2)
distCoeffs = np.zeros((1, 5), np.float32)
if True:
distCoeffs1 = distCoeffs2 = distCoeffs
cameraMatrix1 = cameraMatrix2 = cameraMatrix
image1 = cv2.undistort(image1, cameraMatrix1, distCoeffs1)
image2 = cv2.undistort(image2, cameraMatrix2, distCoeffs2)
#CALIBRATION QUALITY CHECK
#Using the epipolar geometry constraint: m2' * F * m1 = 0
assert image1_pts[0].shape == image2_pts[0].shape
newshape = image1_pts[0].shape
newshape = (newshape[0], 1, newshape[1])
#Work in undistorted space (An undocumented signature is used here. @Dstray)
image1_pts = cv2.undistortPoints(image1_pts[0].reshape(newshape, order = 'F'),
cameraMatrix1, distCoeffs1, P = cameraMatrix1)
image2_pts = cv2.undistortPoints(image2_pts[0].reshape(newshape, order = 'F'),
cameraMatrix2, distCoeffs2, P = cameraMatrix2)
points1, points2 = image1_pts.reshape(-1, 2), image2_pts.reshape(-1, 2)
#implemented in opencv 3.0.0
if False:
lines1 = cv2.computeCorrespondEpilines(points2, 2, F).reshape(-1, 3)
lines2 = cv2.computeCorrespondEpilines(points1, 1, F).reshape(-1, 3)
image1, image2 = drawlines(image1, image2, lines1, points2)
image2, image1 = drawlines(image2, image1, lines2, points1)
cv2.imshow('Image 1', image1); cv2.imshow('Image 2', image2)
cv2.waitKey(0)
#COMPUTE AND DISPLAY RECTIFICATION
urmaps, Q = bouguetRectify(image1_pts, image2_pts, imgSize,
cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, R, T)
'''urmaps, F = hartleyRectify(image1_pts, image2_pts, imgSize,
cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, F)'''
print 'Q\n', Q
Q = np.float32([[1, 0, 0, -0.5*imgSize[0]],
[0, 1, 0, -0.5*imgSize[1]],
[0, 0, 0, cameraMatrix[0][0]],
[0, 0, 0.07, 0]])
#Save the results
np.save('rectmap.npy', urmaps)
np.save('Q.npy', Q)
#RECTIFY THE IMAGES AND FIND DISPARITY MAPS
#Setup for finding correspondences
img1r = cv2.remap(image1, urmaps[0], urmaps[1], cv2.INTER_LINEAR)
img2r = cv2.remap(image2, urmaps[2], urmaps[3], cv2.INTER_LINEAR)
cv2.imshow('Image 1', img1r); cv2.imshow('Image 2', img2r)
cv2.waitKey(0)
'''disp, points = stereoBMMatch(img1r, img2r, Q)
colors = cv2.cvtColor(image1, cv2.COLOR_BGR2RGB)
mask = disp > disp.min()
points = points[mask]; colors = colors[mask]
write_ply('_3DPoints.ply', points, colors)'''
#stereosgbm_match(img1r, img2r, '_3DPoints.ply', Q)
cv2.waitKey(0)
cv2.destroyAllWindows()
if (key == 115 and not options.oneCamera):
savedCorners1 += [corners1];
savedCorners2 += [corners2];
objectPoints += [patternPoints];
elif (key == 115 and options.oneCamera):
raise BaseException("Option S is only available with two cameras");
h, w = img1.shape[:2]
cameraMatrix1 = None;
cameraMatrix2 = None;
distCoeffs1 = None;
distCoeffs2 = None;
R,T,E,F = [None,None,None,None]
if (len(savedCorners1)>0):
try:
ret,cameraMatrix1,distCoeffs1,cameraMatrix2,distCoeffs2,R,T,E,F = cv2.stereoCalibrate(objectPoints, savedCorners1, savedCorners2, (w,h), cameraMatrix1, distCoeffs1,cameraMatrix2,distCoeffs2, R,T,E,F);
except:
print len(savedCorners1);
print len(savedCorners2);
print savedCorners1;
print savedCorners2;
else:
ret,cameraMatrix1,distCoeffs1,cameraMatrix2,distCoeffs2,R,T,E,F = cv2.stereoCalibrate([patternPoints], [corners1], [corners2], (w,h), cameraMatrix1, distCoeffs1,cameraMatrix2,distCoeffs2, R,T,E,F);
print "------------------------"
printMatrix(R);
r = cv2.Rodrigues(R)[0];
try:
theta, vector = parseRotationMatrix(R);
print "Theta: %10.7f (%d)" % (theta, theta*360/(2*np.pi));
print "Vector: "+printVector(vector,returnString=True);
def cal_fromcorners(self):
'''This function based on code from the ROS camera calibration package.'''
ret = self.get_calib_data()
if ret is None: # If too few correspondencies, just quit.
return
cam1_ipts, cam2_ipts, b = ret
#TODO add debug flag for useful printouts like this:
"""
print "Calibration data info:"
print " CAM1 data array shape:\t %s" % str(cam1_ipts.shape)
print " CAM2 data array shape:\t %s" % str(cam2_ipts.shape)
print " BOARD data array shape:\t %s" % str(b.shape)
"""
if self.cam1_do_mono_calib or self.cam2_do_mono_calib:
print "Doing mono calibration for specified cameras."
self.mono_cal_fromcorners(cam1_ipts, cam2_ipts, b)
print "Doing stereo calibration..."
flags = cv2.CALIB_FIX_INTRINSIC # Intrinsics must be provided.
self.T = numpy.zeros((3, 1), dtype=numpy.float64)
self.R = numpy.eye(3, dtype=numpy.float64)
#self.cam2_D = np.zeros((5,1), np.float64)
if LooseVersion(cv2.__version__).version[0] == 2:
cv2.stereoCalibrate(b, cam1_ipts, cam2_ipts,
self.cam1_size,
self.cam1_K, self.cam1_D,
self.cam2_K, self.cam2_D,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
else:
cv2.stereoCalibrate(self.CHESSBOARD, cam1_ipts, cam2_ipts,
self.cam1_K, self.cam1_D,
self.cam2_K, self.cam2_D,
self.size,
self.R, # R
self.T, # T
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
flags = flags)
print "=======STEREO======"
print "R = "
print self.R,
print "\nT = "
print self.T
homog = np.hstack((self.R, np.array([[0],[0],[0]])))
homog = np.vstack((homog, np.array([0,0,0,1])))
print "\nR as a quaternion:"
q = tf.transformations.quaternion_from_matrix(homog)
print q
# NOTE quat_from_mat() doesn't return a quaternion object!
# But the Pose msg requires one!
# Fun times if ya like debugging cryptic errors.
q_msg = Quaternion(q[0], q[1], q[2], q[3])
t_msg = Point(self.T[0], self.T[1], self.T[2])
if self.save_calib_results:
filename = self.save_data_dir + "stereoParams.yaml"
while os.path.isfile(filename):
root, ext = os.path.splitext(filename)
filename = root + "_new" + ext
data = dict(
R = self.R.flatten().tolist(),
T = self.T.flatten().tolist(),
R_quaternion = q.flatten().tolist())
with open(filename,'w') as f:
yaml.dump(data, f, default_flow_style=False)
print "===Saved results to " + filename
else:
"Results not saved."
print "~~~ Calibration finished! ~~~"
'''
def calibrateImages(obj_pts, ptsL, ptsR, imsize):
'''Calibrate the Stereo camera rig, given:
@obj_pts: points that specify the calibration checkerboard pattern
@ptsL: detected image points in the left calibration images
@ptsR: detected image points in the right calibration images
@imsize: the stipulated size of all calibration images
return: updated StereoCam object
'''
# Perform Stereo Calibration
retval, cam1, dist1, cam2, dist2, R, T, E, F = \
cv2.stereoCalibrate(
obj_pts, ptsL, ptsR, imsize)
dist1 = dist1.ravel()
dist2 = dist2.ravel()
cameraObj = StereoCam()
cameraObj.cam1, cameraObj.dist1 = cam1, dist1
cameraObj.cam2, cameraObj.dist2 = cam2, dist2
cameraObj.R, cameraObj.T, cameraObj.E, cameraObj.F = R, T, E, F
# Perform Stereo Rectification
(R1, R2, P1, P2, Q, roi1, roi2) = \
cv2.stereoRectify(cam1, dist1, cam2, dist2, imsize, R, T)
# update the left and right rotation and projection matrices
cameraObj.R1, cameraObj.R2 = R1, R2
cameraObj.P1, cameraObj.P2 = P1, P2
# update the image projection (aka disparity-to-depth mapping) matrix
cameraObj.Q = Q
# Get optimal new camera matrices from the rectified projection matrices
K_L = cv2.decomposeProjectionMatrix(P1)
K1 = K_L[0]
K_R = cv2.decomposeProjectionMatrix(P2)
K2 = K_R[0]
cameraObj.K1, cameraObj.K2 = K1, K2
return cameraObj
