diff --git a/calibrate.py b/calibrate.py index 6221777dd..47b333abd 100755 --- a/calibrate.py +++ b/calibrate.py @@ -27,72 +27,39 @@ red = (255, 0, 0) green = (0, 255, 0) -if hasattr(dai.CameraBoardSocket, 'CAM_A'): - stringToCam = { - 'RGB' : dai.CameraBoardSocket.CAM_A, - 'LEFT' : dai.CameraBoardSocket.CAM_B, - 'RIGHT' : dai.CameraBoardSocket.CAM_C, - 'CAM_A' : dai.CameraBoardSocket.CAM_A, - 'CAM_B' : dai.CameraBoardSocket.CAM_B, - 'CAM_C' : dai.CameraBoardSocket.CAM_C, - 'CAM_D' : dai.CameraBoardSocket.CAM_D, - 'CAM_E' : dai.CameraBoardSocket.CAM_E, - 'CAM_F' : dai.CameraBoardSocket.CAM_F, - 'CAM_G' : dai.CameraBoardSocket.CAM_G, - 'CAM_H' : dai.CameraBoardSocket.CAM_H - } - camToString = { - dai.CameraBoardSocket.CAM_A : 'RGB' , - dai.CameraBoardSocket.CAM_B : 'LEFT' , - dai.CameraBoardSocket.CAM_C : 'RIGHT', - dai.CameraBoardSocket.CAM_A : 'CAM_A', - dai.CameraBoardSocket.CAM_B : 'CAM_B', - dai.CameraBoardSocket.CAM_C : 'CAM_C', - dai.CameraBoardSocket.CAM_D : 'CAM_D', - dai.CameraBoardSocket.CAM_E : 'CAM_E', - dai.CameraBoardSocket.CAM_F : 'CAM_F', - dai.CameraBoardSocket.CAM_G : 'CAM_G', - dai.CameraBoardSocket.CAM_H : 'CAM_H' - } -else: - stringToCam = { - 'RGB': dai.CameraBoardSocket.RGB, - 'LEFT': dai.CameraBoardSocket.LEFT, - 'RIGHT': dai.CameraBoardSocket.RIGHT, - 'AUTO': dai.CameraBoardSocket.AUTO, - 'CAM_A' : dai.CameraBoardSocket.RGB, - 'CAM_B' : dai.CameraBoardSocket.LEFT, - 'CAM_C' : dai.CameraBoardSocket.RIGHT - } - - camToString = { - # dai.CameraBoardSocket.RGB : 'RGB' , - # dai.CameraBoardSocket.LEFT : 'LEFT' , - # dai.CameraBoardSocket.RIGHT : 'RIGHT', - # dai.CameraBoardSocket.AUTO : 'AUTO', - dai.CameraBoardSocket.RGB : 'CAM_A', - dai.CameraBoardSocket.LEFT : 'CAM_B', - dai.CameraBoardSocket.RIGHT : 'CAM_C', - } + +stringToCam = { + 'RGB' : dai.CameraBoardSocket.CAM_A, + 'LEFT' : dai.CameraBoardSocket.CAM_B, + 'RIGHT' : dai.CameraBoardSocket.CAM_C, + 'CAM_A' : dai.CameraBoardSocket.CAM_A, + 'CAM_B' : dai.CameraBoardSocket.CAM_B, + 'CAM_C' : dai.CameraBoardSocket.CAM_C, + 'CAM_D' : dai.CameraBoardSocket.CAM_D, + 'CAM_E' : dai.CameraBoardSocket.CAM_E, + 'CAM_F' : dai.CameraBoardSocket.CAM_F, + 'CAM_G' : dai.CameraBoardSocket.CAM_G, + 'CAM_H' : dai.CameraBoardSocket.CAM_H +} camToMonoRes = { - 'OV7251' : dai.MonoCameraProperties.SensorResolution.THE_480_P, - 'OV9*82' : dai.MonoCameraProperties.SensorResolution.THE_800_P, - 'OV9282' : dai.MonoCameraProperties.SensorResolution.THE_800_P, - 'AR0234' : dai.MonoCameraProperties.SensorResolution.THE_1200_P, - } + 'OV7251' : dai.MonoCameraProperties.SensorResolution.THE_480_P, + 'OV9*82' : dai.MonoCameraProperties.SensorResolution.THE_800_P, + 'OV9282' : dai.MonoCameraProperties.SensorResolution.THE_800_P, + 'AR0234' : dai.MonoCameraProperties.SensorResolution.THE_1200_P, +} camToRgbRes = { - 'IMX378' : dai.ColorCameraProperties.SensorResolution.THE_4_K, - 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, - 'OV9*82' : dai.ColorCameraProperties.SensorResolution.THE_800_P, - 'OV9282' : dai.ColorCameraProperties.SensorResolution.THE_800_P, - 'OV9782' : dai.ColorCameraProperties.SensorResolution.THE_800_P, - 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP, - 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, - 'IMX296' : dai.ColorCameraProperties.SensorResolution.THE_1440X1080, - } + 'IMX378' : dai.ColorCameraProperties.SensorResolution.THE_4_K, + 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, + 'OV9*82' : dai.ColorCameraProperties.SensorResolution.THE_800_P, + 'OV9282' : dai.ColorCameraProperties.SensorResolution.THE_800_P, + 'OV9782' : dai.ColorCameraProperties.SensorResolution.THE_800_P, + 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP, + 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, + 'IMX296' : dai.ColorCameraProperties.SensorResolution.THE_1440X1080, +} def create_blank(width, height, rgb_color=(0, 0, 0)): """Create new image(numpy array) filled with certain color in RGB""" @@ -164,12 +131,10 @@ def parse_args(): help="Invert horizontal axis of the camera for the display") # parser.add_argument("-ep", "--maxEpiploarError", default="1.0", type=float, required=False, # help="Sets the maximum epiploar allowed with rectification") - parser.add_argument("-cm", "--cameraMode", default="perspective", type=str, - required=False, help="Choose between perspective and Fisheye") + parser.add_argument("-cm", "--cameraModel", default="", choices=["perspective", "fisheye"], type=str, + required=False, help="Overwrite the camera model specified in board config file. Choose between perspective and fisheye.") parser.add_argument("-rlp", "--rgbLensPosition", default=-1, type=int, required=False, help="Set the manual lens position of the camera for calibration") - parser.add_argument("-fps", "--fps", default=10, type=int, - required=False, help="Set capture FPS for all cameras. Default: %(default)s") parser.add_argument("-cd", "--captureDelay", default=5, type=int, required=False, help="Choose how much delay to add between pressing the key and capturing the image. Default: %(default)s") parser.add_argument("-d", "--debug", default=False, action="store_true", help="Enable debug logs.") @@ -177,12 +142,21 @@ def parse_args(): help="Enable writing to Factory Calibration.") parser.add_argument("-osf", "--outputScaleFactor", type=float, default=0.5, help="set the scaling factor for output visualization. Default: 0.5.") + parser.add_argument('-fps', '--framerate', type=float, default=10, + help="FPS to set for all cameras. Default: %(default)s") parser.add_argument("-sync", "--minSyncTime", type=float, default=0.2, - help="set the minimum time enforced between frames to keep synchronization. Default: 0.2.") + help="set the minimum time enforced between frames to keep synchronization. Default: %(default)s.") parser.add_argument("-q", "--minQueueDepth", type=int, default=4, - help="set the minimum queue depth for syncing before retrieving synced frames. Default: 1.") - - + help="set the minimum queue depth for syncing before retrieving synced frames. Default: %(default)s.") + parser.add_argument('-scp', '--saveCalibPath', type=str, default="", + help="Save calibration file to this path") + parser.add_argument('-dst', '--datasetPath', type=str, default="dataset", + help="Path to dataset used for processing images") + parser.add_argument('-mt', '--mouseTrigger', default=False, action="store_true", + help="Enable mouse trigger for image capture") + parser.add_argument('-l', '--traceLevel', type=int, default=2, + help="Set the debug trace level. Default: %(default)s.") + options = parser.parse_args() # Set some extra defaults, `-brd` would override them @@ -201,7 +175,6 @@ def parse_args(): options.rgbLensPosition = 135 return options - class MessageSync: def __init__(self, num_queues, min_diff_timestamp, max_num_messages=10, min_queue_depth=3): self.num_queues = num_queues @@ -294,6 +267,7 @@ class Main: current_polygon = 0 images_captured_polygon = 0 images_captured = 0 + camera_model = "perspective" def __init__(self): global debug @@ -379,6 +353,10 @@ def close(self): if self.device: self.device.close() + def mouse_event_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + self.mouseTrigger = True + def is_markers_found(self, frame): marker_corners, _, _ = cv2.aruco.detectMarkers( frame, self.aruco_dictionary) @@ -404,7 +382,7 @@ def test_camera_orientation(self, frame_l, frame_r): def create_pipeline(self): pipeline = dai.Pipeline() - fps = self.args.fps + fps = self.args.framerate cams = {} for cam_id in self.board_config['cameras']: cam_info = self.board_config['cameras'][cam_id] @@ -460,7 +438,7 @@ def parse_frame(self, frame, stream_name): filename = calibUtils.image_filename( stream_name, self.current_polygon, self.images_captured) - cv2.imwrite("dataset/{}/{}".format(stream_name, filename), frame) + cv2.imwrite(f"{str(self.dataset_path)}/{stream_name}/{filename}", frame) print("py: Saved image as: " + str(filename)) return True @@ -552,6 +530,7 @@ def capture_images_sync(self): self.display_name = "Image Window" syncCollector = MessageSync(len(self.camera_queue.keys()), min_diff_timestamp=self.args.minSyncTime, min_queue_depth=self.args.minQueueDepth) + self.mouseTrigger = False # Clear events streams = self.device.getQueueEvents(list(self.camera_queue.keys())) @@ -675,10 +654,11 @@ def capture_images_sync(self): if key == 27 or key == ord("q"): print("py: Calibration has been interrupted!") raise SystemExit(0) - elif key == ord(" "): + elif key == ord(" ") or self.mouseTrigger: start_timer = True prev_time = time.time() timer = self.args.captureDelay + self.mouseTrigger = False if start_timer == True: curr_time = time.time() @@ -696,6 +676,10 @@ def capture_images_sync(self): 7, (0, 255, 255), 4, cv2.LINE_AA) + cv2.namedWindow(self.display_name) + if self.args.mouseTrigger: + cv2.setMouseCallback(self.display_name, self.mouse_event_callback) + cv2.imshow(self.display_name, combinedImage) tried = {} allPassed = True @@ -941,21 +925,25 @@ def capture_images(self): def calibrate(self): print("Starting image processing") - stereo_calib = calibUtils.StereoCalibration() - # self.args.cameraMode = 'perspective' # hardcoded for now - try: + stereo_calib = calibUtils.StereoCalibration(self.args.traceLevel) + + self.camera_model = self.board_config.get("camera_model", "perspective") + if self.args.cameraModel != "": # If camera model is specified in the command line, use that + self.camera_model = self.args.cameraModel + + try: # stereo_calib = StereoCalibration() print("Starting image processingxxccx") print(self.args.squaresX) status, result_config = stereo_calib.calibrate( self.board_config, - self.dataset_path, + str(self.dataset_path), self.args.squareSizeCm, self.args.markerSizeCm, self.args.squaresX, self.args.squaresY, - self.args.cameraMode, + self.camera_model, self.args.rectifiedDisp) # Turn off enable disp rectify calibration_handler = dai.CalibrationHandler() @@ -1014,6 +1002,10 @@ def prepare_calibration_handler(self, result_config, calibration_handler): calibration_handler.setDistortionCoefficients(stringToCam[camera], cam_info['dist_coeff']) calibration_handler.setCameraIntrinsics(stringToCam[camera], cam_info['intrinsics'], cam_info['size'][0], cam_info['size'][1]) calibration_handler.setFov(stringToCam[camera], cam_info['hfov']) + if self.camera_model == "perspective": + calibration_handler.setCameraType(stringToCam[camera], dai.CameraModel.Perspective) + elif self.camera_model == "fisheye": + calibration_handler.setCameraType(stringToCam[camera], dai.CameraModel.Fisheye) if 'hasAutofocus' in cam_info and cam_info['hasAutofocus']: calibration_handler.setLensPosition(stringToCam[camera], self.focus_value) @@ -1088,7 +1080,14 @@ def flash(self, result_config): eeepromData.version = 7 print(f'EEPROM VERSION being flashed is -> {eeepromData.version}') mx_serial_id = self.device.getDeviceInfo().getMxId() - calib_dest_path = self.dest_path / f"{mx_serial_id}.json" + + calib_dest_folder = self.dest_path + if self.args.saveCalibPath: + calib_dest_folder = Path(self.args.saveCalibPath) + calib_dest_folder.mkdir(parents=True, exist_ok=True) + + calib_dest_path = calib_dest_folder / f"{mx_serial_id}.json" + calibration_handler.eepromToJsonFile(calib_dest_path) # try: self.device.flashCalibration2(calibration_handler) @@ -1144,13 +1143,21 @@ def flash(self, result_config): def run(self): + + if self.args.datasetPath: + self.dataset_path = Path(self.args.datasetPath) + else: + self.dataset_path = Path("dataset").absolute() + + self.dataset_path.mkdir(parents=True, exist_ok=True) + if 'capture' in self.args.mode: try: - if Path('dataset').exists(): - shutil.rmtree('dataset/') + if self.dataset_path.exists(): + shutil.rmtree(str(self.dataset_path)) for cam_id in self.board_config['cameras']: name = self.board_config['cameras'][cam_id]['name'] - Path("dataset/{}".format(name)).mkdir(parents=True, exist_ok=True) + (self.dataset_path / name).mkdir(parents=True, exist_ok=True) except OSError: traceback.print_exc() @@ -1158,7 +1165,7 @@ def run(self): raise SystemExit(1) self.show_info_frame() self.capture_images_sync() - self.dataset_path = str(Path("dataset").absolute()) + if 'process' in self.args.mode: status, err_text, result_config = self.calibrate() if 'flash' in self.args.mode: diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 805b0dae9..3714c6534 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -11,10 +11,10 @@ import cv2.aruco as aruco from pathlib import Path from collections import deque +from depthai_helpers.image_scaler import ImageScaler + # Creates a set of 13 polygon coordinates -traceLevel = 2 -# trace = 3 -> Undisted image viz -# trace = 2 -> Print logs + def setPolygonCoordinates(height, width): @@ -87,10 +87,10 @@ def polygon_from_image_name(image_name): class StereoCalibration(object): - global traceLevel """Class to Calculate Calibration and Rectify a Stereo Camera.""" - def __init__(self): + def __init__(self, traceLevel: int): + self.traceLevel = traceLevel """Class to Calculate Calibration and Rectify a Stereo Camera.""" def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify): @@ -195,7 +195,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ return 1, board_config - def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): + def analyze_charuco(self, images, resize_img_func = None): """ Charuco base pose estimation. """ @@ -209,43 +209,21 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): # SUB PIXEL CORNER DETECTION CRITERION criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) - count = 0 for im in images: - if traceLevel == 2: + if self.traceLevel == 2: print("=> Processing image {0}".format(im)) img_pth = Path(im) frame = cv2.imread(im) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1]) - - if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]): - if int(expected_height) == req_resolution[0]: - # resizing to have both stereo and rgb to have same - # resolution to capture extrinsics of the rgb-right camera - gray = cv2.resize(gray, req_resolution[::-1], - interpolation=cv2.INTER_CUBIC) - else: - # resizing and cropping to have both stereo and rgb to have same resolution - # to calculate extrinsics of the rgb-right camera - scale_width = req_resolution[1]/gray.shape[1] - dest_res = ( - int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width)) - gray = cv2.resize( - gray, dest_res, interpolation=cv2.INTER_CUBIC) - if gray.shape[0] < req_resolution[0]: - raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format( - gray.shape[0], req_resolution[0])) - # print(gray.shape[0] - req_resolution[0]) - del_height = (gray.shape[0] - req_resolution[0]) // 2 - # gray = gray[: req_resolution[0], :] - gray = gray[del_height: del_height + req_resolution[0], :] - - count += 1 + + if resize_img_func is not None: + gray = resize_img_func(gray) + marker_corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers( gray, self.aruco_dictionary) marker_corners, ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(gray, self.board, marker_corners, ids, rejectedCorners=rejectedImgPoints) - if traceLevel == 1: + if self.traceLevel == 1: print('{0} number of Markers corners detected in the image {1}'.format( len(marker_corners), img_pth.name)) if len(marker_corners) > 0: @@ -282,20 +260,20 @@ def calibrate_intrinsics(self, image_files, hfov): allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files) if self.cameraModel == 'perspective': - ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_charuco( + ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_perspective( allCorners, allIds, imsize, hfov) # (Height, width) - if traceLevel == 3: - self.fisheye_undistort_visualizaation( + if self.traceLevel == 3: + self.fisheye_undistort_visualization( image_files, camera_matrix, distortion_coefficients, imsize) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize else: print('Fisheye--------------------------------------------------') - ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_fisheye( - allCorners, allIds, imsize) - if traceLevel == 3: - self.fisheye_undistort_visualizaation( + ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_fisheye( + allCorners, allIds, imsize, hfov) + if self.traceLevel == 3: + self.fisheye_undistort_visualization( image_files, camera_matrix, distortion_coefficients, imsize) @@ -321,46 +299,18 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu # print(images_left[0]) # print(images_right[0]) - scale = None - scale_req = False - frame_left_shape = cv2.imread(images_left[0], 0).shape # (h,w) - frame_right_shape = cv2.imread(images_right[0], 0).shape - scalable_res = frame_left_shape - scaled_res = frame_right_shape - # print(f' Frame left shape: {frame_left_shape} and right shape: {frame_right_shape}') - if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]: - scale_req = True - scale = frame_right_shape[1] / frame_left_shape[1] - elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]: - scale_req = True - scale = frame_left_shape[1] / frame_right_shape[1] - scalable_res = frame_right_shape - scaled_res = frame_left_shape - - if scale_req: - scaled_height = scale * scalable_res[0] - diff = scaled_height - scaled_res[0] - # if scaled_height < smaller_res[0]: - if diff < 0: - scaled_res = (int(scaled_height), scaled_res[1]) + frame_left_shape = cv2.imread(images_left[0], 0).shape[:2][::-1] # (w,h) + frame_right_shape = cv2.imread(images_right[0], 0).shape[:2][::-1] - print( - f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}') - print("Original res Left :{}".format(frame_left_shape)) - print("Original res Right :{}".format(frame_right_shape)) - print("Scale res :{}".format(scaled_res)) + scaler = ImageScaler(frame_left_shape, frame_right_shape) - # scaled_res = (scaled_height, ) - M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) - M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) + M_lp, M_rp = scaler.transform_intrinsics(M_l, M_r) # print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") - allCorners_l, allIds_l, _, _, imsize_l, _ = self.analyze_charuco( - images_left, scale_req, scaled_res) + allCorners_l, allIds_l, _, _, imsize_l, _ = self.analyze_charuco(images_left, scaler.transform_img_a) # print("~~~~~~~~~~~ POSE ESTIMATION RIGHT CAMERA ~~~~~~~~~~~~~") - allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco( - images_right, scale_req, scaled_res) + allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco(images_right, scaler.transform_img_b) print(f'Image size of right side (w, h):{imsize_r}') print(f'Image size of left side (w, h):{imsize_l}') @@ -370,79 +320,44 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu return self.calibrate_stereo( allCorners_l, allIds_l, allCorners_r, allIds_r, imsize_r, M_lp, d_l, M_rp, d_r, guess_translation, guess_rotation) - def scale_intrinsics(self, intrinsics, originalShape, destShape): - scale = destShape[1] / originalShape[1] # scale on width - scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]]) - scaled_intrinsics = np.matmul(scale_mat, intrinsics) - """ print("Scaled height offset : {}".format( - (originalShape[0] * scale - destShape[0]) / 2)) - print("Scaled width offset : {}".format( - (originalShape[1] * scale - destShape[1]) / 2)) """ - scaled_intrinsics[1][2] -= (originalShape[0] # c_y - along height of the image - * scale - destShape[0]) / 2 - scaled_intrinsics[0][2] -= (originalShape[1] # c_x width of the image - * scale - destShape[1]) / 2 - if traceLevel == 2: - print('original_intrinsics') - print(intrinsics) - print('scaled_intrinsics') - print(scaled_intrinsics) - - return scaled_intrinsics - - def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): + def fisheye_undistort_visualization(self, img_list, K, D, img_size): for im in img_list: # print(im) img = cv2.imread(im) # h, w = img.shape[:2] if self.cameraModel == 'perspective': - # kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0) + kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0) # print(f'K scaled is \n {kScaled} and size is \n {img_size}') # print(f'D Value is \n {D}') map1, map2 = cv2.initUndistortRectifyMap( K, D, np.eye(3), K, img_size, cv2.CV_32FC1) else: map1, map2 = cv2.fisheye.initUndistortRectifyMap( - K, D, np.eye(3), K, img_size, cv2.CV_32FC1) + K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1) undistorted_img = cv2.remap( img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) cv2.imshow("undistorted", undistorted_img) - if traceLevel == 4: + if self.traceLevel == 4: print(f'image path - {im}') print(f'Image Undistorted Size {undistorted_img.shape}') k = cv2.waitKey(0) if k == 27: # Esc key to stop break + cv2.destroyWindow("undistorted") - def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): + def calibrate_camera_perspective(self, allCorners, allIds, imsize, hfov): """ Calibrates the camera using the dected corners. """ - f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2))) - # TODO(sachin): Change the initialization to be initialized using the guess from fov print("CAMERA CALIBRATION") print(imsize) + f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2))) cameraMatrixInit = np.array([[f, 0.0, imsize[0]/2], [0.0, f, imsize[1]/2], [0.0, 0.0, 1.0]]) - - # cameraMatrixInit = np.array([[857.1668, 0.0, 643.9126], - # [0.0, 856.0823, 387.56018], - # [0.0, 0.0, 1.0]]) - """ if imsize[1] < 700: - cameraMatrixInit = np.array([[400.0, 0.0, imsize[0]/2], - [0.0, 400.0, imsize[1]/2], - [0.0, 0.0, 1.0]]) - elif imsize[1] < 1100: - cameraMatrixInit = np.array([[857.1668, 0.0, 643.9126], - [0.0, 856.0823, 387.56018], - [0.0, 0.0, 1.0]]) - else: - cameraMatrixInit = np.array([[3819.8801, 0.0, 1912.8375], - [0.0, 3819.8801, 1135.3433], - [0.0, 0.0, 1.]]) """ - if traceLevel == 1: + + if self.traceLevel == 1: print( f'Camera Matrix initialization with HFOV of {hfov} is.............') print(cameraMatrixInit) @@ -454,7 +369,6 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): # + cv2.CALIB_FIX_K6 ) - # flags = (cv2.CALIB_RATIONAL_MODEL) (ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, stdDeviationsIntrinsics, stdDeviationsExtrinsics, @@ -467,12 +381,12 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): distCoeffs=distCoeffsInit, flags=flags, criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9)) - if traceLevel == 2: + if self.traceLevel == 2: print('Per View Errors...') print(perViewErrors) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors - def calibrate_fisheye(self, allCorners, allIds, imsize): + def calibrate_camera_fisheye(self, allCorners, allIds, imsize, hfov): one_pts = self.board.chessboardCorners obj_points = [] for i in range(len(allIds)): @@ -481,16 +395,19 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): obj_pts_sub.append(one_pts[allIds[i][j]]) obj_points.append(np.array(obj_pts_sub, dtype=np.float32)) - cameraMatrixInit = np.array([[500, 0.0, 643.9126], - [0.0, 500, 387.56018], - [0.0, 0.0, 1.0]]) + f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2))) + cameraMatrixInit = np.array([[f, 0.0, imsize[0]/2], + [0.0, f, imsize[1]/2], + [0.0, 0.0, 1.0]]) print("Camera Matrix initialization.............") print(cameraMatrixInit) - flags = 0 - # flags |= cv2.fisheye.CALIB_CHECK_COND - # flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS - flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW + flags = ( + + cv2.fisheye.CALIB_USE_INTRINSIC_GUESS + + cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + + cv2.fisheye.CALIB_CHECK_COND + + cv2.fisheye.CALIB_FIX_SKEW + ) distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + @@ -518,8 +435,6 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: # continue for j in range(len(allIds_l[i])): - # print(f' allIds_l[i][j] is {allIds_l[i][j]}') - # print(f' allIds_r[i] is {allIds_r[i]}') idx = np.where(allIds_r[i] == allIds_l[i][j]) if idx[0].size == 0: continue @@ -536,36 +451,30 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz return -1, "Stereo Calib failed due to less common features" stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 50000, 1e-9) + cv2.TERM_CRITERIA_EPS, 20000, 1e-9) if self.cameraModel == 'perspective': - flags = 0 - flags |= cv2.CALIB_FIX_INTRINSIC - # flags |= cv2.CALIB_USE_INTRINSIC_GUESS - flags |= cv2.CALIB_RATIONAL_MODEL - # flags |= cv2.CALIB_FIX_K6 - # print(flags) - if traceLevel == 1: + flags = ( + + cv2.CALIB_FIX_INTRINSIC + + cv2.CALIB_RATIONAL_MODEL + ) + + if self.traceLevel == 1: print('Printing Extrinsics guesses...') print(r_in) print(t_in) - if 1: - ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended( - obj_pts, left_corners_sampled, right_corners_sampled, - cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, - R=r_in, T=t_in, criteria=stereocalib_criteria , flags=flags) - else: - ret, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, R, T, E, F, _ = cv2.stereoCalibrateExtended( - obj_pts, left_corners_sampled, right_corners_sampled, - cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, - R=r_in, T=t_in, criteria=stereocalib_criteria , flags=flags) + + ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended( + obj_pts, left_corners_sampled, right_corners_sampled, + cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, + R=r_in, T=t_in, criteria=stereocalib_criteria , flags=flags) print(f'Reprojection error is {ret}') print('Printing Extrinsics res...') print(R) print(T) r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True) - print(f'Euler angles in XYZ {r_euler}') + print(f'Euler angles in XYZ {r_euler} degs') # ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate( @@ -573,7 +482,6 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, # criteria=stereocalib_criteria, flags=flags) # if np.absolute(T[1]) > 0.2: - print(f'Image size---: {imsize}') R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix_l, @@ -593,69 +501,72 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz return [ret, R, T, R_l, R_r, P_l, P_r] elif self.cameraModel == 'fisheye': - flags = 0 - flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - flags |= cv2.fisheye.CALIB_CHECK_COND - flags |= cv2.fisheye.CALIB_FIX_SKEW - # flags |= cv2.CALIB_FIX_INTRINSIC - # flags |= cv2.CALIB_RATIONAL_MODEL - # TODO(sACHIN): Try without intrinsic guess - # flags |= cv2.CALIB_USE_INTRINSIC_GUESS - # TODO(sACHIN): Try without intrinsic guess - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW - if traceLevel == 3: + # make sure all images have the same *number of* points + min_num_points = min([len(pts) for pts in obj_pts]) + obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts] + left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled] + right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled] + + flags = ( + + cv2.fisheye.CALIB_FIX_INTRINSIC + + cv2.fisheye.CALIB_FIX_K1 + + cv2.fisheye.CALIB_FIX_K2 + + cv2.fisheye.CALIB_FIX_K3 + + cv2.fisheye.CALIB_FIX_K4 + ) + + if self.traceLevel == 3: print('Fisyeye stereo model..................') - print(len(left_corners_sampled)) - for i in range(0, len(left_corners_sampled)): - print(f'{len(left_corners_sampled[i])} -- {len(obj_pts[i])}') - print('Right cornered samples..................') - print(len(right_corners_sampled)) - for i in range(0, len(right_corners_sampled)): - print(f'{len(right_corners_sampled[i])} -- {len(obj_pts[i])}') - del obj_pts[4] - del right_corners_sampled[4] - del left_corners_sampled[4] - ret, M1, d1, M2, d2, R, T, E, F = cv2.fisheye.stereoCalibrate( - obj_pts, left_corners_sampled, right_corners_sampled, + + (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate( + obj_pts_truncated, left_corners_truncated, right_corners_truncated, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, flags=flags, criteria=stereocalib_criteria), None, None - R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( + + print(f'Reprojection error is {ret}') + print('Printing Extrinsics res...') + print(R) + print(T) + r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True) + print(f'Euler angles in XYZ {r_euler} degs') + isHorizontal = np.absolute(T[0]) > np.absolute(T[1]) + + R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, - imsize, R, T) + imsize, R, T) # , alpha=0.1 + + r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) + print(f'R_L Euler angles in XYZ {r_euler}') + r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) + print(f'R_R Euler angles in XYZ {r_euler}') - return [ret, R, T, R_l, R_r] + return [ret, R, T, R_l, R_r, P_l, P_r] - def display_rectification(self, image_data_pairs, isHorizontal): + def display_rectification(self, image_data_pairs, images_corners_l, images_corners_r, image_epipolar_color, isHorizontal): print( "Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") - for image_data_pair in image_data_pairs: + colors = [(0, 255 , 0), (0, 0, 255)] + for idx, image_data_pair in enumerate(image_data_pairs): if isHorizontal: img_concat = cv2.hconcat( [image_data_pair[0], image_data_pair[1]]) + for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]): + cv2.line(img_concat, + (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]) + image_data_pair[0].shape[1], int(right_pt[0][1])), + colors[colorMode], 1) else: img_concat = cv2.vconcat( [image_data_pair[0], image_data_pair[1]]) - - line_row = 0 - while isHorizontal and line_row < img_concat.shape[0]: - cv2.line(img_concat, - (0, line_row), (img_concat.shape[1], line_row), - (0, 255, 0), 1) - line_row += 30 - - line_col = 0 - while not isHorizontal and line_col < img_concat.shape[1]: - cv2.line(img_concat, - (line_col, 0), (line_col, img_concat.shape[0]), - (0, 255, 0), 1) - line_col += 40 + for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]): + cv2.line(img_concat, + (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]), int(right_pt[0][1]) + image_data_pair[0].shape[0]), + colors[colorMode], 1) img_concat = cv2.resize( - img_concat, (0, 0), fx=0.4, fy=0.4) + img_concat, (0, 0), fx=0.8, fy=0.8) # show image cv2.imshow('Stereo Pair', img_concat) @@ -663,173 +574,8 @@ def display_rectification(self, image_data_pairs, isHorizontal): if k == 27: # Esc key to stop break - # os._exit(0) - # raise SystemExit() - cv2.destroyWindow('Stereo Pair') - def scale_image(self, img, scaled_res): - expected_height = img.shape[0]*(scaled_res[1]/img.shape[1]) - # print("Expected Height: {}".format(expected_height)) - - if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]): - if int(expected_height) == scaled_res[0]: - # resizing to have both stereo and rgb to have same - # resolution to capture extrinsics of the rgb-right camera - img = cv2.resize(img, (scaled_res[1], scaled_res[0]), - interpolation=cv2.INTER_CUBIC) - return img - else: - # resizing and cropping to have both stereo and rgb to have same resolution - # to calculate extrinsics of the rgb-right camera - scale_width = scaled_res[1]/img.shape[1] - dest_res = ( - int(img.shape[1] * scale_width), int(img.shape[0] * scale_width)) - img = cv2.resize( - img, dest_res, interpolation=cv2.INTER_CUBIC) - if img.shape[0] < scaled_res[0]: - raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format( - img.shape[0], scaled_res[0])) - # print(gray.shape[0] - req_resolution[0]) - del_height = (img.shape[0] - scaled_res[0]) // 2 - # gray = gray[: req_resolution[0], :] - img = img[del_height: del_height + scaled_res[0], :] - return img - else: - return img - - def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal): - mapx_l, mapy_l = cv2.initUndistortRectifyMap( - M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1) - mapx_r, mapy_r = cv2.initUndistortRectifyMap( - M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1) - - - image_data_pairs = [] - imagesCount = 0 - # print(len(images_left)) - # print(len(images_right)) - for image_left, image_right in zip(images_left, images_right): - # read images - imagesCount += 1 - # print(imagesCount) - img_l = cv2.imread(image_left, 0) - img_r = cv2.imread(image_right, 0) - - img_l = self.scale_image(img_l, scaled_res) - img_r = self.scale_image(img_r, scaled_res) - # print(img_l.shape) - # print(img_r.shape) - - # warp right image - # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR) - img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR) - - image_data_pairs.append((img_l, img_r)) - # print(f'Images data pair size ios {len(image_data_pairs)}') - imgpoints_r = [] - imgpoints_l = [] - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) - - for i, image_data_pair in enumerate(image_data_pairs): - marker_corners_l, ids_l, rejectedImgPoints = cv2.aruco.detectMarkers( - image_data_pair[0], self.aruco_dictionary) - marker_corners_l, ids_l, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[0], self.board, - marker_corners_l, ids_l, - rejectedCorners=rejectedImgPoints) - - marker_corners_r, ids_r, rejectedImgPoints = cv2.aruco.detectMarkers( - image_data_pair[1], self.aruco_dictionary) - marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, - marker_corners_r, ids_r, - rejectedCorners=rejectedImgPoints) - - res2_l = cv2.aruco.interpolateCornersCharuco( - marker_corners_l, ids_l, image_data_pair[0], self.board) - res2_r = cv2.aruco.interpolateCornersCharuco( - marker_corners_r, ids_r, image_data_pair[1], self.board) - - # img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) - # img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - # line_row = 0 - # while line_row < img_concat.shape[0]: - # cv2.line(img_concat, - # (0, line_row), (img_concat.shape[1], line_row), - # (0, 255, 0), 1) - # line_row += 30 - - # cv2.imshow('Stereo Pair', img_concat) - # k = cv2.waitKey(0) - # if k == 27: # Esc key to stop - # break - - if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3: - - cv2.cornerSubPix(image_data_pair[0], res2_l[1], - winSize=(5, 5), - zeroZone=(-1, -1), - criteria=criteria) - cv2.cornerSubPix(image_data_pair[1], res2_r[1], - winSize=(5, 5), - zeroZone=(-1, -1), - criteria=criteria) - - # termination criteria - img_pth_right = Path(images_right[i]) - img_pth_left = Path(images_left[i]) - org = (100, 50) - # cv2.imshow('ltext', lText) - # cv2.waitKey(0) - localError = 0 - corners_l = [] - corners_r = [] - for j in range(len(res2_l[2])): - idx = np.where(res2_r[2] == res2_l[2][j]) - if idx[0].size == 0: - continue - corners_l.append(res2_l[1][j]) - corners_r.append(res2_r[1][idx]) - - imgpoints_l.extend(corners_l) - imgpoints_r.extend(corners_r) - epi_error_sum = 0 - for l_pt, r_pt in zip(corners_l, corners_r): - if isHorizontal: - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - else: - epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) - # localError = epi_error_sum / len(corners_l) - - # print("Average Epipolar in test Error per image on host in " + img_pth_right.name + " : " + - # str(localError)) - else: - print('Numer of corners is in left -> {} and right -> {}'.format( - len(marker_corners_l), len(marker_corners_r))) - raise SystemExit(1) - - epi_error_sum = 0 - for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): - if isHorizontal: - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - else: - epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) - - avg_epipolar = epi_error_sum / len(imgpoints_r) - print("Average Epipolar Error in test is : " + str(avg_epipolar)) - return avg_epipolar - - def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r, p_l, p_r): images_left = glob.glob(left_img_pth + '/*.png') images_right = glob.glob(right_img_pth + '/*.png') @@ -837,43 +583,14 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, images_right.sort() assert len(images_left) != 0, "ERROR: Images not read correctly" assert len(images_right) != 0, "ERROR: Images not read correctly" - isHorizontal = True - if np.absolute(t[1]) > 0.6: - isHorizontal = False - - scale = None - scale_req = False - frame_left_shape = cv2.imread(images_left[0], 0).shape - frame_right_shape = cv2.imread(images_right[0], 0).shape - scalable_res = frame_left_shape - scaled_res = frame_right_shape - if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]: - scale_req = True - scale = frame_right_shape[1] / frame_left_shape[1] - elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]: - scale_req = True - scale = frame_left_shape[1] / frame_right_shape[1] - scalable_res = frame_right_shape - scaled_res = frame_left_shape - - if scale_req: - scaled_height = scale * scalable_res[0] - diff = scaled_height - scaled_res[0] - # if scaled_height < smaller_res[0]: - if diff < 0: - scaled_res = (int(scaled_height), scaled_res[1]) - - print( - f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}') - print("Original res Left :{}".format(frame_left_shape)) - print("Original res Right :{}".format(frame_right_shape)) - # print("Scale res :{}".format(scaled_res)) + isHorizontal = np.absolute(t[0]) > np.absolute(t[1]) - M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) - M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) + frame_left_shape = cv2.imread(images_left[0], 0).shape[:2][::-1] # (w,h) + frame_right_shape = cv2.imread(images_right[0], 0).shape[:2][::-1] + scaler = ImageScaler(frame_left_shape, frame_right_shape) - # p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) - # p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) + M_lp, M_rp = scaler.transform_intrinsics(M_l, M_r) + scaled_res = scaler.target_size[::-1] # (h, w) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) @@ -890,21 +607,20 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # print(d_r) # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_l, d_l, scaled_res[::-1], 0) # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) - kScaledL = M_rp - kScaledR = kScaledL - # print('Intrinsics from the getOptimalNewCameraMatrix....') - # print(kScaledL) - # print(kScaledR) - # oldEpipolarError = None - # epQueue = deque() - # movePos = True - - r_id = np.identity(3) - mapx_l, mapy_l = cv2.initUndistortRectifyMap( - M_lp, d_l, r_l, M_rp, scaled_res[::-1], cv2.CV_32FC1) - mapx_r, mapy_r = cv2.initUndistortRectifyMap( - M_rp, d_r, r_r, M_rp, scaled_res[::-1], cv2.CV_32FC1) - + kScaledR = kScaledL = M_rp + + print('Lets find the best epipolar Error') + + if self.cameraModel == 'perspective': + mapx_l, mapy_l = cv2.initUndistortRectifyMap( + M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1) + mapx_r, mapy_r = cv2.initUndistortRectifyMap( + M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1) + else: + mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap( + M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1) + mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap( + M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1) image_data_pairs = [] for image_left, image_right in zip(images_left, images_right): @@ -912,8 +628,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, img_l = cv2.imread(image_left, 0) img_r = cv2.imread(image_right, 0) - img_l = self.scale_image(img_l, scaled_res) - img_r = self.scale_image(img_r, scaled_res) + img_l, img_r = scaler.transform_img(img_l, img_r) # print(img_l.shape) # print(img_r.shape) @@ -933,7 +648,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, image_data_pairs.append((img_l, img_r)) - if traceLevel == 3: + if self.traceLevel == 4: cv2.imshow("undistorted-Left", img_l) cv2.imshow("undistorted-right", img_r) # print(f'image path - {im}') @@ -941,12 +656,13 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, k = cv2.waitKey(0) if k == 27: # Esc key to stop break - if traceLevel == 3: + if self.traceLevel == 4: cv2.destroyWindow("undistorted-left") cv2.destroyWindow("undistorted-right") # compute metrics imgpoints_r = [] imgpoints_l = [] + image_epipolar_color = [] # new_imagePairs = [] no_markers_found_error_count = 0 for i, image_data_pair in enumerate(image_data_pairs): @@ -1019,15 +735,22 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, corners_l.append(res2_l[1][j]) corners_r.append(res2_r[1][idx]) - imgpoints_l.extend(corners_l) - imgpoints_r.extend(corners_r) + imgpoints_l.append(corners_l) # append or extend? + imgpoints_r.append(corners_r) epi_error_sum = 0 + corner_epipolar_color = [] for l_pt, r_pt in zip(corners_l, corners_r): if isHorizontal: - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + curr_epipolar_error = abs(l_pt[0][1] - r_pt[0][1]) + else: + curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0]) + if curr_epipolar_error >= 1: + corner_epipolar_color.append(1) else: - epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) + corner_epipolar_color.append(0) + epi_error_sum += curr_epipolar_error localError = epi_error_sum / len(corners_l) + image_epipolar_color.append(corner_epipolar_color) print("Average Epipolar Error per image on host in " + img_pth_right.name + " : " + str(localError)) @@ -1041,17 +764,20 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, epi_error_sum = 0 - for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): - if isHorizontal: - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - else: - epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) + total_corners = 0 + for corners_l, corners_r in zip(imgpoints_l, imgpoints_r): + total_corners += len(corners_l) + for l_pt, r_pt in zip(corners_l, corners_r): + if isHorizontal: + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + else: + epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) - avg_epipolar = epi_error_sum / len(imgpoints_r) + avg_epipolar = epi_error_sum / total_corners print("Average Epipolar Error is : " + str(avg_epipolar)) if self.enable_rectification_disp: - self.display_rectification(image_data_pairs, isHorizontal) + self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal) return avg_epipolar @@ -1061,10 +787,16 @@ def create_save_mesh(self): # , output_path): print("Mesh path") print(curr_path) - map_x_l, map_y_l = cv2.initUndistortRectifyMap( - self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1) - map_x_r, map_y_r = cv2.initUndistortRectifyMap( - self.M2, self.d2, self.R2, self.M2, self.img_shape, cv2.CV_32FC1) + if self.cameraModel == "perspective": + map_x_l, map_y_l = cv2.initUndistortRectifyMap( + self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1) + map_x_r, map_y_r = cv2.initUndistortRectifyMap( + self.M2, self.d2, self.R2, self.M2, self.img_shape, cv2.CV_32FC1) + else: + map_x_l, map_y_l = cv2.fisheye.initUndistortRectifyMap( + self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1) + map_x_r, map_y_r = cv2.fisheye.initUndistortRectifyMap( + self.M2, self.d2, self.R2, self.M2, self.img_shape, cv2.CV_32FC1) """ map_x_l_fp32 = map_x_l.astype(np.float32) diff --git a/depthai_helpers/image_scaler.py b/depthai_helpers/image_scaler.py new file mode 100644 index 000000000..b45e05afc --- /dev/null +++ b/depthai_helpers/image_scaler.py @@ -0,0 +1,109 @@ +from typing import List, Dict, Optional, Tuple +import numpy as np +import cv2 + +class ImageScaler: + scale_a: float = 1 + scale_b: float = 1 + translate_y_a: float = 0 + translate_y_b: float = 0 + + def __init__(self, size_a: Tuple[int, int], size_b: Tuple[int, int]): + """ + Given the sizes of two images, calculate the scale and translation values to make them the same size. + + The size of the images must be given as (width, height). + """ + + self.size_a = size_a + self.size_b = size_b + + w_a, h_a = size_a + w_b, h_b = size_b + + # calculate the scale factors + if w_a > w_b: + self.scale_a = w_b / w_a + self.scale_size_a = (round(w_a * self.scale_a), round(h_a * self.scale_a)) + self.scale_b = 1 + self.scale_size_b = (w_b, h_b) + self.scale_a = self.scale_size_a[0] / w_a # recalculate the scale factor to avoid rounding errors + else: + self.scale_b = w_a / w_b + self.scale_size_b = (round(w_b * self.scale_b), round(h_b * self.scale_b)) + self.scale_a = 1 + self.scale_size_a = (w_a, h_a) + self.scale_b = self.scale_size_b[0] / w_b # recalculate the scale factor to avoid rounding errors + + + assert self.scale_size_a[0] == self.scale_size_b[0], "The width of the images should be the same after scaling." + + # calculate the translation values + if self.scale_size_a[1] > self.scale_size_b[1]: + self.target_size = self.scale_size_b + self.translate_y_a = -((self.scale_size_a[1] - self.scale_size_b[1]) // 2) + self.translate_y_b = 0 + else: + self.target_size = self.scale_size_a + self.translate_y_b = -((self.scale_size_b[1] - self.scale_size_a[1]) // 2) + self.translate_y_a = 0 + + def transform_img_a(self, img_a: np.ndarray): + new_img_a = cv2.resize(img_a.copy(), self.scale_size_a, interpolation=cv2.INTER_CUBIC) + crop_a = -self.translate_y_a + if crop_a != 0: + new_img_a = new_img_a[crop_a:self.target_size[1]+crop_a] + + return new_img_a + + + def transform_img_b(self, img_b: np.ndarray): + new_img_b = cv2.resize(img_b.copy(), self.scale_size_b, interpolation=cv2.INTER_CUBIC) + crop_b = -self.translate_y_b + if crop_b != 0: + new_img_b = new_img_b[crop_b:self.target_size[1]+crop_b] + + return new_img_b + + def transform_img(self, img_a: np.ndarray, img_b: np.ndarray): + """ + Scale and translate the images to make them the same size. + """ + new_img_a = self.transform_img_a(img_a) + new_img_b = self.transform_img_b(img_b) + + return new_img_a, new_img_b + + def transform_points(self, points_a: np.ndarray, points_b: np.ndarray): + """ + Scale and translate the points to align them with the scaled and cropped images. The points are expected to be in the format [[x,y], [x,y], ...]. + """ + points_a = points_a.copy() + points_b = points_b.copy() + + points_a[:, 0] *= self.scale_a + points_a[:, 1] *= self.scale_a + points_a[:, 1] += self.translate_y_a + + points_b[:, 0] *= self.scale_b + points_b[:, 1] *= self.scale_b + points_b[:, 1] += self.translate_y_b + + return points_a, points_b + + def transform_intrinsics(self, intrinsics_a: np.ndarray, intrinsics_b: np.ndarray): + """ + Scale the intrinsics to align them with the scaled images. + """ + intrinsics_a = intrinsics_a.copy() + intrinsics_b = intrinsics_b.copy() + + intrinsics_a[0, :] *= self.scale_a + intrinsics_a[1, :] *= self.scale_a + intrinsics_a[1, 2] += self.translate_y_a + + intrinsics_b[0, :] *= self.scale_b + intrinsics_b[1, :] *= self.scale_b + intrinsics_b[1, 2] += self.translate_y_b + + return intrinsics_a, intrinsics_b \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index a5133b39c..2c10834d3 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,4 @@ -requests==2.26.0 +requests==2.31.0 --extra-index-url https://www.piwheels.org/simple numpy>=1.21.4 # For RPi Buster (last successful build) and macOS M1 (first build). But allow for higher versions, to support Python3.11 (not available in 1.21.4 yet) opencv-contrib-python==4.5.5.62 # Last successful RPi build, also covers M1 with above pinned numpy (otherwise 4.6.0.62 would be required, but that has a bug with charuco boards). Python version not important, abi3 wheels diff --git a/resources/boards/VERMEER_NO_RGB.json b/resources/boards/VERMEER_NO_RGB.json new file mode 100644 index 000000000..5c2cb3bd6 --- /dev/null +++ b/resources/boards/VERMEER_NO_RGB.json @@ -0,0 +1,37 @@ +{ + "board_config": + { + "name": "VERMEER", + "revision": "R0M0E0", + "camera_model": "fisheye", + "cameras":{ + "CAM_C": { + "name": "right", + "hfov": 71.86, + "type": "color", + "extrinsics": { + "to_cam": "CAM_B", + "specTranslation": { + "x": -10, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "color" + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} \ No newline at end of file diff --git a/resources/boards/VISION-no-night.json b/resources/boards/VISION-no-night.json new file mode 100644 index 000000000..bc5a95548 --- /dev/null +++ b/resources/boards/VISION-no-night.json @@ -0,0 +1,54 @@ +{ + "board_config": + { + "name": "FLASH", + "revision": "R1M1E1", + "cameras":{ + "CAM_D": { + "name": "rgb", + "hfov": 68, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -7.5, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_C": { + "name": "right", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_D", + "specTranslation": { + "x": 5.5, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} \ No newline at end of file diff --git a/resources/boards/VISION.json b/resources/boards/VISION.json new file mode 100644 index 000000000..840e0ebe6 --- /dev/null +++ b/resources/boards/VISION.json @@ -0,0 +1,72 @@ +{ + "board_config": + { + "name": "FLASH", + "revision": "R1M1E1", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -7.5, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_C": { + "name": "right", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 5.5, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_D": { + "name": "rgb", + "hfov": 68, + "type": "color", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 2, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} \ No newline at end of file diff --git a/test.json b/test.json new file mode 100644 index 000000000..420b3a637 --- /dev/null +++ b/test.json @@ -0,0 +1,302 @@ +{ + "batchName": "", + "batchTime": 1682604710, + "boardConf": "IR-C00M00-00", + "boardCustom": "", + "boardName": "BK2091", + "boardOptions": 3, + "boardRev": "R0M0E0", + "cameraData": [ + [ + 1, + { + "cameraType": 0, + "distortionCoeff": [ + 0.329445481300354, + 0.049340587109327316, + -0.00014132818614598364, + -5.1860530220437795e-05, + 0.007958278991281986, + 0.6764867305755615, + 0.08518566191196442, + 0.025907307863235474, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "extrinsics": { + "rotationMatrix": [ + [ + 0.993652880191803, + 0.00040501737385056913, + 0.11248921602964401 + ], + [ + -0.016856124624609947, + 0.9892390966415405, + 0.14533373713493347 + ], + [ + -0.1112198680639267, + -0.14630740880966187, + 0.9829670786857605 + ] + ], + "specTranslation": { + "x": -6.25, + "y": 0.0, + "z": 0.0 + }, + "toCameraSocket": 2, + "translation": { + "x": 8.195008277893066, + "y": 3.712445020675659, + "z": -2.221177101135254 + } + }, + "height": 800, + "intrinsicMatrix": [ + [ + 588.1680908203125, + 0.0, + 613.21484375 + ], + [ + 0.0, + 588.1680908203125, + 369.7418518066406 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "lensPosition": 0, + "specHfovDeg": 139.1999969482422, + "width": 1280 + } + ], + [ + 0, + { + "cameraType": 0, + "distortionCoeff": [ + 0.5175411105155945, + 0.010855120606720448, + 0.00011568265472305939, + -0.00016107785631902516, + -0.0005671291728504002, + 0.8691806793212891, + 0.10366542637348175, + -0.0024669996928423643, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "extrinsics": { + "rotationMatrix": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ], + "specTranslation": { + "x": -0.0, + "y": -0.0, + "z": -0.0 + }, + "toCameraSocket": -1, + "translation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "height": 1080, + "intrinsicMatrix": [ + [ + 512.986572265625, + 0.0, + 721.78173828125 + ], + [ + 0.0, + 512.986572265625, + 560.6287841796875 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "lensPosition": 0, + "specHfovDeg": 139.1999969482422, + "width": 1440 + } + ], + [ + 2, + { + "cameraType": 0, + "distortionCoeff": [ + 0.035288888961076736, + 0.006737724877893925, + 0.009381605312228203, + 0.0038988629821687937, + 0.0030439619440585375, + 0.38551080226898193, + -0.03806420415639877, + 0.012981339357793331, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "extrinsics": { + "rotationMatrix": [ + [ + 0.9946601390838623, + -0.01590065285563469, + -0.10197263956069946 + ], + [ + 0.0009700041846372187, + 0.989456832408905, + -0.14482498168945313 + ], + [ + 0.10320033878087997, + 0.14395271241664886, + 0.9841886758804321 + ] + ], + "specTranslation": { + "x": 3.125, + "y": 0.0, + "z": 0.0 + }, + "toCameraSocket": 0, + "translation": { + "x": -5.660194396972656, + "y": -4.021817684173584, + "z": 0.7852433323860168 + } + }, + "height": 800, + "intrinsicMatrix": [ + [ + 576.2265014648438, + 0.0, + 584.5711059570313 + ], + [ + 0.0, + 576.2265014648438, + 278.7242736816406 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "lensPosition": 0, + "specHfovDeg": 139.1999969482422, + "width": 1280 + } + ] + ], + "hardwareConf": "F1-FV00-BC001", + "imuExtrinsics": { + "rotationMatrix": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ], + "specTranslation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "toCameraSocket": -1, + "translation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "miscellaneousData": [], + "productName": "OAK-D-EVOLV", + "stereoRectificationData": { + "leftCameraSocket": 1, + "rectifiedRotationLeft": [ + [ + 0.8986309170722961, + 0.43173250555992126, + -0.07790699601173401 + ], + [ + -0.4262399971485138, + 0.9012516736984253, + 0.07787758857011795 + ], + [ + 0.10383609682321548, + -0.03677612915635109, + 0.9939142465591431 + ] + ], + "rectifiedRotationRight": [ + [ + 0.88433837890625, + 0.40061673521995544, + -0.23969128727912903 + ], + [ + -0.41440916061401367, + 0.9100563526153564, + -0.00790234375745058 + ], + [ + 0.2149667739868164, + 0.10631861537694931, + 0.970816969871521 + ] + ], + "rightCameraSocket": 2 + }, + "version": 7 +}