def stereo_initialize(): #left camera_matrixL = np.array([[219.44178, 0., 157.85633], [0., 219.82357, 122.50906], [0.,0.,1.]]) dist_coefsL = np.array([-0.35878, 0.10911, -0.00069, -0.00088, 0.]) #right camera_matrixR = np.array([[216.15875, 0., 163.36207], [0., 216.12017, 118.46570], [0.,0.,1.]]) dist_coefsR = np.array([-0.37513, 0.13854, -0.00075, 0.00137, 0.]) #extrinsic om = np.array([-0.00549, 0.01268, -0.06347]) T = np.array([-63.36115, 0.61235, -1.71106]) R1 = np.zeros(shape=(3,3)) R2 = np.zeros(shape=(3,3)) P1 = np.zeros(shape=(3,3)) P2 = np.zeros(shape=(3,3)) R = cv2.Rodrigues(om) cv2.stereoRectify(camera_matrixL, dist_coefsL, camera_matrixR, dist_coefsR,(320, 240), R[0], T, R1, R2, P1, P2, Q=None, alpha=-1, newImageSize=(0,0)) map1x, map1y = cv2.initUndistortRectifyMap(camera_matrixL, dist_coefsL, R1, camera_matrixL, (320, 240), cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(camera_matrixR, dist_coefsR, R2, camera_matrixR, (320, 240), cv2.CV_32FC1) return map1x, map1y, map2x, map2y
def fromCamchain(camchain, cam1_no, cam2_no, alpha=0.0): res = StereoCameraModel() res.cam1 = CameraModel.fromCamchain(camchain, cam1_no) res.cam2 = CameraModel.fromCamchain(camchain, cam2_no) if res.cam2.T is not None: R = res.cam2.T[0:3,0:3] T = res.cam2.T[0:3,3] #print res.cam2.T, R,T else: print 'error' return None # construct R cv2.stereoRectify(res.cam1.K, res.cam1.D, res.cam2.K, res.cam2.D, res.cam1.size, R, T, res.cam1.R, res.cam2.R, res.cam1.P, res.cam2.P, alpha = alpha) return res
def triangulate_features(K,d,train_pts,test_pts,R,T): R1, R2, P_1, P_2, Q, roi1, roi2 = cv2.stereoRectify(K, d, K, d, train_frame.shape[:2], R, T, alpha=1) points1 = np.vstack(train_pts)[:,:-1].T #points1 = cv2.convertPointsToHomogeneous(points1) points2 = np.vstack(test_pts)[:,:-1].T #points2 = cv2.convertPointsToHomogeneous(points2) #threeD_points = cv2.triangulatePoints(P1, P2, points1[:2], points2[:2]) normRshp1 = points1 normRshp2 = points2 Tpoints1 = cv2.triangulatePoints(P_1, P_2, normRshp1, normRshp2) fin_pts = [] #print Tpoints1[0] #print Tpoints1[1] #print Tpoints1[2] #print Tpoints1[3] a = (Tpoints1/Tpoints1[3])[0:3] #print a #print a.shape a = a.T a = a/100 print a max_ax = 0 max_ay = 0 max_az = 0 for zz in range(a.shape[0]): if(abs(a[zz][0]-abs(max_ax))< max_ax + 10) and (abs(a[zz][1]-abs(max_ay))< max_ay + 10) and (abs(a[zz][2]-abs(max_az))< max_az + 10): if(a[zz][0] > abs(max_ax)): max_ax = a[zz][0] if(a[zz][1] > abs(max_ay)): max_ay = a[zz][1] if(a[zz][2] > abs(max_az)): max_az = a[zz][2] az.scatter(a[zz][0] + new_origin[0], a[zz][1] + new_origin[1], a[zz][2] + new_origin[2], c='g', marker='o')
def draw_rect(K, d, train_frame, R, T, name): #perform the rectification R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(K, d, K, d, train_frame.shape[:2], R, T, alpha=1) mapx1, mapy1 = cv2.initUndistortRectifyMap(K, d, R1, K, train_frame.shape[:2], cv2.CV_32F) mapx2, mapy2 = cv2.initUndistortRectifyMap(K, d, R2, K, query_frame.shape[:2], cv2.CV_32F) img_rect1 = cv2.remap(train_bckp, mapx1, mapy1, cv2.INTER_LINEAR) img_rect2 = cv2.remap(query_bckp, mapx2, mapy2, cv2.INTER_LINEAR) # draw the images side by side total_size = (max(img_rect1.shape[0], img_rect2.shape[0]), img_rect1.shape[1] + img_rect2.shape[1],3) img = np.zeros(total_size, dtype=np.uint8) img[:img_rect1.shape[0], :img_rect1.shape[1]] = img_rect1 img[:img_rect2.shape[0], img_rect1.shape[1]:] = img_rect2 # draw horizontal lines every 25 px accross the side by side image for i in range(20, img.shape[0], 25): cv2.line(img, (0, i), (img.shape[1], i), (255, 0, 0)) h1, w1 = train_frame.shape[:2] h2, w2 = query_frame.shape[:2] org_imgs = np.zeros((max(h1, h2), w1+w2,3), np.uint8) org_imgs[:h1, :w1] = train_bckp org_imgs[:h2, w1:w1+w2] = query_bckp for m in good: # draw the keypoints # print m.queryIdx, m.trainIdx, m.distance color = tuple([np.random.randint(0, 255) for _ in xrange(3)]) cv2.line(org_imgs, (int(train_keypoints[m.queryIdx].pt[0]), int(train_keypoints[m.queryIdx].pt[1])) , (int(query_keypoints[m.trainIdx].pt[0] + w1), int(query_keypoints[m.trainIdx].pt[1])), color) cv2.circle(org_imgs, (int(train_keypoints[m.queryIdx].pt[0]), int(train_keypoints[m.queryIdx].pt[1])) , 5, color, 1) cv2.circle(org_imgs, (int(query_keypoints[m.trainIdx].pt[0] + w1), int(query_keypoints[m.trainIdx].pt[1])) , 5, color, 1) cv2.imshow('original', org_imgs) cv2.imshow(name, img)
def compute_rectification_transforms(intrinsics_left, intrinsics_right, image_size, rotation_matrix, translation_vector): ''' Computes rectification transforms for stereo vision system Arguments: intrinsics_left -- a tuple (camera_matrix_left, dist_coefs_left) containing left camera intrinsic parameters: camera matrix and distortion coeffitient intrinsics_right -- a tuple (camera_matrix_right, dist_coefs_right) containing right camera intrinsic parameters: camera matrix and distortion coeffitient image_size -- a tuple describing the size of an image in pixels (returned by cvfunctions.images.get_image_size function) rotation_matrix -- rotation matrix between left and right camera coordinate systems translation_vector -- translation vector between left and right camera coordinate systems Returns a tuple as a result of the cv2.stereoRectify function call, containing the following results: R1, R2, P1, P2, Q, validPixROI1, validPixROI2 ''' camera_matrix_left, dist_coefs_left = intrinsics_left camera_matrix_right, dist_coefs_right = intrinsics_right res = cv2.stereoRectify(camera_matrix_left, dist_coefs_left, camera_matrix_right, dist_coefs_right, image_size, rotation_matrix, translation_vector) return res
def rectify(mtx1, dist1, mtx2, dist2, R, T): # R:行对准的矫正旋转矩阵;P:3*4的左右投影方程;Q:4*4的重投影矩阵 R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify( mtx1, dist1, mtx2, dist2, (BINIMG_W, BINIMG_H), R, T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=-1, newImageSize=(BINIMG_W, BINIMG_H) ) if __name__ == '__main__': printMat(R1, R2, P1, P2, Q, roi1, roi2) # 产生校正图像所需的变换参数(mapx, mapy) mapx1, mapy1 = cv2.initUndistortRectifyMap( mtx1, dist1, R1, P1, (BINIMG_W, BINIMG_H), cv2.CV_16SC2 ) mapx2, mapy2 = cv2.initUndistortRectifyMap( mtx2, dist2, R2, P2, (BINIMG_W, BINIMG_H), cv2.CV_16SC2 ) return mapx1, mapy1, mapx2, mapy2, Q, roi1, roi2
def __stereo(self, left, right): # return self.undistortLeft(left), self.undistortRight(right) if not self.maps_read: # clear init flag self.maps_read = True # use stereoRectify to calculate what we need to rectify stereo images M1 = self.info["cameraMatrix1"] d1 = self.info["distCoeffs1"] M2 = self.info["cameraMatrix2"] d2 = self.info["distCoeffs2"] size = self.info['imageSize'] R = self.info['R'] T = self.info['T'] h, w = size[:2] R1, R2, self.P1, self.P2, self.Q, roi1, roi2 = cv2.stereoRectify(M1, d1, M2, d2, (w,h), R, T, alpha=self.alpha) # these return undistortion and rectification maps which are both # stored in maps_x for camera 1 and 2 self.maps_1 = cv2.initUndistortRectifyMap(M1, d1, R1, self.P1, (w,h), cv2.CV_16SC2) # CV_32F? self.maps_2 = cv2.initUndistortRectifyMap(M2, d2, R2, self.P2, (w,h), cv2.CV_16SC2) # return self.__fix2(left, self.maps_1), self.__fix2(right, self.maps_2) inter=cv2.INTER_LANCZOS4 return cv2.remap(left, self.maps_1[0], self.maps_1[1], inter), cv2.remap(right, self.maps_2[0], self.maps_2[1], inter)
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 stereo_rectify(config, R, T): R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify( config.camera.left_intrinsics, config.camera.left_distortion, config.camera.right_intrinsics, config.camera.right_distortion, config.camera.size, R, T, alpha=0) return R1, R2, P1, P2
def doThings(self): sgbm = cv2.StereoSGBM() sgbm.SADWindowSize, numberOfDisparitiesMultiplier, sgbm.preFilterCap, sgbm.minDisparity, \ sgbm.uniquenessRatio, sgbm.speckleWindowSize, sgbm.P1, sgbm.P2, \ sgbm.speckleRange = [v for v,_ in self.params.itervalues()] sgbm.numberOfDisparities = numberOfDisparitiesMultiplier*16 sgbm.disp12MaxDiff = -1 sgbm.fullDP = False R1, R2, P1, P2, Q, topValidRoi, bottomValidRoi = cv2.stereoRectify(self.M1, self.D1, self.M2, self.D2, (self.top.shape[1],self.top.shape[0]), self.R, self.T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0) top_map1, top_map2 = cv2.initUndistortRectifyMap(self.M1, self.D1, R1, P1, (self.top.shape[1],self.top.shape[0]), cv2.CV_16SC2) bottom_map1, bottom_map2 = cv2.initUndistortRectifyMap(self.M2, self.D2, R2, P2, (self.bottom.shape[1], self.bottom.shape[0]), cv2.CV_16SC2) self.top_r = cv2.remap(self.top, top_map1, top_map2, cv2.cv.CV_INTER_LINEAR); self.bottom_r = cv2.remap(self.bottom, bottom_map1, bottom_map2, cv2.cv.CV_INTER_LINEAR) top_small = cv2.resize(self.top_r, (self.top_r.shape[1]/2,self.top_r.shape[0]/2)) bottom_small = cv2.resize(self.bottom_r, (self.bottom_r.shape[1]/2,self.bottom_r.shape[0]/2)) cv2.imshow('top', top_small); cv2.imshow('bottom', bottom_small); # top_r = cv2.equalizeHist(top_r) top_r = cv2.blur(self.top_r, (5,5)) # bottom_r = cv2.equalizeHist(bottom_r) bottom_r = cv2.blur(self.bottom_r, (5,5)) dispTop = sgbm.compute(top_r.T, bottom_r.T).T; dispTopPositive = dispTop dispTopPositive[dispTop<0] = 0 disp8 = (dispTopPositive / (sgbm.numberOfDisparities * 16.) * 255).astype(np.uint8); disp_small = cv2.resize(disp8, (disp8.shape[1]/2, disp8.shape[0]/2)); cv2.imshow(self.winname, disp_small); self.disp8 = disp8 self.xyz = cv2.reprojectImageTo3D(dispTop, Q, handleMissingValues=True) # self.xyzrgb = np.zeros((self.xyz.shape[0],self.xyz.shape[1],4)) # import struct # def color_to_float(color): # if color.size == 1: # color = [color]*3 # rgb = (color[2] << 16 | color[1] << 8 | color[0]); # rgb_hex = hex(rgb)[2:-1] # s = '0'*(8-len(rgb_hex)) + rgb_hex.capitalize() ## print color, rgb, hex(rgb) # rgb_float = struct.unpack('!f', s.decode('hex'))[0] ## print rgb_float # return rgb_float # for i in range(self.xyz.shape[0]): # for j in range(self.xyz.shape[1]): # self.xyzrgb[i,j] = np.append(self.xyz[i,j], color_to_float(self.top[i,j]))
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 set_alpha(self, a): """ Set the alpha value for the calibrated camera solution. The alpha value is a zoom, and ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). """ cv2.stereoRectify(self.l.intrinsics, self.l.distortion, self.r.intrinsics, self.r.distortion, self.size, self.R, self.T, self.l.R, self.r.R, self.l.P, self.r.P, alpha = a) cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, self.l.mapx, self.l.mapy) cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, self.r.mapx, self.r.mapy)
def __init__(self, params, winname, top, bottom): self.top = top self.bottom = bottom top_small = cv2.resize(top, (top.shape[1] / 2, top.shape[0] / 2)) bottom_small = cv2.resize(bottom, (bottom.shape[1] / 2, bottom.shape[0] / 2)) cv2.imshow('top', top_small); cv2.imshow('bottom', bottom_small); extrinsic_filepath = config.PROJPATH + 'extrinsics.yml' intrinsic_filepath = config.PROJPATH + 'intrinsics.yml' self.R = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R')) self.T = np.asarray(cv2.cv.Load(extrinsic_filepath, name='T')) self.R1 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R1')) self.R2 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R2')) self.P1 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='P1')) self.P2 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='P2')) self.Q = np.asarray(cv2.cv.Load(extrinsic_filepath, name='Q')) self.M1 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='M1')) self.M2 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='M2')) self.D1 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='D1')) self.D2 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='D2')) self.do_tune = config.TUNE_DISPARITY_MAP R1, R2, P1, P2, self.Q, topValidRoi, bottomValidRoi = cv2.stereoRectify(self.M1, self.D1, self.M2, self.D2, (self.top.shape[1], self.top.shape[0]), self.R, self.T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=-1) top_map1, top_map2 = cv2.initUndistortRectifyMap(self.M1, self.D1, R1, P1, (self.top.shape[1], self.top.shape[0]), cv2.CV_16SC2) bottom_map1, bottom_map2 = cv2.initUndistortRectifyMap(self.M2, self.D2, R2, P2, (self.bottom.shape[1], self.bottom.shape[0]), cv2.CV_16SC2) self.top_r = cv2.remap(self.top, top_map1, top_map2, cv2.cv.CV_INTER_LINEAR); self.bottom_r = cv2.remap(self.bottom, bottom_map1, bottom_map2, cv2.cv.CV_INTER_LINEAR) top_r_small = cv2.resize(self.top_r, (self.top_r.shape[1] / 2, self.top_r.shape[0] / 2)) bottom_r_small = cv2.resize(self.bottom_r, (self.bottom_r.shape[1] / 2, self.bottom_r.shape[0] / 2)) cv2.imshow('top rectified', top_r_small); cv2.imshow('bottom rectified', bottom_r_small); tx1,ty1,tx2,ty2 = topValidRoi bx1,by1,bx2,by2 = bottomValidRoi self.roi = (max(tx1, bx1), max(ty1, by1), min(tx2, bx2), min(ty2, by2)) self.top_r = cv2.blur(self.top_r, (5, 5)) self.bottom_r = cv2.blur(self.bottom_r, (5, 5)) # top_r = cv2.equalizeHist(self.top_r) # bottom_r = cv2.equalizeHist(self.bottom_r) super(SGBMTuner, self).__init__(params, winname)
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 computeStereoRectify(params): imsize = (1280,960) cameraMatrix1 = params['cam'][0]['KK'] distCoeffs1 = params['cam'][0]['distort'] cameraMatrix2 = params['cam'][1]['KK'] distCoeffs2 = params['cam'][1]['distort'] (R1, R2, P1, P2, Q, size1, size2) = cv2.stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imsize, params['cam'][0]['E_R'], params['cam'][0]['E_t']) map1x, map1y = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imsize, cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imsize, cv2.CV_32FC1) return (R1, R2, P1, P2, Q, size1, size2, map1x, map1y, map2x, map2y)
def rectify(fst_camera, fst_image, snd_camera, snd_image): R = calculate_rotation(fst_camera, snd_camera) T = calculate_translation(fst_camera, snd_camera) height, width = fst_image.shape size = (width, height) #params: #cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(fst_camera.camera_matrix, fst_camera.distortion,\ snd_camera.camera_matrix, snd_camera.distortion,\ size, R, T, None, 0) fst_rectified_image = get_rectified_image(fst_image, fst_camera, R1, P1) snd_rectified_image = get_rectified_image(snd_image, snd_camera, R2, P2) return fst_rectified_image, R1, snd_rectified_image, Q
def plot_rectified_images(self, feat_mode="SURF"): """Plots rectified images This method computes and plots a rectified version of the two images side by side. :param feat_mode: whether to use rich descriptors for feature matching ("surf") or optic flow ("flow") """ self._extract_keypoints(feat_mode) self._find_fundamental_matrix() self._find_essential_matrix() self._find_camera_matrices_rt() R = self.Rt2[:, :3] T = self.Rt2[:, 3] #perform the rectification R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(self.K, self.d, self.K, self.d, self.img1.shape[:2], R, T, alpha=1.0) mapx1, mapy1 = cv2.initUndistortRectifyMap(self.K, self.d, R1, self.K, self.img1.shape[:2], cv2.CV_32F) mapx2, mapy2 = cv2.initUndistortRectifyMap(self.K, self.d, R2, self.K, self.img2.shape[:2], cv2.CV_32F) img_rect1 = cv2.remap(self.img1, mapx1, mapy1, cv2.INTER_LINEAR) img_rect2 = cv2.remap(self.img2, mapx2, mapy2, cv2.INTER_LINEAR) # draw the images side by side total_size = (max(img_rect1.shape[0], img_rect2.shape[0]), img_rect1.shape[1] + img_rect2.shape[1], 3) img = np.zeros(total_size, dtype=np.uint8) img[:img_rect1.shape[0], :img_rect1.shape[1]] = img_rect1 img[:img_rect2.shape[0], img_rect1.shape[1]:] = img_rect2 # draw horizontal lines every 25 px accross the side by side image for i in range(20, img.shape[0], 25): cv2.line(img, (0, i), (img.shape[1], i), (255, 0, 0)) cv2.imshow('imgRectified', img) cv2.waitKey()
def compute_stereo_rectification_maps(stereo_rig, im_size, size_factor): new_size = (int(im_size[1] * size_factor), int(im_size[0] * size_factor)) rotation1, rotation2, pose1, pose2 = \ cv2.stereoRectify(cameraMatrix1=stereo_rig.cameras[0].intrinsics.intrinsic_mat, distCoeffs1=stereo_rig.cameras[0].intrinsics.distortion_coeffs, cameraMatrix2=stereo_rig.cameras[1].intrinsics.intrinsic_mat, distCoeffs2=stereo_rig.cameras[1].intrinsics.distortion_coeffs, imageSize=(im_size[1], im_size[0]), R=stereo_rig.cameras[1].extrinsics.rotation, T=stereo_rig.cameras[1].extrinsics.translation, flags=cv2.CALIB_ZERO_DISPARITY, newImageSize=new_size )[0:4] map1x, map1y = cv2.initUndistortRectifyMap(stereo_rig.cameras[0].intrinsics.intrinsic_mat, stereo_rig.cameras[0].intrinsics.distortion_coeffs, rotation1, pose1, new_size, cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(stereo_rig.cameras[1].intrinsics.intrinsic_mat, stereo_rig.cameras[1].intrinsics.distortion_coeffs, rotation2, pose2, new_size, cv2.CV_32FC1) return map1x, map1y, map2x, map2y
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)
def StereoRectify(self, R, T): """Computes rectification transforms for each head of a calibrated stereo camera.""" # Prepares the external parameters. cameraMatrix1 = StereoCameras.Instance.Parameters.CameraMatrix1 distCoeffs1 = StereoCameras.Instance.Parameters.DistCoeffs1 cameraMatrix2 = StereoCameras.Instance.Parameters.CameraMatrix2 distCoeffs2 = StereoCameras.Instance.Parameters.DistCoeffs2 imageSize = StereoCameras.Instance.Size # Computes rectification transforms. R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, alpha=0) # Records the external parameters. StereoCameras.Instance.Parameters.R1 = R1 StereoCameras.Instance.Parameters.R2 = R2 StereoCameras.Instance.Parameters.P1 = P1 StereoCameras.Instance.Parameters.P2 = P2 StereoCameras.Instance.Parameters.Q = Q # Define that the stereo cameras are calibrated self.IsCalibrated = True
def rectify(camera_matrix,dist_coefs,Size,R,T,gray,displacement): canvasMD = gray[displacement:,:] (w,h,q) = canvasMD.shape R1,R2,P1,P2,Q,qw,qw2 = cv2.stereoRectify(camera_matrix,dist_coefs, camera_matrix,dist_coefs, (w,h), R, T) mapx1,mapy1 = cv2.initUndistortRectifyMap(camera_matrix,dist_coefs,R1,P1[:,0:3],(w,h),cv2.CV_32FC1) img1 = cv2.remap(canvasMD,mapx1,mapy1,cv2.INTER_LINEAR) canvasMU = gray[:displacement,:] (w,h,q) = canvasMU.shape browse.browse(canvasMU) mapx2,mapy2 = cv2.initUndistortRectifyMap(camera_matrix,dist_coefs,R2,P2[:,0:3],(w,h),cv2.CV_32FC1) img2 = cv2.remap(canvasMU,mapx2,mapy2,cv2.INTER_LINEAR) return img1,img2
def rectify_stereo_camera(matrix_1, dist_1, matrix_2, dist_2, R, T): rotation_1, rotation_2, pose_1, pose_2, Q, roi_left, roi_right = cv2.stereoRectify( matrix_1, dist_1, matrix_2, dist_2, (9, 6), R, T) return [rotation_1, rotation_2, pose_1, pose_2, Q, roi_left, roi_right]
def convert_doveeye_to_dlc(config, doveeye_calib_file): cfg_3d = auxiliaryfunctions.read_config(config) img_path, path_corners, path_camera_matrix, path_undistort = auxiliaryfunctions_3d.Foldernames3Dproject(cfg_3d) cam_names = cfg_3d['camera_names'] dist_pickle = {} stereo_params = {} for cam in cam_names: dist_pickle.setdefault(cam, []) doveye_calib=readYAMLFile(doveeye_calib_file) for cam in cam_names: cam_idx=cameras.index(cam) # Save the camera calibration result for later use (we won't use rvecs / tvecs) dist_pickle[cam] = {'mtx': doveye_calib['C'][cam_idx], 'dist': doveye_calib['D'][cam_idx], 'objpoints': [], 'imgpoints': []} pickle.dump(dist_pickle, open(os.path.join(path_camera_matrix, cam + '_intrinsic_params.pickle'), "wb")) for i in range(len(cam_names)): cam1_idx=cameras.index(cam_names[i]) for j in range(i + 1, len(cam_names)): cam2_idx=cameras.index(cam_names[j]) pair = [cam_names[i], cam_names[j]] pair_idx=-1 for potential_pair_idx,potential_pair in enumerate(camera_pairs): if (potential_pair[0]==cam_names[i] and potential_pair[1]==cam_names[j]) or (potential_pair[0]==cam_names[j] and potential_pair[1]==cam_names[i]): pair_idx=potential_pair_idx break # Stereo Rectification rectify_scale = 0.4 # Free scaling parameter check this https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#fisheye-stereorectify R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(doveye_calib['C'][cam1_idx], doveye_calib['D'][cam2_idx], doveye_calib['C'][cam2_idx], doveye_calib['D'][cam1_idx], (1086, 2040), doveye_calib['R'][pair_idx], doveye_calib['T'][pair_idx], alpha=rectify_scale) stereo_params[pair[0] + '-' + pair[1]] = {"cameraMatrix1": doveye_calib['C'][cam1_idx], "cameraMatrix2": doveye_calib['C'][cam2_idx], "distCoeffs1": doveye_calib['D'][cam1_idx], "distCoeffs2": doveye_calib['D'][cam2_idx], "R": doveye_calib['R'][pair_idx], "T": doveye_calib['T'][pair_idx], "E": [], "F": doveye_calib['F'][pair_idx], "R1": R1, "R2": R2, "P1": P1, "P2": P2, "roi1": roi1, "roi2": roi2, "Q": Q, "image_shape": [(1086,2040),(1086,2040)]} print('Saving the stereo parameters for every pair of cameras as a pickle file in %s' % str( os.path.join(path_camera_matrix))) auxiliaryfunctions.write_pickle(os.path.join(path_camera_matrix, 'stereo_params.pickle'), stereo_params) print("Camera calibration done! Use the function ``check_undistortion`` to check the check the calibration") pass
def camera_cali(self): # calibration a single camera ''' for c in xrange(0, 1): # 3d points in real world space obj_pts = [] # 2d points in image plane img_pts = [] boards = self.boards[c].chessboards corners = self.corners[c].corners_pts # check if this image is the reference image if c == 0: for b in xrange(0, len(boards)): h, w = len(boards[b]), len(boards[b][0]) obj_pts_board = np.zeros((h * w, 3), np.float32) img_pts_board = np.zeros((h * w, 2), np.float32) board = boards[b] for i in xrange(0, h): for j in xrange(0, w): obj_pts_board[i * w + j, 0] = j * self.corner_dist obj_pts_board[i * w + j, 1] = i * self.corner_dist img_pts_board[i * w + j, 0] = corners[int(board[i, j])][0] img_pts_board[i * w + j, 1] = corners[int(board[i, j])][1] obj_pts.append(obj_pts_board) img_pts.append(img_pts_board) camera_mat = np.zeros((3, 3), np.float32) camera_mat[0, 0] = 900. camera_mat[0, 2] = len(self.img[c][0]) / 2 camera_mat[1, 1] = 900. camera_mat[1, 2] = len(self.img[c]) / 2 camera_mat[2, 2] = 1. ret, mtx, dist, rv, tv = cv2.calibrateCamera(obj_pts, img_pts, (len(self.img[c][0]), len(self.img[c])), camera_mat, None, None, None, flags=(cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_FIX_K3)) print(mtx, dist) ''' # first camera/image is the reference camera/image boards_ref = self.boards[0].chessboards corners_ref = self.corners[0].corners_pts # only support two-camera calibration for c in xrange(1, len(self.boards)): match = self.matching[2 * (c - 1)] rot = self.matching[2 * (c - 1) + 1] print match print rot obj_pts_ref = [] img_pts_ref = [] obj_pts_tar = [] img_pts_tar = [] boards_tar = self.boards[c].chessboards corners_tar = self.corners[c].corners_pts for b in xrange(0, len(match)): if match[b] != -1: # reference camera obj_pts_board_ref = [] img_pts_board_ref = [] board_ref = boards_ref[b] for i in xrange(0, len(board_ref)): for j in xrange(0, len(board_ref[0])): obj_pts_board_ref.append([ j * self.corner_dist, i * self.corner_dist, 0 ]) img_pts_board_ref.append(corners_ref[board_ref[i, j]]) # target camera obj_pts_board_tar = [] img_pts_board_tar = [] board_tar = boards_tar[int(match[b])] for k in xrange(0, int(rot[b])): board_tar = np.flipud(board_tar).T for i in xrange(0, len(board_tar)): for j in xrange(0, len(board_tar[0])): obj_pts_board_tar.append([ j * self.corner_dist, i * self.corner_dist, 0 ]) img_pts_board_tar.append(corners_tar[board_tar[i, j]]) obj_pts_ref.append( np.array(obj_pts_board_ref).astype(np.float32)) img_pts_ref.append( np.array(img_pts_board_ref).astype(np.float32)) obj_pts_tar.append( np.array(obj_pts_board_tar).astype(np.float32)) img_pts_tar.append( np.array(img_pts_board_tar).astype(np.float32)) ''' # visualize the corners found on image for no_board in xrange(0, len(img_pts_ref)): I_1 = self.img[0] I_2 = self.img[1] for i in xrange(0, len(img_pts_ref[no_board])): x = int(math.ceil(img_pts_ref[no_board][i, 0])) y = int(math.ceil(img_pts_ref[no_board][i, 1])) I_1[y-2:y+2, x-2:x+2] = [0, 0, 255] cv2.imshow('left', I_1) cv2.waitKey(0) cv2.destroyAllWindows() for i in xrange(0, len(img_pts_tar[no_board])): x = int(math.ceil(img_pts_tar[no_board][i, 0])) y = int(math.ceil(img_pts_tar[no_board][i, 1])) I_2[y-2:y+2, x-2:x+2] = [0, 0, 255] cv2.imshow('right', I_2) cv2.waitKey(0) cv2.destroyAllWindows() ''' mono_ret, mono_mat_1, mono_dist_1, mono_r, mono_t = \ cv2.calibrateCamera(obj_pts_ref, img_pts_ref, (len(self.img[c][0]), len(self.img[c])), flags=( cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5)) print 'mono', mono_mat_1, mono_dist_1 # stereo camera calibration ret, cam_mat_1, cam_dist_1, cam_mat_2, cam_dist_2, R, T, E, F = \ cv2.stereoCalibrate(obj_pts_ref, img_pts_ref, img_pts_tar, (len(self.img[c][0]), len(self.img[c])), flags=(cv2.CALIB_FIX_K4+cv2.CALIB_FIX_K5)) rect_rot_1, rect_rot_2, cam_proj_1, cam_proj_2, Q, ROI1, ROI2 = \ cv2.stereoRectify(cam_mat_1, cam_dist_1, cam_mat_2, cam_dist_2, (len(self.img[c][0]), len(self.img[c])), R, T, None, None, None, None, None, flags=(cv2.CALIB_ZERO_DISPARITY), alpha=0, newImageSize=(1091, 547)) # DEBUG cam_mat_1 = np.array([[6.47816e2, 0.0, 7.71052e2], [0.0, 6.45650e2, 4.33588e2], [0.0, 0.0, 1.0]]) cam_mat_2 = np.array([[6.47793e2, 0.0, 7.49001e2], [0.0, 6.47026e2, 4.44525e2], [0.0, 0.0, 1.0]]) R = np.array([[9.99915e-01, -2.07121e-03, -1.28613e-02], [2.03651e-03, 9.99994e-01, -2.70997e-03], [1.28669e-02, 2.68355e-03, 9.9991e-01]]) T = np.array([[-6.311307521502e-01], [-3.756969942287e-03], [8.773418730107e-03]]) self.intrinsic.append(cam_mat_1) self.intrinsic.append(cam_mat_2) self.distortion.append(cam_dist_1) self.distortion.append(cam_dist_2) self.rotation.append(R) self.translation.append(T) self.cam_proj.append(cam_proj_1) self.cam_proj.append(cam_proj_2) self.rect_rot.append(rect_rot_1) self.rect_rot.append(rect_rot_2) for b in xrange(0, len(img_pts_ref)): # TODO: update corner pixel coordinates to new rectified coordinate and # use rectified projection matrix cam_proj_1 = np.concatenate((self.intrinsic[0], np.zeros( (3, 1))), axis=1) cam_proj_2 = self.intrinsic[1].dot( np.concatenate((self.rotation[0], self.translation[0]), axis=1)) obj_pts_board = cv2.triangulatePoints( cam_proj_1, cam_proj_2, img_pts_ref[b].astype(np.float32).T, img_pts_tar[b].astype(np.float32).T) obj_pts_board = obj_pts_board.T obj_pts_board = obj_pts_board[:, 0:3] / obj_pts_board[:, 3:4] self.boards_pts_set.append(obj_pts_board) # visualize the corners found on image '''
import cv2 import numpy as np K0 = np.load('CMAT_DIST/logitech_c310_cam_mat.npy') dist0 = np.load('CMAT_DIST/logitech_c310_dist_coeffs.npy') K1 = np.load('CMAT_DIST/logitech_c310_cam_mat.npy') dist1 = np.load('CMAT_DIST/logitech_c310_dist_coeffs.npy') R = np.load('STEREO_CALIB/stereo_rmat.npy') t = np.load('STEREO_CALIB/stereo_tvec.npy') R1, R2, P1, P2, Q, roi_left, roi_right = cv2.stereoRectify(K0, dist0, K1, dist1, (1280, 720), R, t) x = np.dot(R1, P1) print(x) y = np.dot(R2, P2) print(y) np.save('STEREO_RECTIFY/P1', x) np.save('STEREO_RECTIFY/P2', y)
def publish_3d_coords(self, left_max_x, left_max_y, right_max_x, right_max_y, imageSize): #monocular calibration 1 = left 2 = right CMatr1 = np.matrix([[2531.915668, 0.000000, 615.773452], [0.000000, 2594.436434, 344.505755], [0.000000, 0.000000, 1.000000]]).astype(np.float) pp = pprint.PrettyPrinter(indent=4) # print("CMatr1", CMatr1) #left distortion parameters: 1.281681 -15.773048 -0.010428 0.012822 0.000000 CMatr2 = np.matrix([[1539.714285, 0.000000, 837.703760], [0.000000, 1506.265655, 391.687374], [0.000000, 0.000000, 1.000000]]).astype(np.float) # print("CMatr2", CMatr2) projPoints1 = np.array([[left_max_x],[left_max_y]]).astype(np.float) projPoints2 = np.array([[right_max_x],[right_max_y]]).astype(np.float) distort_left = np.array([1.281681, -15.773048, -0.010428, 0.012822, 0.000000]).astype(np.float) distort_right = np.array([0.091411, -0.461269, 0.021006, 0.040117, 0.000000]).astype(np.float) # print("distort_left", distort_left) # print("distort_right", distort_right) RMat1 = self.quatToRot(np.array([-0.029, 0.992, -0.110, 0.053])) RMat2 = self.quatToRot(np.array([-0.019, 0.997, -0.038, -0.071])) RFinal = np.matmul(np.linalg.inv(RMat1),RMat2) T_left = np.array([-0.268, 0.516, 3.283]).astype(np.float) #--------------------CHANGE T_right = np.array([0.018, -0.184, 1.255]).astype(np.float) #--------------------CHANGE T_final = T_right - T_left #print("RFinal", RFinal) #print("T_final", T_final) #print(imageSize) R1,R2,P1,P2,Q, a,b = cv2.stereoRectify(CMatr1, distort_left, CMatr2, distort_right, (1280,720), RFinal, T_final, alpha=-1) #print("R1",R1) #print("R2",R2)CMatr1 #print("P1",P1) #print("P2",P2) #pnt1 = cv2.undistortPoints(projPoints1, CMatr1, distort_left, R=RMat1, P=P1) #pnt2 = cv2.undistortPoints(projPoints2, CMatr2, distort_right, R=RMat2, P=P2) #print("left:",projPoints1) #print("right:", projPoints2) P1_stereo = np.array([[4890.538324810042, 0.0, -1734.3179817199707, 0.0],[ 0.0, 4890.538324810042, 398.04181480407715, 0.0],[ 0.0, 0.0, 1.0, 0.0]]) P2_stereo = np.array([[4890.538324810042, 0.0, -1734.3179817199707, 8092.200252104331],[ 0.0, 4890.538324810042, 398.04181480407715, 0.0],[ 0.0, 0.0, 1.0, 0.0]]) points4D = cv2.triangulatePoints(P1_stereo, P2_stereo, projPoints1, projPoints2) #points3D = cv2.convertPointsFromHomogeneous(points4D) #print(points4D) #Converts 4D to 3D by [x,y,z,w] -> [x/w, y/w, z/w] points3D = np.array([ points4D[0]/points4D[3], points4D[1]/points4D[3], points4D[2]/points4D[3] ])
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate( objpoints, imgpointsL, imgpointsR, mtxL, distL, mtxR, distR, ChessImaR.shape[::-1], criteria=criteria_stereo, flags=cv2.CALIB_FIX_INTRINSIC) # 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) #******************************************* #***** Parameters for the StereoVision ***** #******************************************* # Create StereoSGBM and prepare all parameters window_size = 3 min_disp = 2
K2 = np.array([[5.665439587514537e+02, 0, 3.300422688805023e+02],\ [0, 7.557038654043611e+02, 2.679839339287851e+02],\ [0, 0, 1]]) D2 = np.array([[-0.242511849444114, 0.980331254691578,\ 0.002963361851048, 0.003507404137556, 0.00000]]) R = np.array([[0.999809488415120, -3.803081241759167e-04, 0.019515179753873],\ [4.462727193334061e-04, 1, -0.003375924839205],\ [-0.019513782712618, 0.003383990778750, 0.999803861210115]]) T = np.array([-58.953856363020520, 0.403802936505059, -1.051816198390031]) rotation1, rotation2, pose1, pose2 = cv2.stereoRectify( cameraMatrix1=K1, distCoeffs1=D1, cameraMatrix2=K2, distCoeffs2=D2, imageSize=(dispW, dispH), R=R, T=T, #flags=cv2.CALIB_ZERO_DISPARITY, #newImageSize=(dispW, dispH) )[0:4] #print("Retify coefficients calculated") if not cap_left.isOpened() and not cap_right.isOpened( ): # If we can't get images from both sources, error print("Can't opened the streams!") sys.exit(-9) # Change the resolution in need # cap_right.set(cv2.CAP_PROP_FRAME_WIDTH, 640) # float # cap_right.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) # float
start_time = time.time() retL, frameL = capL.read() retR, frameR = capR.read() # image size height, width, ch = frameL.shape imSize = (width, height) #rectify get new camera matrixes and rectification transforms #P1, P2 both new camera matrixes #R1, R2 are rectification transforms output cv2.stereoRectify(camMatL, distCoL, camMatR, distCoR, imSize, R, T, R1, R2, P1, P2, Q=None, flags=0 , alpha=-1, newImageSize=(0,0)) #appy rectification transform to camera 1 with new camera matrix P1 and rectification transform R1 from stereoRectify above mapxL,mapyL = cv2.initUndistortRectifyMap(camMatL,distCoL,rectTransL,P1,imSize,cv2.CV_32FC1) mapxR,mapyR = cv2.initUndistortRectifyMap(camMatR,distCoR,rectTransR,P2,imSize,cv2.CV_32FC1) #remap cameras to remove distortion rectL = cv2.remap(frameL,mapxL,mapyL,cv2.INTER_LINEAR) rectR = cv2.remap(frameR,mapxR,mapyR,cv2.INTER_LINEAR) # Depth Map # # convert rectified from RGB to 8 bit grayscale grayRectL = cv2.cvtColor(rectL, cv2.COLOR_BGR2GRAY) grayRectR = cv2.cvtColor(rectR, cv2.COLOR_BGR2GRAY) grayFrameL = cv2.cvtColor(frameL, cv2.COLOR_BGR2GRAY)
_, rightCameraMatrix, rightDistortionCoefficients, _, _ = cv2.calibrateCamera( objectPoints, rightImagePoints, imageSize, None, None) print("Calibrating cameras together...") (_, _, _, _, _, 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...") # TODO: Why do I care about the disparityToDepthMap? (leftRectification, rightRectification, leftProjection, rightProjection, dispartityToDepthMap, leftROI, rightROI) = cv2.stereoRectify( leftCameraMatrix, leftDistortionCoefficients, rightCameraMatrix, rightDistortionCoefficients, imageSize, rotationMatrix, translationVector, None, None, None, None, None, cv2.CALIB_ZERO_DISPARITY, OPTIMIZE_ALPHA) print("Saving calibration...") leftMapX, leftMapY = cv2.initUndistortRectifyMap(leftCameraMatrix, leftDistortionCoefficients, leftRectification, leftProjection, imageSize, cv2.CV_32FC1) rightMapX, rightMapY = cv2.initUndistortRectifyMap( rightCameraMatrix, rightDistortionCoefficients, rightRectification, rightProjection, imageSize, cv2.CV_32FC1) np.savez_compressed(outputFile, imageSize=imageSize, leftMapX=leftMapX,
y = tp[1] / tp[3] z = tp[2] / tp[3] ax.scatter3D(tp[0] / tp[3], tp[1] / tp[3], tp[2] / tp[3], c=tp[2] / tp[3], marker='o') ax.set_xlim(-10, 10) ax.set_ylim(-10, 10) ax.set_zlim(-10, 10) plt.show() R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(cmatrix_left, dis_coeff_left, cmatrix_right, dis_coeff_right, img2.shape[:2], R, T) np.savetxt( 'D:/SPRING 2020/CSE 598 Perception in Robotics/Project/Project 2a/project_2a/parameters/R1.txt', R1) np.savetxt( 'D:/SPRING 2020/CSE 598 Perception in Robotics/Project/Project 2a/project_2a/parameters/R2.txt', R2) np.savetxt( 'D:/SPRING 2020/CSE 598 Perception in Robotics/Project/Project 2a/project_2a/parameters/Q.txt', Q) (height, width) = img1.shape[:2] print(height, width) mapx1, mapy1 = cv2.initUndistortRectifyMap(cmatrix_left, dis_coeff_left, R1,
def bouguetRectify(points1, points2, imgSize, M1, M2, D1, D2, R, T): R1, R2, P1, P2, Q, ROI1, ROI2 = cv2.stereoRectify(M1, D1, M2, D2, imgSize, R, T) map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, P1, imgSize, cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, P2, imgSize, cv2.CV_32FC1) return (map1x, map1y, map2x, map2y), Q
plt.imshow(disparity, 'CMRmap_r') plt.show() Tmat = np.array([0.4, 0., 0.]) #R, t = Pose_Est(imgR,imgL,K2,K1) #Tmat = t print(np.linalg.norm(Tmat)) rev_proj_matrix = np.zeros((4, 4)) cv2.stereoRectify(cameraMatrix1 = K1,cameraMatrix2 = K2, \ distCoeffs1 = 0, distCoeffs2 = 0, \ imageSize = imgL.shape[:2], \ R = np.identity(3), T = Tmat, \ R1 = None, R2 = None, \ P1 = None, P2 = None, Q = rev_proj_matrix) h, w = imgL.shape[:2] f = 0.8 * w Q = np.float32([[1, 0, 0, -0.5 * w], [0, -1, 0, 0.5 * h], [0, 0, 0, -f], [0, 0, 1, 0]]) points = cv2.reprojectImageTo3D(disparity, Q) reflect_matrix = np.identity(3) reflect_matrix[0] *= -1 points = np.matmul(points, reflect_matrix) colors = cv2.cvtColor(imgLc, cv2.COLOR_BGR2RGB)
if len(allObjPoints) > 40: retval, leftCameraMatrix, leftDistCoeffs, rightCameraMatrix, rightDistCoeffs, R, T, E, F = cv2.stereoCalibrate( objectPoints=allObjPoints, imagePoints1=allLeftImgPoints, imagePoints2=allRightImgPoints, imageSize=imgSize, cameraMatrix1=leftCameraMatrix, distCoeffs1=leftDistCoeffs, cameraMatrix2=rightCameraMatrix, distCoeffs2=rightDistCoeffs, flags=flags) flags |= cv2.CALIB_USE_INTRINSIC_GUESS allObjPoints = [] allLeftImgPoints = [] allRightImgPoints = [] R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(leftCameraMatrix, leftDistCoeffs, rightCameraMatrix, rightDistCoeffs, imgSize, R, T, flags=cv2.CALIB_ZERO_DISPARITY) leftMapX, leftMapY = cv2.initUndistortRectifyMap(leftCameraMatrix, leftDistCoeffs, R1, P1, imgSize, cv2.CV_32FC1) rightMapX, rightMapY = cv2.initUndistortRectifyMap(rightCameraMatrix, rightDistCoeffs, R2, P2, imgSize, cv2.CV_32FC1) if leftMapX is not None: left = cv2.remap(left, leftMapX, leftMapY, cv2.INTER_LINEAR) right = cv2.remap(right, rightMapX, rightMapY, cv2.INTER_LINEAR) frame = cv2.hconcat([left, right]) cv2.imshow('Frame', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break else: break
# b=np.abs(T[0]) b = 4.14353 # f=cameraMatrix1[0,0] f = 1035.033 path1 = '/Users/zhouying/Desktop/三维重建/DATASET/train/d3/k3/Left_Image.png' path2 = '/Users/zhouying/Desktop/三维重建/DATASET/train/d3/k3/Right_Image.png' savepath = "/Users/zhouying/Desktop" I1 = cv.imread(path1) I2 = cv.imread(path2) R1, R2, P1, P2, Q, validPixROI1, validPixROI2=\ cv.stereoRectify(cameraMatrix1,distCoeffs1, cameraMatrix2,distCoeffs2,imageSize,R,T, flags=cv.CALIB_ZERO_DISPARITY, alpha=-1, newImageSize=(0,0)) left_map1, left_map2 = cv.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, cv.CV_16SC2) right_map1, right_map2 = cv.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize, cv.CV_16SC2) I1_rectified = cv.remap(I1, left_map1, left_map2, cv.INTER_LINEAR) I2_rectified = cv.remap(I2, right_map1, right_map2, cv.INTER_LINEAR) minDisparity = 0 numDisparities = 192 SADWindowSize = 3 P1 = 8 * 3 * SADWindowSize**2
F, flags=0, criteria=criteria) R1 = None R2 = None P1 = None P2 = None # rectify: R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (640, 480), R, T, R1, R2, P1, P2, Q=None, flags=0, alpha=-1, newImageSize=(0, 0)) def load_image_into_numpy_array(image): (im_width, im_height) = image.size return np.array(image.getdata()).reshape( (im_height, im_width, 3)).astype(np.uint8) def compare_SIFT(box1, img1, box2, img2):
plt.figure(figsize=(10, 4)) plt.subplot(121) plt.imshow(left_im[..., ::-1]) plt.subplot(122) plt.imshow(right_im[..., ::-1]) plt.show() R1 = np.zeros((3, 3)) #output 3x3 matrix R2 = np.zeros((3, 3)) #output 3x3 matrix P1 = np.zeros((3, 4)) #output 3x4 matrix P2 = np.zeros((3, 4)) #output 3x4 matrix R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify( cameraMatrix1=mtx, #intrinsic parameters of the first camera cameraMatrix2=mtx, #intrinsic parameters of the second camera distCoeffs1=dist, #distortion parameters of the first camera distCoeffs2=dist, #distortion parameters of the second camera imageSize=(1536, 2048), #image dimensions R=R, #Rotation matrix between first and second cameras (returned by cv2.stereoCalibrate) T=T) #last 4 parameters point to inizialized output variables R1 = np.array(R1) #convert output back to numpy format R2 = np.array(R2) P1 = np.array(P1) P2 = np.array(P2) map1_x, map1_y = cv2.initUndistortRectifyMap( mtx, dist, R1, P1, (left_im.shape[1], left_im.shape[0]), cv2.CV_32FC1) map2_x, map2_y = cv2.initUndistortRectifyMap( mtx, dist, R2, P2, (left_im.shape[1], left_im.shape[0]), cv2.CV_32FC1) im_left = cv2.imread('CAM00054_s1.jpg')
def cali_save(): # Arrays to store object points and image points from all images objpoints = [] imgpointsR = [] imgpointsL = [] global retR, mtxR, distR, rvecsR, tvecsR, retS, MLS, dLS, MRS, dRS, R, T, E, F, RL, RR, PL, PR, Q, roiL, roiR, Shape # Start calibration from the camera print('Starting calibration for the 2 cameras... ') # Call all saved images for i in range(0, 20): if (i % 1 != 0): continue t = str(i) print(t) ChessImgR = cv2.imread('python/Stereo/imgs/chessboard-R' + t + '.png', 0) ChessImgL = cv2.imread('python/Stereo/imgs/chessboard-L' + t + '.png', 0) ChessImgL = cv2.resize(ChessImgL, (0, 0), fx=.5, fy=.5) ChessImgR = cv2.resize(ChessImgR, (0, 0), fx=.5, fy=.5) retR, cornersR = cv2.findChessboardCorners(ChessImgR, (9, 6), None) retL, cornersL = cv2.findChessboardCorners(ChessImgL, (9, 6), None) if (retR == True) & (retL == True): objpoints.append(objp) cv2.cornerSubPix(ChessImgR, cornersR, (11, 11), (-1, -1), criteria) cv2.cornerSubPix(ChessImgL, cornersL, (11, 11), (-1, -1), criteria) imgpointsR.append(cornersR) imgpointsL.append(cornersL) Shape = ChessImgR.shape[::-1] # Determine the new values for different parameters # Right side retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera( objpoints, imgpointsR, ChessImgR.shape[::-1], None, None) hR, wR = ChessImgR.shape[:2] OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR, (wR, hR), 1, (wR, hR)) # Left side retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera( objpoints, imgpointsL, ChessImgL.shape[::-1], None, None) hL, wL = ChessImgL.shape[:2] OmtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (wL, hL), 1, (wL, hL)) print('Cameras Ready to use') #******************************************** #***** Calibrate the Cameras for Stereo ***** #******************************************** # 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, ChessImgR.shape[::-1], criteria_stereo, flags) # StereoRectify function rectify_scale = 0 RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify( MLS, dLS, MRS, dRS, Shape, R, T, rectify_scale, (0, 0)) # last parameter is alpha, if 0 croped, if 1 not croped fs = cv2.FileStorage() fs.open("cali.yaml", cv2.FILE_STORAGE_WRITE) fs.write("MLS", MLS) fs.write("dLS", dLS) fs.write("MRS", MRS) fs.write("dRS", dRS) fs.write("mtxR", mtxR) fs.write("mtxL", mtxL) fs.write("distR", distR) fs.write("distL", distL) fs.write("R", R) fs.write("T", T) fs.write("E", E) fs.write("F", F) #rectification fs.write("RL", RL) fs.write("RR", RR) fs.write("PL", PL) fs.write("PR", PR) fs.write("Q", Q) fs.write("Shape", Shape)
calPath = "CalibrationPhotos/calibration_gusTest.json" datathings = cb.loadCalibration(calPath) M1, M2, d1, d2, R, T, E, F, dims = datathings print datathings print('T', T) print('T*sqr', T * sqr_size) flags = 0 flags |= cv2.CALIB_ZERO_DISPARITY R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(M1, d1, M2, d2, dims, R, T, alpha=-1, flags=flags) print('R1', R1) print('R2', R2) print('P1', P1) print('P2', P2) print('Q', Q) print('roi1', roi1) print('roi2', roi2) # an assortment of upper and lower bounds for the different colors we use in HSV. greenLower = (45, 86, 30) greenUpper = (100, 255, 255)
# cv.imshow('rectified image pairs with rectified matching lines', draw_region) # cv.waitKey(0) # cv.destroyAllWindows() # -------------------- DISPARITY MAP OF RECTIFIED IMAGES ------------------------ # disparityo1 = disparity_bmg(rectimg1, rectimg2, 11) # function from part 2, # Later I found 11 might be a sweet spot disparityo = cv.normalize(disparityo1, None, alpha=0, beta=255, norm_type=cv.NORM_MINMAX) disparityo = np.uint8(disparityo) # Q1-to-2, might be R1, R2, P1, P2, Q, roi1, roi2 = cv.stereoRectify(new_kmat, dist, new_kmat, dist, (C, R), rot, trans) # # Q2-to-1, might be # R1, R2, P1, P2, Q, roi1, roi2 = cv.stereoRectify(new_kmat, dist, new_kmat, dist, (C, R), invRot, invTrans) # Q[0, 3] += 400 # Q[1, 3] += 300 # pt_cloud = cv.reprojectImageTo3D(disparityo1, Q) # pt_cloud = cv.resize(pt_cloud, (960, 540)) colored_disparityo = cv.resize( cv.applyColorMap(disparityo, cv.COLORMAP_JET), (960, 540)) # cv.imshow('Disparity Map', colored_disparityo) # cv.imshow('Point Cloud', pt_cloud) # cv.waitKey(0) # cv.destroyAllWindows()
K_L = calib.__getattribute__('K_cam' + left_cam) K_R = calib.__getattribute__('K_cam' + right_cam) D_L = calib.__getattribute__('D_cam' + left_cam) D_R = calib.__getattribute__('D_cam' + right_cam) img_size = calib.__getattribute__('S_cam' + left_cam) R_L = calib.__getattribute__('R_cam' + left_cam) R_R = calib.__getattribute__('R_cam' + right_cam) R = np.matmul(R_L.transpose(), R_R) T_L = calib.__getattribute__('T_cam' + left_cam) T_R = calib.__getattribute__('T_cam' + right_cam) T = T_L - T_R _, _, _, _, Q, _, _ = cv2.stereoRectify(K_L, D_L, K_R, D_R, (int(img_size[0]), int(img_size[1])), R, T) points = cv2.reprojectImageTo3D(disp, Q, True) xs = points[:, :, 0].flatten() ys = points[:, :, 1].flatten() zs = points[:, :, 2].flatten() with open('points.obj', 'w') as f: for x, y, z in zip(xs, ys, zs): if (not (math.isinf(x) or math.isinf(y) or math.isinf(z))): f.write('v ' + str(x) + ' ' + str(y) + ' ' + str(z) + '\n')
print("\n\nRotation Matrix:\n",R) print("\n\nTransltion:\n",T) print("\n\nEssential Matrix:\n",E) print("\n\nFundamental Matrix:\n",F) print("\n\nDistortion Coefficients:\n",distCoeffs1,'\n', distCoeffs2) print("\n\nBaseline:\n",np.linalg.norm(T)) # (width,height) size = (640, 480) # size of your picture left = cv2.imread('data/left/left01.jpg',0) right = cv2.imread('data/right/right01.jpg',0) R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, size, R, T) mapL1, mapL2 = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, size, cv2.CV_16SC2) mapR1, mapR2 = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, size, cv2.CV_16SC2) left_img_remap = cv2.remap(left, mapL1, mapL2, cv2.INTER_LINEAR) right_img_remap = cv2.remap(right, mapR1, mapR2, cv2.INTER_LINEAR) cv2.imshow('rectified left',left_img_remap) cv2.imshow('rectified right',right_img_remap) cv2.waitKey(0) x,y,w,h = roi1 left_img_remap = left_img_remap[y:y+h, x:x+w] x,y,w,h = roi2
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
imageShape = grayimg.shape[::-1] # Determine whether circles are found in the image and the coordinates of the centers circlesFound, centerCoords = cv2.findCirclesGrid(grayimg, (4, 11)) # If circles are found, add object and image points if circlesFound: objectPoints.append(objp) imagePoints.append(centerCoords) return objectPoints, imagePoints, imageShape # Find object and image points from both cameras (object points will be the same) objectPointsL, imagePointsL, imageShapeL = addObjAndImgPoints(pathLeft) objectPointsR, imagePointsR, imageShapeR = addObjAndImgPoints(pathRight) # Grab initial stereo calibration data from both cameras (these are inputs for final calibration data). isStereoCalibrated, cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, rotationMatrixStereo, translationMatrixStereo, essentialMatrix, fundamentalMatrix = cv2.stereoCalibrate(objectPointsL, imagePointsL, imagePointsR, None, None, None, None, imageShapeL) # Grab final stereo calibration data if isStereoCalibrated: rotMatrixL, rotMatrixR, cameraMatrixNewL, cameraMatrixNewR, dispToDepthMap, roiL, roiR = cv2.stereoRectify(cameraMatrixL, distCoeffsL, cameraMatrixR, distCoeffsR, imageShapeL, rotationMatrixStereo, translationMatrixStereo) mapXL, mapYL = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffsL, rotMatrixL, cameraMatrixNewL, imageShapeL, cv2.CV_32FC1) mapXR, mapYR = cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffsR, rotMatrixR, cameraMatrixNewR, imageShapeL, cv2.CV_32FC1) # Save the calibration data np.savez_compressed(calibrationData, imageShapeL = imageShapeL, imageShapeR = imageShapeR, mapXL=mapXL, mapYL = mapYL, roiL = roiL , mapXR = mapXR, mapYR = mapYR, roiR = roiR)
res_l = calib['camera_calibrations']['camera_%d' % c]['resolution'] c = 1 K_r = calib['camera_calibrations']['camera_%d' % c]['K'] D_r = calib['camera_calibrations']['camera_%d' % c]['D'] R_r = calib['camera_calibrations']['camera_%d' % c]['R'] T_r = calib['camera_calibrations']['camera_%d' % c]['T'] res_r = calib['camera_calibrations']['camera_%d' % c]['resolution'] # Compute relative transformation R = R_r.dot(np.linalg.inv(R_l)) T = -R_r.dot(np.linalg.inv(R_l).dot(T_l)) + T_r # Rectification newsize = tuple(res_l) R_l, R_r, P_l, P_r, _, _, _ = cv2.stereoRectify(K_l, D_l, K_r, D_r, newsize, R, T, flags = cv2.CALIB_ZERO_DISPARITY, alpha = 0) rectMap = [[[], []], [[], []]] rectMap[0][0], rectMap[0][1] = cv2.initUndistortRectifyMap(K_l, D_l, R_l, P_l, newsize, cv2.CV_16SC2) rectMap[1][0], rectMap[1][1] = cv2.initUndistortRectifyMap(K_r, D_r, R_r, P_r, newsize, cv2.CV_16SC2) img_l, gray_l = getimage(imgpath1) img_r, gray_r = getimage(imgpath2) imgorig_l = utils.resizeimgh(img_l, H_IMGS) imgorig_r = utils.resizeimgh(img_r, H_IMGS) imgorig = np.hstack((imgorig_l, imgorig_r)) imgRect_l = cv2.remap(img_l, rectMap[0][0], rectMap[0][1], cv2.INTER_CUBIC) imgRect_r = cv2.remap(img_r, rectMap[1][0], rectMap[1][1], cv2.INTER_CUBIC)
M1 = np.array([[ 2.0340227345156890e+002 , 0, 1.5950000000000000e+002 ],[ 0, 2.0340227345156890e+002 , 1.1950000000000000e+002 ],[ 0,0, 1 ]]) M2 = np.array([[2.0340227345156890e+002 , 0, 1.5950000000000000e+002],[0, 2.0340227345156890e+002 , 1.1950000000000000e+002],[0, 0, 1 ]]) D1 = np.array([-4.1344532351734609e-001 , 1.4783234656467026e-001, 4.4055963467856136e-003 , 6.1661017124724649e-003, 0]) D2 = np.array([-4.5592829264409196e-001 , 2.2408910954289543e-001, 1.0425612281350783e-003 , -5.5920270905273290e-003, 0]) R = np.array([[9.9880267535072764e-001 ,1.0478131925395094e-003, -4.8909281325148393e-002],[-1.9549817734339412e-003, 9.9982687809750082e-001, -1.8503834199179697e-002],[4.8881425495729047e-002 , 1.8577295855929390e-002 , 9.9863180918704308e-001]]) T = np.array([-6.0430960451649426e+001 , 9.5357972721080564e-001, -3.4515100808301149e+000]) R1 = np.zeros(shape=(3,3)) R2 = np.zeros(shape=(3,3)) P1 = np.zeros(shape=(3,4)) P2 = np.zeros(shape=(3,4)) Q = np.zeros(shape=(4,4)) aa,bb,cc,dd,ee,roi1,roi2 = cv2.stereoRectify(M1, D1, M2, D2,(width, height), R, T, R1, R2, P1, P2, Q, flags=cv2.cv.CV_CALIB_ZERO_DISPARITY, alpha=-1, newImageSize=(0,0)) print roi1 # print maskl.max() maskl = np.zeros((height,width),dtype=np.uint8) maskr = np.zeros((height,width),dtype=np.uint8) cv2.rectangle(maskl,(roi1[0],roi1[1]),(roi1[2]+roi1[0],roi1[3]+roi1[1]),(255,255,255),-1) cv2.rectangle(maskr,(roi2[0],roi2[1]),(roi2[2]+roi2[0],roi2[3]+roi2[1]),(255,255,255),-1) map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, P1, (width, height), cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, P2, (width, height), cv2.CV_32FC1) print "Undistort complete\n"
def stereo_rectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T): R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T) return P1, P2, Q
list_left_sort = sorted(list_left, key=lambda s: int(p.search(s).group())) list_right_sort = sorted(list_right, key=lambda s: int(p.search(s).group())) cameraMatrix1 = numpy.loadtxt(dorectory_calibration + 'cameraMatrix1.csv',delimiter = ',') cameraMatrix2 = numpy.loadtxt(dorectory_calibration + 'cameraMatrix2.csv',delimiter = ',') distCoeffs1 = numpy.loadtxt(dorectory_calibration + 'distCoeffs1.csv',delimiter = ',') distCoeffs2 = numpy.loadtxt(dorectory_calibration + 'distCoeffs2.csv',delimiter = ',') R = numpy.loadtxt(dorectory_calibration + 'R.csv',delimiter = ',') T = numpy.loadtxt(dorectory_calibration + 'T.csv',delimiter = ',') # 平行化変換のためのRとPおよび3次元変換行列Qを求める flags = 0 alpha = 1 imageSize = (1024,768) newimageSize = (1024,768) R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, flags, alpha, newimageSize) # 平行化変換マップを求める m1type = cv2.CV_32FC1 map1_l, map2_l = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, newimageSize, m1type) #m1type省略不可 map1_r, map2_r = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, newimageSize, m1type) def calculate_contour_area(im_l,im_r): interpolation = cv2.INTER_NEAREST Re_TgtImg_l = cv2.remap(im_l, map1_l, map2_l, interpolation) Re_TgtImg_r = cv2.remap(im_r, map1_r, map2_r, interpolation)
camera_matrix_left = np.array([[1250.12541, 0, 691.61850], [0, 1230.17838, 427.35528], [0, 0, 1]]) # kc_left = [] distortion_left = np.array([-0.05265, 0.14178, -0.00724, 0.00415, 0.00000]) # fc_right_x 0 cc_right_x # 0 fc_right_y cc_right_y # 0 0 1 camera_matrix_right = np.array([[1193.23239, 0, 638.79370], [0, 1179.38126, 411.16069], [0, 0, 1]]) # kc_right = [] distortion_right = np.array([-0.18814, 0.66653, -0.00790, -0.00679, 0.00000]) # rotation vector om = np.array([-0.01199, -0.00651, -0.00405]) R = cv2.Rodrigues(om)[0] # translation vector T = np.array([-47.47028, 6.13393, -20.83518]) # window size size = (1280, 720) R1, R2, P1, P2, Q, validPirxROI1, validPixROI2 = cv2.stereoRectify( camera_matrix_left, distortion_left, camera_matrix_right, distortion_right, size, R, T) left_map1, left_map2 = cv2.initUndistortRectifyMap(camera_matrix_left, distortion_left, R1, P1, size, cv2.CV_16SC2) right_map1, right_map2 = cv2.initUndistortRectifyMap(camera_matrix_right, distortion_right, R2, P2, size, cv2.CV_16SC2)
averageEpipolarError = totalError / totalPoints print "Average epipolar error is {}".format(averageEpipolarError) #--------------------------------------------------# # Calculate extrinsinc pramaters, R1, R2, P1, P2, Q# #--------------------------------------------------# print "Calculating extrinsinc parameters" calib_R_left, calib_R_right, calib_P_left, calib_P_right, calib_Q, validROI_left, validROI_right = \ cv2.stereoRectify(cameraMatrixLeft, distortionCoeffsLeft, cameraMatrixRight, distortionCoeffsRight, \ imageSize=imageSize, \ R=calib_R, \ T=calib_T, \ flags=cv2.CALIB_ZERO_DISPARITY, \ alpha=1, \ newImageSize=imageSize \ ) print "IMPORTANT (isVerticalStereo): {}".format(calib_P_right[1][3] > calib_P_right[0][3]) print "Done calculating extrinsinc parameters" #----------------------------------------# print "Saving calibrations" #instrinsinc np.save("calibrations/cameraMatrixLeft", cameraMatrixLeft) np.save("calibrations/distortionCoeffsLeft", distortionCoeffsLeft)
right_camera_matrix = np.array([[1209.88776209302, 0.000000, 413.856956594846], [0.000000, 1210.67130773907, 228.925227764687], [0.000000, 0.000000, 1.000000]]) right_distortion = np.array([[ -0.629241841057911, 0.384306491947133, -0.00302412142395574, -0.0187961427211770, 0.000000 ]]) #om = np.array([0.01911, 0.03125, -0.00960]) # 旋转关系向量 #R = cv2.Rodrigues(om)[0] # 使用Rodrigues变换将om变换为R T = np.array([-502.581451656473, -10.0897997351901, -15.6283011876912]) # 平移关系向量 R = np.array([[0.998453237208844, 0.0279672732036317, 0.0480516881777589], [-0.0298684171587964, 0.998780442220761, 0.0393129227319578], [-0.0468936111171582, -0.0406873428335421, 0.998070903979040]]) size = (640, 480) # 图像尺寸 # 进行立体更正 R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( left_camera_matrix, left_distortion, right_camera_matrix, right_distortion, size, R, T) # 计算更正map left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2) right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
def _stereo_calibrate(self, dims): """ It performas stereo calibration and makes dictionary of stereo camera calibration model parameters. Returns: camera_model: dict A dictionary containing stereo camera model parameters. """ 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) # make sure to move around args according to opencv version rot_left, _ = cv2.Rodrigues(self.r1[0]) rot_right, _ = cv2.Rodrigues(self.r2[0]) R1, R2, P1, P2, Q, validPixROI1, validPixROI2S = cv2.stereoRectify( M1, d1, M2, d2, dims, R, T, flags=0, alpha=0) mapLx, mapLy = cv2.initUndistortRectifyMap(M1, d1, R1, P1, dims, m1type=cv2.CV_32FC1) mapRx, mapRy = cv2.initUndistortRectifyMap(M2, d2, R2, P2, dims, m1type=cv2.CV_32FC1) camera_model = dict([('M1', M1), ('M2', M2), ('dist1', d1), ('dist2', d2), ('rot_left', rot_left), ('rot_right', rot_right), ('R', R), ('T', T), ('E', E), ('F', F), ('mapLx', mapLx), ('mapLy', mapLy), ('mapRx', mapRx), ('mapRy', mapRy), ('R1', R1), ('R2', R2), ('P1', P1), ('P2', P2), ('dims', dims)]) return camera_model
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) ########## 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) cv_file.write('stereoMapL_x',stereoMapL[0]) cv_file.write('stereoMapL_y',stereoMapL[1]) cv_file.write('stereoMapR_x',stereoMapR[0]) cv_file.write('stereoMapR_y',stereoMapR[1]) cv_file.release()
if not in_front_of_both_cameras(first_inliers, second_inliers, R, T): # Third choice: R = U * Wt * Vt, T = u_3 R = U.dot(W.T).dot(Vt) T = U[:, 2] if not in_front_of_both_cameras(first_inliers, second_inliers, R, T): # Fourth choice: R = U * Wt * Vt, T = -u_3 T = -U[:, 2] # perform the rectification R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(K, d, K, d, imgl.shape[:2], R, T, alpha=1.0) mapx1, mapy1 = cv2.initUndistortRectifyMap(K, d, R1, K, imgl.shape[:2], cv2.CV_32F) mapx2, mapy2 = cv2.initUndistortRectifyMap(K, d, R2, K, imgr.shape[:2], cv2.CV_32F) img_rect1 = cv2.remap(imgl, mapx1, mapy1, cv2.INTER_LINEAR) img_rect2 = cv2.remap(imgr, mapx2, mapy2, cv2.INTER_LINEAR) # draw the images side by side total_size = (max(img_rect1.shape[0], img_rect2.shape[0]), img_rect1.shape[1] + img_rect2.shape[1], 3)
# import camera_configs # load the calibration data camtxL = np.load('camera_matrixL.npy') camtxR = np.load('camera_matrixR.npy') distL = np.load('distortion_cofficientsL.npy') distR = np.load('distortion_cofficientsR.npy') rmtx = np.load('rotation_mat.npy') tvec = np.load('translation_vectors.npy') Limg = cv.imread('.\data\left03.jpg', cv.IMREAD_GRAYSCALE) Rimg = cv.imread('.\data\\right03.jpg', cv.IMREAD_GRAYSCALE) size = (640, 480) # Recfitication RL, RR, ncamtxL, ncamtxR, Q, validPixROI1, validPixROI2 = cv.stereoRectify( camtxL, distL, camtxR, distR, size, rmtx, tvec) Lmap1, Lmap2 = cv.initUndistortRectifyMap(camtxL, distL, RL, ncamtxL, size, cv.CV_16SC2) # cv.imshow('',Limg) # cv.waitKey() Rmap1, Rmap2 = cv.initUndistortRectifyMap(camtxR, distR, RR, ncamtxR, size, cv.CV_16SC2) Limg_Rec = cv.remap(Limg, Lmap1, Lmap2, cv.INTER_LINEAR) Rimg_Rec = cv.remap(Rimg, Rmap1, Rmap2, cv.INTER_LINEAR) cv.imwrite('.\data\left_Rec.jpg', Limg_Rec) cv.imwrite('.\data\\right_Rec.jpg', Rimg_Rec) cv.namedWindow("left") cv.namedWindow("right") cv.namedWindow("depth") cv.moveWindow("left", 0, 0)
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()
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, \ leftCorners,rightCorners,\ mtxLeft, distLeft, \ mtxLeft, distLeft, \ imgSize,\ criteria=stereocalib_criteria, flags=flags) print("Stereo Rectification") R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(\ mtxLeft, distLeft, \ mtxLeft, distLeft, \ imgSize,\ R,\ T, \ alpha=1) mapXLeft, mapYLeft = cv2.initUndistortRectifyMap(mtxLeft, \ distLeft,\ R1,\ P1,\ imgSize, cv2.CV_32F) mapXRight, mapYRight = cv2.initUndistortRectifyMap(mtxLeft, \ distLeft,\ R2,\ P2,\
path = '/Users/tangling/python_L/match/' imgLeft = cv2.imread(path + "images/test/0left.png", cv2.IMREAD_GRAYSCALE) imgRight = cv2.imread(path + "images/test/0right.png", cv2.IMREAD_GRAYSCALE) # imgLeft = cv2.imread(imgLeftPath, cv2.IMREAD_GRAYSCALE) # imgRight = cv2.imread(imgRightPath, cv2.IMREAD_GRAYSCALE) imageSize = (imgLeft.shape[1], imgLeft.shape[0]) K1 = np.loadtxt(path + 'xj/K1.txt') K2 = np.loadtxt(path + 'xj/K2.txt') D1 = np.loadtxt(path + "xj/D1.txt") D2 = np.loadtxt(path + "xj/D1.txt") F = np.loadtxt(path + "xj/F.txt") R = np.loadtxt(path + "xj/R.txt") T = np.loadtxt(path + "xj/T.txt") R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(K1, D1, K2, D2, imageSize, R, T) mapl1, mapl2 = cv2.initUndistortRectifyMap(K1, D1, R1, P1, imageSize, cv2.CV_16SC2) mapr1, mapr2 = cv2.initUndistortRectifyMap(K2, D2, R2, P2, imageSize, cv2.CV_16SC2) left = cv2.remap(imgLeft, mapl1, mapl2, cv2.INTER_LINEAR) cv2.namedWindow("left", cv2.WINDOW_NORMAL) cv2.imshow("left", left) # print imgLeftPath right = cv2.remap(imgRight, mapr1, mapr2, cv2.INTER_LINEAR) cv2.namedWindow("right", cv2.WINDOW_NORMAL) cv2.imshow("right", right) # print imgRightPath projPointsRight = [] while True: cv.SetMouseCallback("left", mycv.onMouseEvent) cv.SetMouseCallback("right", mycv.onMouseEvent) c = cv.WaitKey()
# # poprawa lokalizacji punktow (podpiskelowo) # # # wizualizacja wykrytych naroznikow # # cv2.drawChessboardCorners(img_L, (7, 6), corners2_L, ret_L) # # cv2.imshow("Corners", img_L) ret_L, mtx_L, dist_L, rvecs_L, tvecs_L = cv2.calibrateCamera( objpoints, imgpoints_L, gray_L.shape[::-1], None, None) ret_R, mtx_R, dist_R, rvecs_R, tvecs_R = cv2.calibrateCamera( objpoints, imgpoints_R, gray_R.shape[::-1], None, None) retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate( objpoints, imgpoints_L, imgpoints_R, mtx_L, dist_L, mtx_R, dist_R, gray_L.shape[::-1]) R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, gray_R.shape[::-1], R, T) map1_L, map2_L = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, gray_L.shape[::-1], cv2.CV_16SC2) map1_R, map2_R = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, gray_L.shape[::-1], cv2.CV_16SC2) i = 1 img_L = cv2.imread('images_left/left%02d.jpg' % i) img_R = cv2.imread('images_right/right%02d.jpg' % i) dst_L = cv2.remap(img_L, map1_L, map2_L, cv2.INTER_LINEAR) dst_R = cv2.remap(img_R, map1_R, map2_R, cv2.INTER_LINEAR)
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