Last active
April 15, 2022 00:46
-
-
Save stephanschulz/d2c20c8ebb731d7362250b11730b1359 to your computer and use it in GitHub Desktop.
Revisions
-
stephanschulz revised this gist
Apr 15, 2022 . 1 changed file with 29 additions and 11 deletions.There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal file line number Diff line number Diff line change @@ -37,6 +37,8 @@ left_point = (10,10) right_point = (200,200) left_point_dis = (10,10) right_point_dis = (200,200) left_point_np = [10,10] right_point_np = [200,200] @@ -150,23 +152,27 @@ def init_calibration(calibration_file, image_size) : return cameraMatrix_left, cameraMatrix_right, map_left_x, map_left_y, map_right_x, map_right_y class Resolution : width = 2208 #1280 height = 1242 #720 def onMouse(event, x, y, flags, param): global left_point global right_point global left_point_dis global right_point_dis 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_dis = (x,y) left_point = (x,y) left_point_np = [x,y] # print("left_point", left_point) else : right_point_dis = (x,left_point[1]) right_point = (x-2208,left_point[1]) right_point_np = [x-2208,left_point[1]] # print("right_point", right_point) #make sure variables are numpy arrays @@ -201,8 +207,8 @@ def main() : exit(-1) image_size = Resolution() image_size.width = 2208 #1280 image_size.height = 1242 #720 # Set the video resolution to HD720 cap.set(cv2.CAP_PROP_FRAME_WIDTH, image_size.width*2) @@ -227,19 +233,16 @@ def main() : # 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) # print("right_rect ",right_rect.shape[0],right_rect.shape[1]) # 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)) numpy_horizontal = cv2.line(numpy_horizontal, left_point_dis, right_point_dis,(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 @@ -268,6 +271,7 @@ def main() : 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) # left image #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 @@ -284,6 +288,20 @@ def main() : _o = projected_axes[0] #origin point cv2.line(numpy_horizontal, (_o[0], _o[1]), (_ax[0], _ax[1]), _col, 2) # right image projected_axes = (camera_matrix_right @ 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") -
Apr 14, 2022 .There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal file line number Diff line number Diff line change @@ -0,0 +1,300 @@ ''' /////////////////////////////////////////////////////////////////////////// // // 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()