''' /////////////////////////////////////////////////////////////////////////// // // Copyright (c) 2018, STEREOLABS. // // All rights reserved. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // /////////////////////////////////////////////////////////////////////////// /***************************************************************************************** ** This sample demonstrates how to capture stereo images and calibration parameters ** ** from the ZED camera with OpenCV without using the ZED SDK. ** *****************************************************************************************/ ''' # open /Applications/Python\ 3.7/Install\ Certificates.command # https://stackoverflow.com/questions/50236117/scraping-ssl-certificate-verify-failed-error-for-http-en-wikipedia-org import numpy as np import os import configparser import sys import cv2 import wget left_point = (10,10) right_point = (200,200) left_point_np = [10,10] right_point_np = [200,200] def download_calibration_file(serial_number) : directory = os.getcwd() if os.name == 'nt' : hidden_path = os.getenv('APPDATA') + '\\Stereolabs\\settings\\' else : # hidden_path = '/usr/local/zed/settings/' hidden_path = directory + '/settings/' # serial_number = "000019387" calibration_file = hidden_path + 'SN' + str(serial_number) + '.conf' if os.path.isfile(calibration_file) == False: url = 'http://calib.stereolabs.com/?SN=' filename = wget.download(url=url+str(serial_number), out=calibration_file) if os.path.isfile(calibration_file) == False: print('Invalid Calibration File') return "" return calibration_file def init_calibration(calibration_file, image_size) : cameraMarix_left = cameraMatrix_right = map_left_y = map_left_x = map_right_y = map_right_x = np.array([]) config = configparser.ConfigParser() config.read(calibration_file) check_data = True resolution_str = '' if image_size.width == 2208 : resolution_str = '2K' elif image_size.width == 1920 : resolution_str = 'FHD' elif image_size.width == 1280 : resolution_str = 'HD' elif image_size.width == 672 : resolution_str = 'VGA' else: resolution_str = 'HD' check_data = False T_ = np.array([-float(config['STEREO']['Baseline'] if 'Baseline' in config['STEREO'] else 0), float(config['STEREO']['TY_'+resolution_str] if 'TY_'+resolution_str in config['STEREO'] else 0), float(config['STEREO']['TZ_'+resolution_str] if 'TZ_'+resolution_str in config['STEREO'] else 0)]) left_cam_cx = float(config['LEFT_CAM_'+resolution_str]['cx'] if 'cx' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_cy = float(config['LEFT_CAM_'+resolution_str]['cy'] if 'cy' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_fx = float(config['LEFT_CAM_'+resolution_str]['fx'] if 'fx' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_fy = float(config['LEFT_CAM_'+resolution_str]['fy'] if 'fy' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_k1 = float(config['LEFT_CAM_'+resolution_str]['k1'] if 'k1' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_k2 = float(config['LEFT_CAM_'+resolution_str]['k2'] if 'k2' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_p1 = float(config['LEFT_CAM_'+resolution_str]['p1'] if 'p1' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_p2 = float(config['LEFT_CAM_'+resolution_str]['p2'] if 'p2' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_p3 = float(config['LEFT_CAM_'+resolution_str]['p3'] if 'p3' in config['LEFT_CAM_'+resolution_str] else 0) left_cam_k3 = float(config['LEFT_CAM_'+resolution_str]['k3'] if 'k3' in config['LEFT_CAM_'+resolution_str] else 0) right_cam_cx = float(config['RIGHT_CAM_'+resolution_str]['cx'] if 'cx' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_cy = float(config['RIGHT_CAM_'+resolution_str]['cy'] if 'cy' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_fx = float(config['RIGHT_CAM_'+resolution_str]['fx'] if 'fx' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_fy = float(config['RIGHT_CAM_'+resolution_str]['fy'] if 'fy' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_k1 = float(config['RIGHT_CAM_'+resolution_str]['k1'] if 'k1' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_k2 = float(config['RIGHT_CAM_'+resolution_str]['k2'] if 'k2' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_p1 = float(config['RIGHT_CAM_'+resolution_str]['p1'] if 'p1' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_p2 = float(config['RIGHT_CAM_'+resolution_str]['p2'] if 'p2' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_p3 = float(config['RIGHT_CAM_'+resolution_str]['p3'] if 'p3' in config['RIGHT_CAM_'+resolution_str] else 0) right_cam_k3 = float(config['RIGHT_CAM_'+resolution_str]['k3'] if 'k3' in config['RIGHT_CAM_'+resolution_str] else 0) R_zed = np.array([float(config['STEREO']['RX_'+resolution_str] if 'RX_' + resolution_str in config['STEREO'] else 0), float(config['STEREO']['CV_'+resolution_str] if 'CV_' + resolution_str in config['STEREO'] else 0), float(config['STEREO']['RZ_'+resolution_str] if 'RZ_' + resolution_str in config['STEREO'] else 0)]) R, _ = cv2.Rodrigues(R_zed) cameraMatrix_left = np.array([[left_cam_fx, 0, left_cam_cx], [0, left_cam_fy, left_cam_cy], [0, 0, 1]]) cameraMatrix_right = np.array([[right_cam_fx, 0, right_cam_cx], [0, right_cam_fy, right_cam_cy], [0, 0, 1]]) distCoeffs_left = np.array([[left_cam_k1], [left_cam_k2], [left_cam_p1], [left_cam_p2], [left_cam_k3]]) distCoeffs_right = np.array([[right_cam_k1], [right_cam_k2], [right_cam_p1], [right_cam_p2], [right_cam_k3]]) T = np.array([[T_[0]], [T_[1]], [T_[2]]]) R1 = R2 = P1 = P2 = np.array([]) R1, R2, P1, P2 = cv2.stereoRectify(cameraMatrix1=cameraMatrix_left, cameraMatrix2=cameraMatrix_right, distCoeffs1=distCoeffs_left, distCoeffs2=distCoeffs_right, R=R, T=T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0, imageSize=(image_size.width, image_size.height), newImageSize=(image_size.width, image_size.height))[0:4] map_left_x, map_left_y = cv2.initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, (image_size.width, image_size.height), cv2.CV_32FC1) map_right_x, map_right_y = cv2.initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2, (image_size.width, image_size.height), cv2.CV_32FC1) cameraMatrix_left = P1 cameraMatrix_right = P2 return cameraMatrix_left, cameraMatrix_right, map_left_x, map_left_y, map_right_x, map_right_y class Resolution : width = 1280 height = 720 def onMouse(event, x, y, flags, param): global left_point global right_point global left_point_np global right_point_np if event == cv2.EVENT_LBUTTONDOWN: # cv2.circle(lastImage, (x, y), 3, (255, 0, 0), -1) left_point = (x,y) left_point_np = [x,y] # print("left_point", left_point) else : right_point = (x,left_point[1]) right_point_np = [x,left_point[1]] # print("right_point", right_point) #make sure variables are numpy arrays def DLT(P1, P2, point1, point2): A = [point1[1]*P1[2,:] - P1[1,:], P1[0,:] - point1[0]*P1[2,:], point2[1]*P2[2,:] - P2[1,:], P2[0,:] - point2[0]*P2[2,:] ] A = np.array(A).reshape((4,4)) #print('A: ') #print(A) B = A.transpose() @ A from scipy import linalg U, s, Vh = linalg.svd(B, full_matrices = False) #print('Triangulated point: ') #print(Vh[3,0:3]/Vh[3,3]) return Vh[3,0:3]/Vh[3,3] def main() : # if len(sys.argv) == 1 : # print('Please provide ZED serial number') # exit(1) # Open the ZED camera cap = cv2.VideoCapture(0) if cap.isOpened() == 0: exit(-1) image_size = Resolution() image_size.width = 1280 image_size.height = 720 # Set the video resolution to HD720 cap.set(cv2.CAP_PROP_FRAME_WIDTH, image_size.width*2) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, image_size.height) # serial_number = int(sys.argv[1]) serial_number = "000019387" calibration_file = download_calibration_file(serial_number) if calibration_file == "": exit(1) print("Calibration file found. Loading...") camera_matrix_left, camera_matrix_right, map_left_x, map_left_y, map_right_x, map_right_y = init_calibration(calibration_file, image_size) print("camera_matrix_left ",camera_matrix_left) print("camera_matrix_right ",camera_matrix_right) while True : # Get a new frame from camera retval, frame = cap.read() # Extract left and right images from side-by-side left_right_image = np.split(frame, 2, axis=1) # Display images # cv2.imshow("left RAW", left_right_image[0]) left_rect = cv2.remap(left_right_image[0], map_left_x, map_left_y, interpolation=cv2.INTER_LINEAR) right_rect = cv2.remap(left_right_image[1], map_right_x, map_right_y, interpolation=cv2.INTER_LINEAR) # cv2.imshow("left RECT", left_rect) # cv2.imshow("right RECT", right_rect) #https://answers.opencv.org/question/175912/how-to-display-multiple-images-in-one-window/ numpy_horizontal = np.hstack((left_rect, right_rect)) # start_point = (20,20) # end_point = (200,200) numpy_horizontal = cv2.line(numpy_horizontal, left_point, right_point,(255,255,0),2) # https://github.com/stereolabs/zed-examples/issues/44 # https://github.com/stereolabs/zed-opencv-native/blob/f7e17b6368ecbc432c0ca0d64be4586e88db8799/src/calibration.cpp#L74-L175 point_4d_hom = cv2.triangulatePoints(camera_matrix_left, camera_matrix_right, left_point, right_point) good_pts_mask = np.where(point_4d_hom[3]!= 0)[0] point_4d = point_4d_hom / point_4d_hom[3] # print("point_4d_hom ",point_4d_hom) # print("point_4d ",point_4d) _p3d = DLT(camera_matrix_left, camera_matrix_right, left_point_np, right_point_np) #calculate 3d position of keypoint # print("point_4d_hom ",point_4d_hom) print("_p3d ",_p3d) myStr = "left: " + str(left_point[0]) + ", " + str(left_point[1]) + "\n" myStr += "right: " + str(right_point[0]) + ", " + str(right_point[1]) + "\n" myStr += "x: " + str(point_4d[0]) + "\n" myStr += "y: " + str(point_4d[1]) + "\n" myStr += "z: " + str(point_4d[2]) + "\n" y0, dy = 20, 25 for i, line in enumerate(myStr.split('\n')): y = y0 + i*dy numpy_horizontal = cv2.putText(numpy_horizontal,line, (10,y),cv2.FONT_HERSHEY_SIMPLEX,0.6,(255,255,0),2,cv2.LINE_AA) #define axes points in homogeneous coorindates axes = np.array([[0,0,0,1], [1,0,0,1], [0,1,0,1], [0,0,1,1]]) #project to camera view projected_axes = (camera_matrix_left @ axes.T).T #Remove the homogeneous coordinate information projected_axes = projected_axes[:,:2]/projected_axes[:,2:3] #convert to integers because image space is integer coorindates projected_axes = projected_axes.astype('int32') #define some colors for your axes cols = [(0,0,255), (0,255,0), (255,0,0)] #the axes are drawn with ['red', 'green', 'blue'] colors #draw the axes on the camera image for _ax, _col in zip(projected_axes[1:], cols): _o = projected_axes[0] #origin point cv2.line(numpy_horizontal, (_o[0], _o[1]), (_ax[0], _ax[1]), _col, 2) numpy_horizontal_concat = np.concatenate((left_rect, right_rect), axis=1) cv2.imshow("image", numpy_horizontal) cv2.namedWindow("image") cv2.setMouseCallback("image", onMouse) # cv2.imshow('Numpy Horizontal Concat', numpy_horizontal_concat) if cv2.waitKey(30) >= 0 : break exit(0) if __name__ == "__main__": main()