From 186abaea06211de1cb7caf40367c8b08c0e3d242 Mon Sep 17 00:00:00 2001 From: Sachin Date: Tue, 9 Aug 2022 10:17:51 -0700 Subject: [PATCH 01/68] Added WIP Image Align for visualization --- calibrate.py | 619 +++++++++--- depthai_helpers/calibration_utils.py | 1098 +++++++++------------- depthai_helpers/calibration_utils_old.py | 1020 ++++++++++++++++++++ requirements.txt | 3 +- resources/boards/ACME01.json | 13 - resources/boards/BW1097.json | 13 - resources/boards/BW1098OBC.json | 53 +- resources/boards/DM2097.json | 12 - resources/boards/OAK-D-CM4-POE.json | 12 - resources/boards/OAK-D-CM4.json | 13 - resources/boards/OAK-D-IoT-40.json | 12 - resources/boards/OAK-D-LITE.json | 12 - resources/boards/OAK-D-PRO-W.json | 13 - resources/boards/OAK-D-PRO.json | 13 - resources/boards/OAK-D-W.json | 13 - 15 files changed, 1986 insertions(+), 933 deletions(-) create mode 100644 depthai_helpers/calibration_utils_old.py delete mode 100644 resources/boards/ACME01.json delete mode 100644 resources/boards/BW1097.json delete mode 100644 resources/boards/DM2097.json delete mode 100644 resources/boards/OAK-D-CM4-POE.json delete mode 100644 resources/boards/OAK-D-CM4.json delete mode 100644 resources/boards/OAK-D-IoT-40.json delete mode 100644 resources/boards/OAK-D-LITE.json delete mode 100644 resources/boards/OAK-D-PRO-W.json delete mode 100644 resources/boards/OAK-D-PRO.json delete mode 100644 resources/boards/OAK-D-W.json diff --git a/calibrate.py b/calibrate.py index 8e735eeab..83fe9e867 100755 --- a/calibrate.py +++ b/calibrate.py @@ -6,18 +6,70 @@ from argparse import ArgumentParser from pathlib import Path import time +from datetime import datetime, timedelta import cv2 +from cv2 import resize import depthai as dai import numpy as np -import depthai_helpers.calibration_utils as calibUtils +import depthai_helpers.calibration_utils_old as calibUtils font = cv2.FONT_HERSHEY_SIMPLEX debug = False red = (255, 0, 0) green = (0, 255, 0) +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.CAM_C, + '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.RGB : 'RGB' , + # dai.CameraBoardSocket.LEFT : 'LEFT' , + # dai.CameraBoardSocket.RIGHT : 'RIGHT', + # dai.CameraBoardSocket.AUTO : 'AUTO' + 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' + } + +camToMonoRes = { + 'OV7251' : dai.MonoCameraProperties.SensorResolution.THE_480_P, + 'OV9*82' : dai.MonoCameraProperties.SensorResolution.THE_800_P, + } + +camToRgbRes = { + 'IMX378' : dai.ColorCameraProperties.SensorResolution.THE_4_K, + 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, + 'OV9*82' : dai.ColorCameraProperties.SensorResolution.THE_800_P, + 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP + } def create_blank(width, height, rgb_color=(0, 0, 0)): """Create new image(numpy array) filled with certain color in RGB""" @@ -87,8 +139,8 @@ def parse_args(): help="Invert vertical axis of the camera for the display") parser.add_argument("-ih", "--invertHorizontal", dest="invert_h", default=False, action="store_true", 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("-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("-rlp", "--rgbLensPosition", default=135, type=int, @@ -114,6 +166,54 @@ def parse_args(): return options +class HostSync: + def __init__(self, deltaMilliSec): + self.arrays = {} + # self.arraySize = arraySize + self.recentFrameTs = None + self.deltaMilliSec = timedelta(milliseconds=deltaMilliSec) + # self.synced = queue.Queue() + + def remove(self, t1): + return timedelta(milliseconds=500) < (self.recentFrameTs - t1) + + def add_msg(self, name, data, ts): + if name not in self.arrays: + self.arrays[name] = [] + # Add msg to array + self.arrays[name].append({'data': data, 'timestamp': ts}) + if self.recentFrameTs == None or self.recentFrameTs < ts: + self.recentFrameTs = ts + + for name, arr in self.arrays.items(): + for i, obj in enumerate(arr): + if self.remove(obj['timestamp']): + arr.remove(obj) + else: break + + + def get_synced(self): + synced = {} + + for name, arr in self.arrays.items(): + for i, obj in enumerate(arr): + time_diff = abs(obj['timestamp'] - self.recentFrameTs) + print("Time diff for {0} is {1} milliseconds".format(name ,time_diff.total_seconds() * 1000)) + # 20ms since we add rgb/depth frames at 30FPS => 33ms. If + # time difference is below 20ms, it's considered as synced + if time_diff < self.deltaMilliSec: + synced[name] = obj['data'] + # print(f"{name}: {i}/{len(arr)}") + break + print(f'Size of Synced is {len(synced)} amd array size is {len(self.arrays)}') + if len(synced) == len(self.arrays): + for name, arr in self.arrays.items(): + for i, obj in enumerate(arr): + if self.remove(obj['timestamp']): + arr.remove(obj) + else: break + return synced + return False class Main: output_scale_factor = 0.5 @@ -140,6 +240,9 @@ def __init__(self): 'Board config not found: {}'.format(board_path)) with open(board_path) as fp: self.board_config = json.load(fp) + self.board_config = self.board_config['board_config'] + self.board_config_backup = self.board_config + # TODO: set the total images # random polygons for count self.total_images = self.args.count * \ @@ -147,11 +250,30 @@ def __init__(self): if debug: print("Using Arguments=", self.args) - if self.args.board.upper() == 'OAK-D-LITE': - raise Exception( - "OAK-D-Lite Calibration is not supported on main yet. Please use `lite_calibration` branch to calibrate your OAK-D-Lite!!") + # if self.args.board.upper() == 'OAK-D-LITE': + # raise Exception( + # "OAK-D-Lite Calibration is not supported on main yet. Please use `lite_calibration` branch to calibrate your OAK-D-Lite!!") + + self.device = dai.Device() + cameraProperties = self.device.getConnectedCameraProperties() + for properties in cameraProperties: + for in_cam in self.board_config['cameras'].keys(): + cam_info = self.board_config['cameras'][in_cam] + if properties.socket == stringToCam[in_cam]: + self.board_config['cameras'][in_cam]['sensorName'] = properties.sensorName + print('Cam: {} and focus: {}'.format(cam_info['name'], properties.hasAutofocus)) + self.board_config['cameras'][in_cam]['hasAutofocus'] = properties.hasAutofocus + # self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check() + break + pipeline = self.create_pipeline() - self.device = dai.Device(pipeline) + self.device.startPipeline(pipeline) + + self.camera_queue = {} + for config_cam in self.board_config['cameras']: + cam = self.board_config['cameras'][config_cam] + self.camera_queue[cam['name']] = self.device.getOutputQueue(cam['name'], 1, False) + """ cameraProperties = self.device.getConnectedCameraProperties() for properties in cameraProperties: if properties.sensorName == 'OV7251': @@ -159,10 +281,10 @@ def __init__(self): "OAK-D-Lite Calibration is not supported on main yet. Please use `lite_calibration` branch to calibrate your OAK-D-Lite!!") self.device.startPipeline(pipeline)""" - self.left_camera_queue = self.device.getOutputQueue("left", 30, True) - self.right_camera_queue = self.device.getOutputQueue("right", 30, True) - if not self.args.disableRgb: - self.rgb_camera_queue = self.device.getOutputQueue("rgb", 30, True) + # self.left_camera_queue = self.device.getOutputQueue("left", 30, True) + # self.right_camera_queue = self.device.getOutputQueue("right", 30, True) + # if not self.args.disableRgb: + # self.rgb_camera_queue = self.device.getOutputQueue("rgb", 30, True) def is_markers_found(self, frame): marker_corners, _, _ = cv2.aruco.detectMarkers( @@ -185,53 +307,50 @@ def test_camera_orientation(self, frame_l, frame_r): if left_corner[0][0] - right_corner[0][0] < 0: return False return True - + def create_pipeline(self): pipeline = dai.Pipeline() - cam_left = pipeline.createMonoCamera() - cam_right = pipeline.createMonoCamera() + fps = 10 + for cam_id in self.board_config['cameras']: + cam_info = self.board_config['cameras'][cam_id] + if cam_info['type'] == 'mono': + cam_node = pipeline.createMonoCamera() + xout = pipeline.createXLinkOut() - xout_left = pipeline.createXLinkOut() - xout_right = pipeline.createXLinkOut() + cam_node.setBoardSocket(stringToCam[cam_id]) + cam_node.setResolution(camToMonoRes[cam_info['sensorName']]) + cam_node.setFps(fps) - if self.args.swapLR: - cam_left.setBoardSocket(dai.CameraBoardSocket.RIGHT) - cam_right.setBoardSocket(dai.CameraBoardSocket.LEFT) - else: - cam_left.setBoardSocket(dai.CameraBoardSocket.LEFT) - cam_right.setBoardSocket(dai.CameraBoardSocket.RIGHT) + xout.setStreamName(cam_info['name']) + cam_node.out.link(xout.input) + else: + cam_node = pipeline.createColorCamera() + xout = pipeline.createXLinkOut() - cam_left.setResolution( - dai.MonoCameraProperties.SensorResolution.THE_800_P) - cam_left.setFps(self.args.fps) - - cam_right.setResolution( - dai.MonoCameraProperties.SensorResolution.THE_800_P) - cam_right.setFps(self.args.fps) - - xout_left.setStreamName("left") - cam_left.out.link(xout_left.input) - - xout_right.setStreamName("right") - cam_right.out.link(xout_right.input) - - if not self.args.disableRgb: - rgb_cam = pipeline.createColorCamera() - rgb_cam.setResolution( - dai.ColorCameraProperties.SensorResolution.THE_4_K) - rgb_cam.setInterleaved(False) - rgb_cam.setBoardSocket(dai.CameraBoardSocket.RGB) - rgb_cam.setIspScale(1, 3) - rgb_cam.initialControl.setManualFocus(self.focus_value) - rgb_cam.setFps(self.args.fps) - - xout_rgb_isp = pipeline.createXLinkOut() - xout_rgb_isp.setStreamName("rgb") - rgb_cam.isp.link(xout_rgb_isp.input) + cam_node.setBoardSocket(stringToCam[cam_id]) + cam_node.setResolution(camToRgbRes[cam_info['sensorName']]) + cam_node.setFps(fps) + + xout.setStreamName(cam_info['name']) + cam_node.isp.link(xout.input) + if cam_info['sensorName'] == "OV9*82": + cam_node.initialControl.setSharpness(0) + cam_node.initialControl.setLumaDenoise(0) + cam_node.initialControl.setChromaDenoise(4) + + if cam_info['hasAutofocus']: + cam_node.initialControl.setManualFocus(self.focus_value) + + controlIn = pipeline.createXLinkIn() + controlIn.setStreamName(cam_info['name'] + '-control') + controlIn.out.link(cam_node.inputControl) + xout.input.setBlocking(False) + xout.input.setQueueSize(1) return pipeline + def parse_frame(self, frame, stream_name): if not self.is_markers_found(frame): return False @@ -319,6 +438,173 @@ def empty_calibration(self, calib: dai.CalibrationHandler): for attr in ["boardName", "boardRev"]: if getattr(data, attr): return False return True + + def capture_images_sync(self): + finished = False + capturing = False + start_timer = False + timer = self.args.captureDelay + prev_time = None + curr_time = None + + self.display_name = "Image Window" + syncCollector = HostSync(20) + while not finished: + currImageList = {} + for key in self.camera_queue.keys(): + frameMsg = self.camera_queue[key].get() + + print(f'key is {key}') + syncCollector.add_msg(key, frameMsg, frameMsg.getTimestamp()) + gray_frame = None + if frameMsg.getType() == dai.RawImgFrame.Type.RAW8: + gray_frame = frameMsg.getCvFrame() + else: + gray_frame = cv2.cvtColor(frameMsg.getCvFrame(), cv2.COLOR_BGR2GRAY) + currImageList[key] = gray_frame + print(gray_frame.shape) + + resizeHeight = 0 + resizeWidth = 0 + for name, imgFrame in currImageList.items(): + + print(f'original Shape of {name} is {imgFrame.shape}' ) + currImageList[name] = cv2.resize( + imgFrame, (0, 0), fx=self.output_scale_factor, fy=self.output_scale_factor) + + height, width = currImageList[name].shape + + print(f'resized Shape of {name} is {width}x{height}' ) + + widthRatio = resizeWidth / width + heightRatio = resizeHeight / height + + + # if widthRatio > 1.0 and heightRatio > 1.0 and widthRatio < 1.2 and heightRatio < 1.2: + # continue + + + if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0): + resizeWidth = width + resizeHeight = height + # elif widthRatio > 1.2 and heightRatio > 1.2: + + + # if width < resizeWidth: + # resizeWidth = width + # if height > resizeHeight: + # resizeHeight = height + + print(f'Scale Shape is {resizeWidth}x{resizeHeight}' ) + + combinedImage = None + for name, imgFrame in currImageList.items(): + height, width = imgFrame.shape + if width > resizeWidth and height > resizeHeight: + imgFrame = cv2.resize( + imgFrame, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width) + + print(f'final_scaledImageSize is {imgFrame.shape}') + if self.polygons is None: + self.height, self.width = imgFrame.shape + print(self.height, self.width) + self.polygons = calibUtils.setPolygonCoordinates( + self.height, self.width) + + cv2.polylines( + imgFrame, np.array([self.polygons[self.current_polygon]]), + True, (0, 0, 255), 4) + + # TODO(Sachin): Add this back with proper alignment + # cv2.putText( + # imgFrame, + # "Polygon Position: {}. Captured {} of {} {} images".format( + # self.current_polygon + 1, self.images_captured, self.total_images, name), + # (0, 700), cv2.FONT_HERSHEY_TRIPLEX, 1.0, (255, 0, 0)) + + height, width = imgFrame.shape + height_offset = (resizeHeight - height)//2 + width_offset = (resizeWidth - width)//2 + subImage = np.pad(imgFrame, ((height_offset, height_offset), (width_offset,width_offset)), 'constant', constant_values=0) + if combinedImage is None: + combinedImage = subImage + else: + combinedImage = np.hstack((combinedImage, subImage)) + + key = cv2.waitKey(1) + if key == 27 or key == ord("q"): + print("py: Calibration has been interrupted!") + raise SystemExit(0) + elif key == ord(" "): + start_timer = True + prev_time = time.time() + timer = self.args.captureDelay + + if start_timer == True: + curr_time = time.time() + if curr_time - prev_time >= 1: + prev_time = curr_time + timer = timer - 1 + if timer <= 0 and start_timer == True: + start_timer = False + capturing = True + print('Start capturing...') + + image_shape = combinedImage.shape + cv2.putText(combinedImage, str(timer), + (image_shape[1]//2, image_shape[0]//2), font, + 7, (0, 255, 255), + 4, cv2.LINE_AA) + + cv2.imshow(self.display_name, combinedImage) + tried = {} + allPassed = True + if capturing: + syncedMsgs = syncCollector.get_synced() + if syncedMsgs == False: + continue # + for name, frameMsg in syncedMsgs: + tried[name] = self.parse_frame(frameMsg.getCvFrame(), name) + allPassed = allPassed and tried[name] + + if allPassed: + if not self.images_captured: + leftStereo = self.board_config['cameras'][self.board_config['stereo_config']['left_cam']]['name'] + rightStereo = self.board_config['cameras'][self.board_config['stereo_config']['right_cam']]['name'] + print(f'Left Camera of stereo is {leftStereo} and right Camera of stereo is {rightStereo}') + if not self.test_camera_orientation(syncedMsgs[leftStereo].getCvFrame(), syncedMsgs[rightStereo].getCvFrame()): + self.show_failed_orientation() + + self.images_captured += 1 + self.images_captured_polygon += 1 + else: + self.show_failed_capture_frame() + + print(f'self.images_captured_polygon {self.images_captured_polygon}') + print(f'self.current_polygon {self.current_polygon}') + print(f'len(self.polygons) {len(self.polygons)}') + + if self.images_captured_polygon == self.args.count: + self.images_captured_polygon = 0 + self.current_polygon += 1 + + if self.current_polygon == len(self.polygons): + finished = True + cv2.destroyAllWindows() + break + + + + + + + + + + + + + def capture_images(self): finished = False @@ -513,129 +799,150 @@ def capture_images(self): def calibrate(self): print("Starting image processing") - cal_data = calibUtils.StereoCalibration() + stereo_calib = calibUtils.StereoCalibration() dest_path = str(Path('resources').absolute()) self.args.cameraMode = 'perspective' # hardcoded for now try: - epiploar_error, epiploar_error_rRgb, calibData = cal_data.calibrate(self.dataset_path, self.args.squareSizeCm, - self.args.markerSizeCm, self.args.squaresX, self.args.squaresY, self.args.cameraMode, not self.args.disableRgb, self.args.rectifiedDisp) - if epiploar_error > self.args.maxEpiploarError: - image = create_blank(900, 512, rgb_color=red) - text = "High L-r epiploar_error: " + str(epiploar_error) - cv2.putText(image, text, (10, 250), font, 2, (0, 0, 0), 2) - text = "Requires Recalibration " - cv2.putText(image, text, (10, 300), font, 2, (0, 0, 0), 2) - - cv2.imshow("Result Image", image) - cv2.waitKey(0) - print("Requires Recalibration.....!!") - raise SystemExit(1) - elif epiploar_error_rRgb is not None and epiploar_error_rRgb > self.args.maxEpiploarError: - image = create_blank(900, 512, rgb_color=red) - text = "High RGB-R epiploar_error: " + str(epiploar_error_rRgb) - cv2.putText(image, text, (10, 250), font, 2, (0, 0, 0), 2) - text = "Requires Recalibration " - cv2.putText(image, text, (10, 300), font, 2, (0, 0, 0), 2) - - cv2.imshow("Result Image", image) - cv2.waitKey(0) - print("Requires Recalibration.....!!") - raise SystemExit(1) - - left = dai.CameraBoardSocket.LEFT - right = dai.CameraBoardSocket.RIGHT - if self.args.swapLR: - left = dai.CameraBoardSocket.RIGHT - right = dai.CameraBoardSocket.LEFT - - calibration_handler = self.device.readCalibration() - - # calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) - # Set board name / revision only if calibration is empty - if self.empty_calibration(calibration_handler): - calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) - - calibration_handler.setCameraIntrinsics(left, calibData[2], 1280, 800) - calibration_handler.setCameraIntrinsics(right, calibData[3], 1280, 800) - measuredTranslation = [ - - self.board_config['board_config']['left_to_right_distance_cm'], 0.0, 0.0] - calibration_handler.setCameraExtrinsics( - left, right, calibData[5], calibData[6], measuredTranslation) - calibration_handler.setDistortionCoefficients(left, calibData[9] ) - calibration_handler.setDistortionCoefficients(right, calibData[10]) - - calibration_handler.setFov(left, self.board_config['board_config']['left_fov_deg']) - calibration_handler.setFov(right, self.board_config['board_config']['left_fov_deg']) - - calibration_handler.setStereoLeft( - left, calibData[0]) - calibration_handler.setStereoRight( - right, calibData[1]) - - if not self.args.disableRgb: - calibration_handler.setCameraIntrinsics(dai.CameraBoardSocket.RGB, calibData[4], 1920, 1080) - calibration_handler.setDistortionCoefficients(dai.CameraBoardSocket.RGB, calibData[11]) - calibration_handler.setFov(dai.CameraBoardSocket.RGB, self.board_config['board_config']['rgb_fov_deg']) - calibration_handler.setLensPosition(dai.CameraBoardSocket.RGB, self.focus_value) - - measuredTranslation = [ - self.board_config['board_config']['left_to_right_distance_cm'] - self.board_config['board_config']['left_to_rgb_distance_cm'], 0.0, 0.0] - calibration_handler.setCameraExtrinsics( - right, dai.CameraBoardSocket.RGB, calibData[7], calibData[8], measuredTranslation) - - resImage = None - if not self.device.isClosed(): + # stereo_calib = StereoCalibration() + status, result_config = stereo_calib.calibrate( + self.board_config, + self.dataset_path, + self.args.squareSizeCm, + self.args.markerSizeCm, + self.args.squaresX, + self.args.squaresY, + self.args.cameraMode, + self.args.rectifiedDisp) # Turn off enable disp rectify + calibration_handler = self.device.readCalibration2() + error_text = [] + + for camera in result_config['cameras'].keys(): + cam_info = result_config['cameras'][camera] + # log_list.append(self.ccm_selected[cam_info['name']]) + + color = green + reprojection_error_threshold = 1.0 + if cam_info['size'][1] > 720: + print(cam_info['size'][1]) + reprojection_error_threshold = reprojection_error_threshold * cam_info['size'][1] / 720 + + if cam_info['name'] == 'rgb': + reprojection_error_threshold = 6 + print('Reprojection error threshold -> {}'.format(reprojection_error_threshold)) + + if cam_info['reprojection_error'] > reprojection_error_threshold: + color = red + error_text.append("high Reprojection Error") + text = cam_info['name'] + ' Reprojection Error: ' + format(cam_info['reprojection_error'], '.6f') + # pygame_render_text(self.screen, text, (vis_x, vis_y), color, 30) + + 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 cam_info['hasAutofocus']: + calibration_handler.setLensPosition(stringToCam[camera], self.lensPosition[cam_info['name']]) + + # log_list.append(self.focusSigma[cam_info['name']]) + # log_list.append(cam_info['reprojection_error']) + # color = green/// + # epErrorZText + if 'extrinsics' in cam_info: + + if 'to_cam' in cam_info['extrinsics']: + right_cam = result_config['cameras'][cam_info['extrinsics']['to_cam']]['name'] + left_cam = cam_info['name'] + + epipolar_threshold = 0.6 + + if cam_info['extrinsics']['epipolar_error'] > epipolar_threshold: + color = red + error_text.append("high epipolar error between " + left_cam + " and " + right_cam) + elif cam_info['extrinsics']['epipolar_error'] == -1: + color = red + error_text.append("Epiploar validation failed between " + left_cam + " and " + right_cam) + + # log_list.append(cam_info['extrinsics']['epipolar_error']) + # text = left_cam + "-" + right_cam + ' Avg Epipolar error: ' + format(cam_info['extrinsics']['epipolar_error'], '.6f') + # pygame_render_text(self.screen, text, (vis_x, vis_y), color, 30) + # vis_y += 30 + specTranslation = np.array([cam_info['extrinsics']['specTranslation']['x'], cam_info['extrinsics']['specTranslation']['y'], cam_info['extrinsics']['specTranslation']['z']], dtype=np.float32) + + calibration_handler.setCameraExtrinsics(stringToCam[camera], stringToCam[cam_info['extrinsics']['to_cam']], cam_info['extrinsics']['rotation_matrix'], cam_info['extrinsics']['translation'], specTranslation) + if result_config['stereo_config']['left_cam'] == camera and result_config['stereo_config']['right_cam'] == cam_info['extrinsics']['to_cam']: + calibration_handler.setStereoLeft(stringToCam[camera], result_config['stereo_config']['rectification_left']) + calibration_handler.setStereoRight(stringToCam[cam_info['extrinsics']['to_cam']], result_config['stereo_config']['rectification_right']) + + + if len(error_text) == 0: + print('Flashing Calibration data into ') + print(calib_dest_path) + + eeepromData = calibration_handler.getEepromData() + print(f'EEPROM VERSION being flashed is -> {eeepromData.version}') + eeepromData.version = 7 + print(f'EEPROM VERSION being flashed is -> {eeepromData.version}') mx_serial_id = self.device.getDeviceInfo().getMxId() calib_dest_path = dest_path + '/' + mx_serial_id + '.json' calibration_handler.eepromToJsonFile(calib_dest_path) - is_write_succesful = False - try: - if self.args.factoryCalibration: - self.device.flashFactoryCalibration(calibration_handler) - is_write_succesful = self.device.flashCalibration( - calibration_handler) - except: + self.device.flashCalibration2(calibration_handler) + is_write_succesful = True + except RuntimeError: + is_write_succesful = False print("Writing in except...") + is_write_succesful = self.device.flashCalibration2(calibration_handler) - if self.args.factoryCalibration: + if self.args.factoryCalibration: + try: self.device.flashFactoryCalibration(calibration_handler) - is_write_succesful = self.device.flashCalibration( - calibration_handler) + is_write_factory_sucessful = True + except RuntimeError: + print("flashFactoryCalibration Failed...") + is_write_factory_sucessful = False if is_write_succesful: + + + """ eepromUnionData = {} + calibHandler = self.device.readCalibration2() + eepromUnionData['calibrationUser'] = calibHandler.eepromToJson() + + calibHandler = self.device.readFactoryCalibration() + eepromUnionData['calibrationFactory'] = calibHandler.eepromToJson() + + eepromUnionData['calibrationUserRaw'] = self.device.readCalibrationRaw() + eepromUnionData['calibrationFactoryRaw'] = self.device.readFactoryCalibrationRaw() + with open(calib_dest_path, "w") as outfile: + json.dump(eepromUnionData, outfile, indent=4) """ + self.device.close() + text = "EEPROM written succesfully" resImage = create_blank(900, 512, rgb_color=green) - text = "Calibration Succesful with" - cv2.putText(resImage, text, (10, 250), - font, 2, (0, 0, 0), 2) - text = "Epipolar error of " + str(epiploar_error) - cv2.putText(resImage, text, (10, 300), - font, 2, (0, 0, 0), 2) + cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2) + cv2.imshow("Result Image", resImage) + cv2.waitKey(0) + else: + self.device.close() + text = "EEPROM write Failed!!" resImage = create_blank(900, 512, rgb_color=red) - text = "EEprom Write Failed!! " + str(epiploar_error) - cv2.putText(resImage, text, (10, 250), - font, 2, (0, 0, 0), 2) - text = "Try recalibrating !!" - cv2.putText(resImage, text, (10, 300), - font, 2, (0, 0, 0), 2) - - + cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2) + cv2.imshow("Result Image", resImage) + cv2.waitKey(0) + # return (False, "EEPROM write Failed!!") + else: - # calib_dest_path = dest_path + '/depthai_calib.json' - # calibration_handler.eepromToJsonFile(calib_dest_path) - resImage = create_blank(900, 512, rgb_color=red) - text = "Calibratin succesful. " + str(epiploar_error) - cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2) - # text = "Device not found to write to EEPROM" - # cv2.putText(resImage, text, (10, 300), font, 2, (0, 0, 0), 2) - - if resImage is not None: - cv2.imshow("Result Image", resImage) - cv2.waitKey(0) - except AssertionError as e: - print("[ERROR] " + str(e)) + self.device.close() + print(error_text) + for text in error_text: + # text = error_text[0] + resImage = create_blank(900, 512, rgb_color=false) + cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2) + cv2.imshow("Result Image", resImage) + cv2.waitKey(0) + except Exception as e: + print(e) raise SystemExit(1) def run(self): @@ -652,7 +959,7 @@ def run(self): print("An error occurred trying to create image dataset directories!") raise SystemExit(1) self.show_info_frame() - self.capture_images() + self.capture_images_sync() self.dataset_path = str(Path("dataset").absolute()) if 'process' in self.args.mode: self.calibrate() diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index ff9a77617..dadf3821c 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -5,6 +5,7 @@ import os import shutil import numpy as np +from scipy.spatial.transform import Rotation as R import re import time import json @@ -21,24 +22,36 @@ def setPolygonCoordinates(height, width): slope = 150 p_coordinates = [ - [[margin,margin], [margin, height-margin], [width-margin, height-margin], [width-margin, margin]], - - [[margin,0], [margin,height], [width//2, height-slope], [width//2, slope]], - [[horizontal_shift, 0], [horizontal_shift, height], [width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]], - [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 + horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]], - - [[width-margin, 0], [width-margin, height], [width//2, height-slope], [width//2, slope]], - [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width//2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]], - [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width//2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]], - - [[0,margin], [width, margin], [width-slope, height//2], [slope, height//2]], - [[0,vertical_shift], [width, vertical_shift], [width-slope, height//2+vertical_shift], [slope, height//2+vertical_shift]], - [[0,vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope, height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]], - - [[0,height-margin], [width, height-margin], [width-slope, height//2], [slope, height//2]], - [[0,height-vertical_shift], [width, height-vertical_shift], [width-slope, height//2-vertical_shift], [slope, height//2-vertical_shift]], - [[0,height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width-slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]] - ] + [[margin, margin], [margin, height-margin], + [width-margin, height-margin], [width-margin, margin]], + + [[margin, 0], [margin, height], [width//2, height-slope], [width//2, slope]], + [[horizontal_shift, 0], [horizontal_shift, height], [ + width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]], + [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 + + horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]], + + [[width-margin, 0], [width-margin, height], + [width//2, height-slope], [width//2, slope]], + [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width // + 2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]], + [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width // + 2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]], + + [[0, margin], [width, margin], [ + width-slope, height//2], [slope, height//2]], + [[0, vertical_shift], [width, vertical_shift], [width-slope, + height//2+vertical_shift], [slope, height//2+vertical_shift]], + [[0, vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope, + height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]], + + [[0, height-margin], [width, height-margin], + [width-slope, height//2], [slope, height//2]], + [[0, height-vertical_shift], [width, height-vertical_shift], [width - + slope, height//2-vertical_shift], [slope, height//2-vertical_shift]], + [[0, height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width - + slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]] + ] return p_coordinates @@ -76,115 +89,82 @@ class StereoCalibration(object): def __init__(self): """Class to Calculate Calibration and Rectify a Stereo Camera.""" - def calibrate(self, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, calibrate_rgb, enable_disp_rectify): + def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify): """Function to calculate calibration for stereo camera.""" start_time = time.time() # init object data - self.calibrate_rgb = calibrate_rgb self.enable_rectification_disp = enable_disp_rectify - self.cameraModel = camera_model + self.cameraModel = camera_model self.data_path = filepath self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) self.board = aruco.CharucoBoard_create( - # 22, 16, - squaresX, squaresY, - square_size, - mrk_size, - self.aruco_dictionary) + # 22, 16, + squaresX, squaresY, + square_size, + mrk_size, + self.aruco_dictionary) - - # parameters = aruco.DetectorParameters_create() + # parameters = aruco.DetectorParameters_create() assert mrk_size != None, "ERROR: marker size not set" - self.calibrate_charuco3D(filepath) - - # self.stereo_calibrate_two_homography_calib() - print('~~~~~ Starting Stereo Calibration ~~~~~') - # self.stereo_calib_two_homo() - - # rgb-right extrinsic calibration - if self.calibrate_rgb: - # if True: - # things to do. - # First: change the center and other thigns of the calibration matrix of rgb camera - self.rgb_calibrate(filepath) - else: - self.M3 = np.zeros((3, 3), dtype=np.float32) - self.R_rgb = np.zeros((3, 3), dtype=np.float32) - self.T_rgb = np.zeros(3, dtype=np.float32) - self.d3 = np.zeros(14, dtype=np.float32) - - # self.M3_scaled_write = np.copy(self.M3_scaled) - # self.M3_scaled_write[1, 2] += 40 - - R1_fp32 = self.R1.astype(np.float32) - R2_fp32 = self.R2.astype(np.float32) - M1_fp32 = self.M1.astype(np.float32) - M2_fp32 = self.M2.astype(np.float32) - M3_fp32 = self.M3.astype(np.float32) - - R_fp32 = self.R.astype(np.float32) # L-R rotation - T_fp32 = self.T.astype(np.float32) # L-R translation - R_rgb_fp32 = self.R_rgb.astype(np.float32) - T_rgb_fp32 = self.T_rgb.astype(np.float32) - - d1_coeff_fp32 = self.d1.astype(np.float32) - d2_coeff_fp32 = self.d2.astype(np.float32) - d3_coeff_fp32 = self.d3.astype(np.float32) - - if self.calibrate_rgb: - R_rgb_fp32 = np.linalg.inv(R_rgb_fp32) - T_rgb_fp32[0] = -T_rgb_fp32[0] - T_rgb_fp32[1] = -T_rgb_fp32[1] - T_rgb_fp32[2] = -T_rgb_fp32[2] - - self.calib_data = [R1_fp32, R2_fp32, M1_fp32, M2_fp32, M3_fp32, R_fp32, T_fp32, R_rgb_fp32, T_rgb_fp32, d1_coeff_fp32, d2_coeff_fp32, d3_coeff_fp32] - - if 1: # Print matrices, to compare with device data - np.set_printoptions(suppress=True, precision=6) - print("\nR1 (left)"); print(R1_fp32) - print("\nR2 (right)"); print(R2_fp32) - print("\nM1 (left)"); print(M1_fp32) - print("\nM2 (right)"); print(M2_fp32) - print("\nR"); print(R_fp32) - print("\nT"); print(T_fp32) - print("\nM3 (rgb)"); print(M3_fp32) - print("\R (rgb)") - print(R_rgb_fp32) - print("\nT (rgb)") - print(T_rgb_fp32) - - if 0: # Print computed homography, to compare with device data - np.set_printoptions(suppress=True, precision=6) - for res_height in [800, 720, 400]: - m1 = np.copy(M1_fp32) - m2 = np.copy(M2_fp32) - if res_height == 720: - m1[1, 2] -= 40 - m2[1, 2] -= 40 - if res_height == 400: - m_scale = [[0.5, 0, 0], - [0, 0.5, 0], - [0, 0, 1]] - m1 = np.matmul(m_scale, m1) - m2 = np.matmul(m_scale, m2) - h1 = np.matmul(np.matmul(m2, R1_fp32), np.linalg.inv(m1)) - h2 = np.matmul(np.matmul(m2, R2_fp32), np.linalg.inv(m2)) - h1 = np.linalg.inv(h1) - h2 = np.linalg.inv(h2) - print('\nHomography H1, H2 for height =', res_height) - print(h1) - print() - print(h2) - - print("\tTook %i seconds to run image processing." % - (round(time.time() - start_time, 2))) - - self.create_save_mesh() - - if self.calibrate_rgb: - return self.test_epipolar_charuco_lr(filepath), self.test_epipolar_charuco_rgbr(filepath), self.calib_data - else: - return self.test_epipolar_charuco_lr(filepath), None, self.calib_data + + for camera in board_config['cameras'].keys(): + cam_info = board_config['cameras'][camera] + print( + '<------------Calibrating {} ------------>'.format(cam_info['name'])) + images_path = filepath + '/' + cam_info['name'] + ret, intrinsics, dist_coeff, _, _, size = self.calibrate_intrinsics( + images_path, cam_info['hfov']) + cam_info['intrinsics'] = intrinsics + cam_info['dist_coeff'] = dist_coeff + cam_info['size'] = size + cam_info['reprojection_error'] = ret + print( + '<------------Camera Name: {} ------------>'.format(cam_info['name'])) + print("Reprojection error of {0}: {1}".format(cam_info['name'], ret)) + print("intrinsics of {0}: \n {1}".format(cam_info['name'], intrinsics)) + # print(intrinsics) + # print(ret) + + for camera in board_config['cameras'].keys(): + left_cam_info = board_config['cameras'][camera] + if 'extrinsics' in left_cam_info: + if 'to_cam' in left_cam_info['extrinsics']: + left_cam = camera + right_cam = left_cam_info['extrinsics']['to_cam'] + left_path = filepath + '/' + left_cam_info['name'] + + right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']] + right_path = filepath + '/' + right_cam_info['name'] + print('<-------------Extrinsics calibration of {} and {} ------------>'.format( + left_cam_info['name'], right_cam_info['name'])) + + specTranslation = left_cam_info['extrinsics']['specTranslation'] + rot = left_cam_info['extrinsics']['rotation'] + + translation = np.array( + [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32) + rotation = R.from_euler( + 'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32) + + extrinsics = self.calibrate_extrinsics(left_path, right_path, left_cam_info['intrinsics'], left_cam_info[ + 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation) + if extrinsics[0] == -1: + return -1, extrinsics[1] + + if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: + board_config['stereo_config']['rectification_left'] = extrinsics[3] + board_config['stereo_config']['rectification_right'] = extrinsics[4] + + left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] + left_cam_info['extrinsics']['translation'] = extrinsics[2] + left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] + + print('<-------------Epipolar error of {} and {} ------------>'.format( + left_cam_info['name'], right_cam_info['name'])) + left_cam_info['extrinsics']['epipolar_error'] = self.test_epipolar_charuco( + left_path, right_path, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], extrinsics[3], extrinsics[4]) + + return 1, board_config def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): """ @@ -202,36 +182,26 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) count = 0 for im in images: - print("=> Processing image {0}".format(im)) + # print("=> Processing image {0}".format(im)) + img_pth = Path(im) frame = cv2.imread(im) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - # gray = cv2.flip(gray, 0) # TODO(Sachin) : remove this later - # width = scale[1] expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1]) - # print('expected height -------------------> ' + str(expected_height)) - # print('required height -------------------> ' + - # str(req_resolution)) if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]): - # print("scaling----------------------->") 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) - # print('~~~~~~~~~~ Only resizing.... ~~~~~~~~~~~~~~~~') 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)) - # print("destination resolution------>") - # print(dest_res) gray = cv2.resize( gray, dest_res, interpolation=cv2.INTER_CUBIC) - # print("scaled gray shape") - # print(gray.shape) 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])) @@ -240,26 +210,14 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): # gray = gray[: req_resolution[0], :] gray = gray[del_height: del_height + req_resolution[0], :] - # print("del height ??") - # print(del_height) - # print(gray.shape) count += 1 - # self.parse_frame(gray, 'rgb_resized', - # 'rgb_resized_'+str(count)) 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) - print('{0} number of Markers corners detected in the above image'.format( - len(marker_corners))) + print('{0} number of Markers corners detected in the image {1}'.format( + len(marker_corners), img_pth.name)) if len(marker_corners) > 0: - # print(len(marker_corners)) - # SUB PIXEL DETECTION - # for corner in marker_corners: - # cv2.cornerSubPix(gray, corner, - # winSize = (5,5), - # zeroZone = (-1,-1), - # criteria = criteria) res2 = cv2.aruco.interpolateCornersCharuco( marker_corners, ids, gray, self.board) @@ -270,142 +228,123 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): winSize=(5, 5), zeroZone=(-1, -1), criteria=criteria) - allCorners.append(res2[1]) # Charco chess corners - allIds.append(res2[2]) # charuco chess corner id's + allCorners.append(res2[1]) # Charco chess corners + allIds.append(res2[2]) # charuco chess corner id's all_marker_corners.append(marker_corners) all_marker_ids.append(ids) all_recovered.append(recoverd) else: - print("in else") + raise RuntimeError("Failed to detect markers in the image") else: print(im + " Not found") - # decimator+=1 + raise RuntimeError("Failed to detect markers in the image") imsize = gray.shape return allCorners, allIds, all_marker_corners, all_marker_ids, imsize, all_recovered - def calibrate_charuco3D(self, filepath): + def calibrate_intrinsics(self, image_files, hfov): + image_files = glob.glob(image_files + "/*") + image_files.sort() + assert len( + image_files) != 0, "ERROR: Images not read correctly, check directory" + + 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( + allCorners, allIds, imsize[::-1], hfov) + # (Height, width) + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] + else: + ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_fisheye( + allCorners, allIds, imsize[::-1]) + # (Height, width) + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] + + def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): self.objpoints = [] # 3d point in real world space self.imgpoints_l = [] # 2d points in image plane. self.imgpoints_r = [] # 2d points in image plane. - # calcorners_l = [] # 2d points in image - # calcorners_r = [] # 2d points in image - # calids_l = [] # ids found in imag - # calids_r = [] # ids found in imag + images_left = glob.glob(images_left + "/*") + images_right = glob.glob(images_right + "/*") - images_left = glob.glob(filepath + "/left/*") - images_right = glob.glob(filepath + "/right/*") - # images_rgb = glob.glob(filepath + "/rgb/*") - # print("Images left path------------------->") - # print(images_left) images_left.sort() images_right.sort() - # images_rgb.sort() assert len( - images_left) != 0, "ERROR: Images not read correctly, check directory" + images_left) != 0, "ERROR: Images not found, check directory" assert len( - images_right) != 0, "ERROR: Images not read correctly, check directory" - # assert len( - # images_rgb) != 0, "ERROR: Images not read correctly, check directory" - - print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") - allCorners_l, allIds_l, _, _, imsize, _ = self.analyze_charuco( - images_left) - allCorners_r, allIds_r, _, _, imsize, _ = self.analyze_charuco( - images_right) - self.img_shape = imsize[::-1] - - # self.img_shape_rgb = imsize_rgb[::-1] - if self.cameraModel == 'perspective': - ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_l, allIds_l, self.img_shape) - ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_r, allIds_r, self.img_shape) - else: - ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_fisheye(allCorners_l, allIds_l, self.img_shape) - ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_fisheye(allCorners_r, allIds_r, self.img_shape) - # self.fisheye_undistort_visualizaation(images_left, self.M1, self.d1, self.img_shape) - # self.fisheye_undistort_visualizaation(images_right, self.M2, self.d2, self.img_shape) - - - print("~~~~~~~~~~~~~RMS error of left~~~~~~~~~~~~~~") - print(ret_l) - print(ret_r) - print(self.M1) - print(self.M2) - print(self.d1) - print(self.d2) - # if self.cameraModel == 'perspective': - ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) - # else: - # ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) - print("~~~~~~~~~~~~~RMS error of L-R~~~~~~~~~~~~~~") - print(ret) - """ - left_corners_sampled = [] - right_corners_sampled = [] - obj_pts = [] - one_pts = self.board.chessboardCorners - for i in range(len(allIds_l)): - left_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_l[i])): - idx = np.where(allIds_r[i] == allIds_l[i][j]) - if idx[0].size == 0: - continue - left_sub_corners.append(allCorners_l[i][j]) - right_sub_corners.append(allCorners_r[i][idx]) - obj_pts_sub.append(one_pts[allIds_l[i][j]]) - - obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) - left_corners_sampled.append( - np.array(left_sub_corners, dtype=np.float32)) - right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - - self.objpoints = obj_pts - self.imgpoints_l = left_corners_sampled - self.imgpoints_r = right_corners_sampled - - flags = 0 - flags |= cv2.CALIB_USE_INTRINSIC_GUESS - flags |= cv2.CALIB_RATIONAL_MODEL - - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = cv2.stereoCalibrate( - self.objpoints, self.imgpoints_l, self.imgpoints_r, - self.M1, self.d1, self.M2, self.d2, self.img_shape, - criteria=stereocalib_criteria, flags=flags) - print("<~~ ~~~~~~~~~~~ RMS of stereo ~~~~~~~~~~~ ~~>") - print('RMS error of stereo calibration of left-right is {0}'.format(ret)) """ - - # TODO(sachin): Fix rectify for Fisheye - if self.cameraModel == 'perspective': - - self.R1, self.R2, self.P1, self.P2, self.Q, validPixROI1, validPixROI2 = cv2.stereoRectify( - self.M1, - self.d1, - self.M2, - self.d2, - self.img_shape, self.R, self.T) - else: - self.R1, self.R2, self.P1, self.P2, self.Q = cv2.fisheye.stereoRectify(self.M1, - self.d1, - self.M2, - self.d2, - self.img_shape, self.R, self.T) - - self.H1 = np.matmul(np.matmul(self.M2, self.R1), - np.linalg.inv(self.M1)) - self.H2 = np.matmul(np.matmul(self.M2, self.R2), - np.linalg.inv(self.M2)) + images_right) != 0, "ERROR: Images not found, check directory" + # print('Images from left and right') + # print(images_left[0]) + # print(images_right[0]) + + 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)) + + # 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) + + # print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") + allCorners_l, allIds_l, _, _, imsize_l, _ = self.analyze_charuco( + images_left, scale_req, scaled_res) + + # print("~~~~~~~~~~~ POSE ESTIMATION RIGHT CAMERA ~~~~~~~~~~~~~") + allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco( + images_right, scale_req, scaled_res) + + print(f'Image size of right side :{imsize_r}') + print(f'Image size of left side :{imsize_l}') + + assert imsize_r == imsize_l, "Left and right resolution scaling is wrong" + + 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_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] + * scale - destShape[0]) / 2 + scaled_intrinsics[0][2] -= (originalShape[1] + * scale - destShape[1]) / 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): for im in img_list: @@ -414,36 +353,50 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): # h, w = img.shape[:2] if self.cameraModel == 'perspective': map1, map2 = cv2.initUndistortRectifyMap( - K, D, np.eye(3), K, img_size, cv2.CV_32FC1) + 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) - - undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + map1, map2 = cv2.fisheye.initUndistortRectifyMap( + K, D, np.eye(3), K, 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) cv2.waitKey(0) - # cv2.destroyAllWindows() - + # cv2.destroyAllWindows() - def calibrate_camera_charuco(self, allCorners, allIds, imsize): + def calibrate_camera_charuco(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) - if imsize[1] < 1100: + 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.]]) - + [0.0, 0.0, 1.]]) """ + print("Camera Matrix initialization.............") print(cameraMatrixInit) distCoeffsInit = np.zeros((5, 1)) - flags = (cv2.CALIB_USE_INTRINSIC_GUESS + + flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO) # flags = (cv2.CALIB_RATIONAL_MODEL) (ret, camera_matrix, distortion_coefficients, @@ -457,8 +410,9 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize): cameraMatrix=cameraMatrixInit, distCoeffs=distCoeffsInit, flags=flags, - criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9)) - + criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9)) + print('Per View Errors...') + print(perViewErrors) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors def calibrate_fisheye(self, allCorners, allIds, imsize): @@ -469,32 +423,31 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): for j in range(len(allIds[i])): 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]]) - + print("Camera Matrix initialization.............") print(cameraMatrixInit) - flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + flags = 0 distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags = flags, criteria = term_criteria) - - def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r): + cv2.TERM_CRITERIA_EPS, 100, 1e-5) + + return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) + + def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in): left_corners_sampled = [] right_corners_sampled = [] obj_pts = [] one_pts = self.board.chessboardCorners - print('allIds_l') - print(len(allIds_l)) - print(len(allIds_r)) - print('allIds_l') + # print('allIds_l') + # print(len(allIds_l)) + # print('allIds_r') + # print(len(allIds_r)) + # print('allIds_l') # print(allIds_l) - print('allIds_r') # print(allIds_r) for i in range(len(allIds_l)): @@ -510,180 +463,203 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz left_sub_corners.append(allCorners_l[i][j]) right_sub_corners.append(allCorners_r[i][idx]) obj_pts_sub.append(one_pts[allIds_l[i][j]]) - - obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) - left_corners_sampled.append( - np.array(left_sub_corners, dtype=np.float32)) - right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) + if len(left_sub_corners) > 3 and len(right_sub_corners) > 3: + obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) + left_corners_sampled.append( + np.array(left_sub_corners, dtype=np.float32)) + right_corners_sampled.append( + np.array(right_sub_corners, dtype=np.float32)) + else: + return -1, "Stereo Calib failed due to less common features" stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) + cv2.TERM_CRITERIA_EPS, 50000, 1e-9) if self.cameraModel == 'perspective': flags = 0 - flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess + # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS + # print(flags) + + flags |= cv2.CALIB_FIX_INTRINSIC + # flags |= cv2.CALIB_USE_INTRINSIC_GUESS flags |= cv2.CALIB_RATIONAL_MODEL + # print(flags) + print('Printing Extrinsics guesses...') + print(r_in) + print(t_in) - return cv2.stereoCalibrate( + 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, - criteria=stereocalib_criteria, flags=flags) + R=r_in, T=t_in, criteria=stereocalib_criteria, flags=flags) + print('Printing Extrinsics res...') + print(R) + print(T) + # ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate( + # obj_pts, left_corners_sampled, right_corners_sampled, + # cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, + # criteria=stereocalib_criteria, flags=flags) + + R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( + cameraMatrix_l, + distCoeff_l, + cameraMatrix_r, + distCoeff_r, + imsize, R, T) + # self.P_l = P_l + # self.P_r = P_r + + return [ret, R, T, R_l, R_r] + elif self.cameraModel == 'fisheye': - print(len(obj_pts)) - print('obj_pts') - # print(obj_pts) - print(len(left_corners_sampled)) - print('left_corners_sampled') - # print(left_corners_sampled) - print(len(right_corners_sampled)) - print('right_corners_sampled') + # print(len(obj_pts)) + # print('obj_pts') + # print(obj_pts) + # print(len(left_corners_sampled)) + # print('left_corners_sampled') + # print(left_corners_sampled) + # print(len(right_corners_sampled)) + # print('right_corners_sampled') # print(right_corners_sampled) - for i in range(len(obj_pts)): - print('---------------------') - print(i) - print(len(obj_pts[i])) - print(len(left_corners_sampled[i])) - print(len(right_corners_sampled[i])) + # for i in range(len(obj_pts)): + # print('---------------------') + # print(i) + # print(len(obj_pts[i])) + # print(len(left_corners_sampled[i])) + # print(len(right_corners_sampled[i])) flags = 0 - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(sACHIN): Try without intrinsic guess - return cv2.fisheye.stereoCalibrate( + flags |= cv2.CALIB_FIX_INTRINSIC + flags |= cv2.CALIB_RATIONAL_MODEL + # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(SACHIN): Try without intrinsic guess + ret, M1, d1, M2, d2, R, T, E, F = cv2.fisheye.stereoCalibrate( obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, flags=flags, criteria=stereocalib_criteria), None, None - - def rgb_calibrate(self, filepath): - images_right = glob.glob(filepath + "/right/*") - images_rgb = glob.glob(filepath + "/rgb/*") - images_rgb_pth = Path(filepath + "/rgb") - if not images_rgb_pth.exists(): - raise RuntimeError("RGB dataset folder not found!! To skip rgb calibration use -drgb argument") + R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( + cameraMatrix_l, + distCoeff_l, + cameraMatrix_r, + distCoeff_r, + imsize, R, T) - images_right.sort() - images_rgb.sort() - - allCorners_rgb_scaled, allIds_rgb_scaled, _, _, imsize_rgb_scaled, _ = self.analyze_charuco( - images_rgb, scale_req=True, req_resolution=(720, 1280)) - self.img_shape_rgb_scaled = imsize_rgb_scaled[::-1] - - ret_rgb_scaled, self.M3_scaled, self.d3_scaled, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_rgb_scaled, allIds_rgb_scaled, imsize_rgb_scaled[::-1]) - - allCorners_r_rgb, allIds_r_rgb, _, _, _, _ = self.analyze_charuco( - images_right, scale_req=True, req_resolution=(720, 1280)) - - print("RGB callleded RMS at 720") - print(ret_rgb_scaled) - print(imsize_rgb_scaled) - print('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~') - # print(self.M3_scaled) - - # sampling common detected corners - rgb_scaled_rgb_corners_sampled = [] - rgb_scaled_right_corners_sampled = [] - rgb_scaled_obj_pts = [] - rgb_pts = None - right_pts = None - one_pts = self.board.chessboardCorners - for i in range(len(allIds_rgb_scaled)): - rgb_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_rgb_scaled[i])): - idx = np.where(allIds_r_rgb[i] == allIds_rgb_scaled[i][j]) - if idx[0].size == 0: - continue - rgb_sub_corners.append(allCorners_rgb_scaled[i][j]) - right_sub_corners.append(allCorners_r_rgb[i][idx]) - obj_pts_sub.append(one_pts[allIds_rgb_scaled[i][j]]) - - rgb_scaled_obj_pts.append( - np.array(obj_pts_sub, dtype=np.float32)) - rgb_scaled_rgb_corners_sampled.append( - np.array(rgb_sub_corners, dtype=np.float32)) - rgb_scaled_right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - if rgb_pts is None: - rgb_pts = np.array(rgb_sub_corners, dtype=np.float32) - right_pts = np.array(right_sub_corners, dtype=np.float32) - else: - np.vstack( - (rgb_pts, np.array(rgb_sub_corners, dtype=np.float32))) - np.vstack((right_pts, np.array( - right_sub_corners, dtype=np.float32))) - - self.objpoints_rgb_r = rgb_scaled_obj_pts - self.imgpoints_rgb = rgb_scaled_rgb_corners_sampled - self.imgpoints_rgb_right = rgb_scaled_right_corners_sampled - - flags = 0 - flags |= cv2.CALIB_FIX_INTRINSIC - flags |= cv2.CALIB_RATIONAL_MODEL + return [ret, R, T, R_l, R_r] + def display_rectification(self, image_data_pairs): + print( + "Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") + for image_data_pair in image_data_pairs: + img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) + # draw epipolar lines for debug purposes + 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 - # print(M_RGB) - print('vs. intrinisics computed after scaling the image --->') - # self.M3, self.d3 - scale = 1920/1280 - print(scale) - scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]]) - self.M3 = np.matmul(scale_mat, self.M3_scaled) - self.d3 = self.d3_scaled - print(self.M3_scaled) - print(self.M3) - - self.M2_rgb = np.copy(self.M2) - self.M2_rgb[1, 2] -= 40 - self.d2_rgb = np.copy(self.d1) - - ret, _, _, _, _, self.R_rgb, self.T_rgb, E, F = cv2.stereoCalibrate( - self.objpoints_rgb_r, self.imgpoints_rgb, self.imgpoints_rgb_right, - self.M3_scaled, self.d3_scaled, self.M2_rgb, self.d2_rgb, self.img_shape_rgb_scaled, - criteria=stereocalib_criteria, flags=flags) - print("~~~~~~ Stereo calibration rgb-left RMS error ~~~~~~~~") - print(ret) - - # Rectification is only to test the epipolar - self.R1_rgb, self.R2_rgb, self.P1_rgb, self.P2_rgb, self.Q_rgb, validPixROI1, validPixROI2 = cv2.stereoRectify( - self.M3_scaled, - self.d3_scaled, - self.M2_rgb, - self.d2_rgb, - self.img_shape_rgb_scaled, self.R_rgb, self.T_rgb) - - def test_epipolar_charuco_lr(self, dataset_dir): - print("<-----------------Epipolar error of LEFT-right camera---------------->") - images_left = glob.glob(dataset_dir + '/left/*.png') - images_right = glob.glob(dataset_dir + '/right/*.png') + # show image + cv2.imshow('Stereo Pair', img_concat) + k = cv2.waitKey(0) + 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 test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, r_l, r_r): + images_left = glob.glob(left_img_pth + '/*.png') + images_right = glob.glob(right_img_pth + '/*.png') images_left.sort() images_right.sort() - print("HU IHER") assert len(images_left) != 0, "ERROR: Images not read correctly" assert len(images_right) != 0, "ERROR: Images not read correctly" + + 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)) + + M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) + M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) + criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) - # if not use_homo: + # print('Scaled Res :{}'.format(scaled_res)) mapx_l, mapy_l = cv2.initUndistortRectifyMap( - self.M1, self.d1, self.R1, self.P1, self.img_shape, cv2.CV_32FC1) + M_lp, d_l, r_l, M_rp, scaled_res[::-1], cv2.CV_32FC1) mapx_r, mapy_r = cv2.initUndistortRectifyMap( - self.M2, self.d2, self.R2, self.P2, self.img_shape, cv2.CV_32FC1) - print("Printing p1 and p2") - print(self.P1) - print(self.P2) + M_rp, d_r, r_r, M_rp, scaled_res[::-1], cv2.CV_32FC1) + image_data_pairs = [] for image_left, image_right in zip(images_left, images_right): # read images img_l = cv2.imread(image_left, 0) img_r = cv2.imread(image_right, 0) - # warp right image + 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 + @@ -720,147 +696,22 @@ def test_epipolar_charuco_lr(self, dataset_dir): 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) - if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[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 = Path(images_right[i]) - name = img_pth.name - print("Image name {}".format(name)) - 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]) -# obj_pts_sub.append(one_pts[allIds_l[i][j]]) - -# obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) -# left_sub_corners_sampled.append(np.array(left_sub_corners, dtype=np.float32)) -# right_sub_corners_sampled.append(np.array(right_sub_corners, dtype=np.float32)) - - 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): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - - print("Average Epipolar Error per image on host in " + img_pth.name + " : " + - str(epi_error_sum / len(corners_l))) - - epi_error_sum = 0 - for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - - avg_epipolar = epi_error_sum / len(imgpoints_r) - print("Average Epipolar Error: " + str(avg_epipolar)) - - if self.enable_rectification_disp: - self.display_rectification(image_data_pairs) - - return avg_epipolar - - def test_epipolar_charuco_rgbr(self, dataset_dir): - images_rgb = glob.glob(dataset_dir + '/rgb/*.png') - images_right = glob.glob(dataset_dir + '/right/*.png') - images_rgb.sort() - images_right.sort() - print("<-----------------Epipolar error of rgb-right camera---------------->") - assert len(images_rgb) != 0, "ERROR: Images not read correctly" - assert len(images_right) != 0, "ERROR: Images not read correctly" - # criteria for marker detection/corner detections - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) - scale_width = 1280/self.img_shape_rgb_scaled[0] - print('scaled using {0}'.format(self.img_shape_rgb_scaled[0])) - - # if not use_homo: - mapx_rgb, mapy_rgb = cv2.initUndistortRectifyMap( - self.M3_scaled, self.d3_scaled, self.R1_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) - mapx_r, mapy_r = cv2.initUndistortRectifyMap( - self.M2_rgb, self.d2_rgb, self.R2_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) - - # self.H1_rgb = np.matmul(np.matmul(self.M2, self.R1_rgb), - # np.linalg.inv(M_rgb)) - # self.H2_r = np.matmul(np.matmul(self.M2, self.R2_rgb), - # np.linalg.inv(self.M2)) - - image_data_pairs = [] - count = 0 - for image_rgb, image_right in zip(images_rgb, images_right): - # read images - img_rgb = cv2.imread(image_rgb, 0) - img_r = cv2.imread(image_right, 0) - img_r = img_r[40: 760, :] - - dest_res = (int(img_rgb.shape[1] * scale_width), - int(img_rgb.shape[0] * scale_width)) - # print("RGB size ....") - # print(img_rgb.shape) - # print(dest_res) - - if img_rgb.shape[0] < 720: - raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format( - img_rgb.shape[0], req_resolution[0])) - del_height = (img_rgb.shape[0] - 720) // 2 - # print("del height ??") - # print(del_height) - img_rgb = img_rgb[del_height: del_height + 720, :] - # print("resized_shape") - # print(img_rgb.shape) - # self.parse_frame(img_rgb, "rectified_rgb_before", - # "rectified_"+str(count)) - - # warp right image - - # img_rgb = cv2.warpPerspective(img_rgb, self.H1_rgb, img_rgb.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - # img_r = cv2.warpPerspective(img_r, self.H2_r, img_r.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - img_rgb = cv2.remap(img_rgb, mapx_rgb, mapy_rgb, cv2.INTER_LINEAR) - img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR) - # self.parse_frame(img_rgb, "rectified_rgb", "rectified_"+str(count)) - image_data_pairs.append((img_rgb, img_r)) - count += 1 - - # compute metrics - imgpoints_r = [] - imgpoints_l = [] - for i, image_data_pair in enumerate(image_data_pairs): - # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - 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) + 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 - 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) + # cv2.imshow('Stereo Pair', img_concat) + # k = cv2.waitKey(0) + # if k == 27: # Esc key to stop + # break - 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) - if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[1]) > 3: + 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), @@ -871,107 +722,64 @@ def test_epipolar_charuco_rgbr(self, dataset_dir): zeroZone=(-1, -1), criteria=criteria) - # termination criteria - 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): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - img_pth = Path(images_right[i]) - # name = img_pth.name - print("Average Epipolar Error per image on host in " + img_pth.name + " : " + - str(epi_error_sum / len(corners_l))) - + # termination criteria + img_pth = Path(images_right[i]) + 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): + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + + print("Average Epipolar Error per image on host in " + img_pth.name + " : " + + str(epi_error_sum / len(corners_l))) + else: + print('Numer of corners is in left -> {} and right -> {}'.format(len(marker_corners_l), len(marker_corners_r))) + return -1 + epi_error_sum = 0 for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) avg_epipolar = epi_error_sum / len(imgpoints_r) - print("Average Epipolar Error of rgb_right: " + str(avg_epipolar)) + print("Average Epipolar Error is : " + str(avg_epipolar)) if self.enable_rectification_disp: self.display_rectification(image_data_pairs) return avg_epipolar - def display_rectification(self, image_data_pairs): - print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") - for image_data_pair in image_data_pairs: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) - img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - - # draw epipolar lines for debug purposes - 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 - - # show image - cv2.imshow('Stereo Pair', img_concat) - k = cv2.waitKey(0) - if k == 27: # Esc key to stop - break - - # os._exit(0) - # raise SystemExit() - - cv2.destroyWindow('Stereo Pair') - - def display_rectification(self, image_data_pairs): - print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") - for image_data_pair in image_data_pairs: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) - img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - - # draw epipolar lines for debug purposes - 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 - - # show image - cv2.imshow('Stereo Pair', img_concat) - k = cv2.waitKey(0) - if k == 27: # Esc key to stop - break - - # os._exit(0) - # raise SystemExit() - - cv2.destroyWindow('Stereo Pair') - - def create_save_mesh(self): #, output_path): + def create_save_mesh(self): # , output_path): curr_path = Path(__file__).parent.resolve() 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) + 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) + """ map_x_l_fp32 = map_x_l.astype(np.float32) map_y_l_fp32 = map_y_l.astype(np.float32) map_x_r_fp32 = map_x_r.astype(np.float32) map_y_r_fp32 = map_y_r.astype(np.float32) + print("shape of maps") print(map_x_l.shape) print(map_y_l.shape) print(map_x_r.shape) - print(map_y_r.shape) + print(map_y_r.shape) """ meshCellSize = 16 mesh_left = [] @@ -1004,14 +812,14 @@ def create_save_mesh(self): #, output_path): row_right.append(map_y_r[y, x]) row_right.append(map_x_r[y, x]) if (map_x_l.shape[1] % meshCellSize) % 2 != 0: - row_left.append(0) - row_left.append(0) - row_right.append(0) - row_right.append(0) + row_left.append(0) + row_left.append(0) + row_right.append(0) + row_right.append(0) mesh_left.append(row_left) - mesh_right.append(row_right) - + mesh_right.append(row_right) + mesh_left = np.array(mesh_left) mesh_right = np.array(mesh_right) left_mesh_fpath = str(curr_path) + '/../resources/left_mesh.calib' diff --git a/depthai_helpers/calibration_utils_old.py b/depthai_helpers/calibration_utils_old.py new file mode 100644 index 000000000..ff9a77617 --- /dev/null +++ b/depthai_helpers/calibration_utils_old.py @@ -0,0 +1,1020 @@ +#!/usr/bin/env python3 + +import cv2 +import glob +import os +import shutil +import numpy as np +import re +import time +import json +import cv2.aruco as aruco +from pathlib import Path +# Creates a set of 13 polygon coordinates + + +def setPolygonCoordinates(height, width): + horizontal_shift = width//4 + vertical_shift = height//4 + + margin = 60 + slope = 150 + + p_coordinates = [ + [[margin,margin], [margin, height-margin], [width-margin, height-margin], [width-margin, margin]], + + [[margin,0], [margin,height], [width//2, height-slope], [width//2, slope]], + [[horizontal_shift, 0], [horizontal_shift, height], [width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]], + [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 + horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]], + + [[width-margin, 0], [width-margin, height], [width//2, height-slope], [width//2, slope]], + [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width//2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]], + [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width//2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]], + + [[0,margin], [width, margin], [width-slope, height//2], [slope, height//2]], + [[0,vertical_shift], [width, vertical_shift], [width-slope, height//2+vertical_shift], [slope, height//2+vertical_shift]], + [[0,vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope, height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]], + + [[0,height-margin], [width, height-margin], [width-slope, height//2], [slope, height//2]], + [[0,height-vertical_shift], [width, height-vertical_shift], [width-slope, height//2-vertical_shift], [slope, height//2-vertical_shift]], + [[0,height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width-slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]] + ] + return p_coordinates + + +def getPolygonCoordinates(idx, p_coordinates): + return p_coordinates[idx] + + +def getNumOfPolygons(p_coordinates): + return len(p_coordinates) + +# Filters polygons to just those at the given indexes. + + +def select_polygon_coords(p_coordinates, indexes): + if indexes == None: + # The default + return p_coordinates + else: + print("Filtering polygons to those at indexes=", indexes) + return [p_coordinates[i] for i in indexes] + + +def image_filename(stream_name, polygon_index, total_num_of_captured_images): + return "{stream_name}_p{polygon_index}_{total_num_of_captured_images}.png".format(stream_name=stream_name, polygon_index=polygon_index, total_num_of_captured_images=total_num_of_captured_images) + + +def polygon_from_image_name(image_name): + """Returns the polygon index from an image name (ex: "left_p10_0.png" => 10)""" + return int(re.findall("p(\d+)", image_name)[0]) + + +class StereoCalibration(object): + """Class to Calculate Calibration and Rectify a Stereo Camera.""" + + def __init__(self): + """Class to Calculate Calibration and Rectify a Stereo Camera.""" + + def calibrate(self, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, calibrate_rgb, enable_disp_rectify): + """Function to calculate calibration for stereo camera.""" + start_time = time.time() + # init object data + self.calibrate_rgb = calibrate_rgb + self.enable_rectification_disp = enable_disp_rectify + self.cameraModel = camera_model + self.data_path = filepath + self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) + self.board = aruco.CharucoBoard_create( + # 22, 16, + squaresX, squaresY, + square_size, + mrk_size, + self.aruco_dictionary) + + + # parameters = aruco.DetectorParameters_create() + assert mrk_size != None, "ERROR: marker size not set" + self.calibrate_charuco3D(filepath) + + # self.stereo_calibrate_two_homography_calib() + print('~~~~~ Starting Stereo Calibration ~~~~~') + # self.stereo_calib_two_homo() + + # rgb-right extrinsic calibration + if self.calibrate_rgb: + # if True: + # things to do. + # First: change the center and other thigns of the calibration matrix of rgb camera + self.rgb_calibrate(filepath) + else: + self.M3 = np.zeros((3, 3), dtype=np.float32) + self.R_rgb = np.zeros((3, 3), dtype=np.float32) + self.T_rgb = np.zeros(3, dtype=np.float32) + self.d3 = np.zeros(14, dtype=np.float32) + + # self.M3_scaled_write = np.copy(self.M3_scaled) + # self.M3_scaled_write[1, 2] += 40 + + R1_fp32 = self.R1.astype(np.float32) + R2_fp32 = self.R2.astype(np.float32) + M1_fp32 = self.M1.astype(np.float32) + M2_fp32 = self.M2.astype(np.float32) + M3_fp32 = self.M3.astype(np.float32) + + R_fp32 = self.R.astype(np.float32) # L-R rotation + T_fp32 = self.T.astype(np.float32) # L-R translation + R_rgb_fp32 = self.R_rgb.astype(np.float32) + T_rgb_fp32 = self.T_rgb.astype(np.float32) + + d1_coeff_fp32 = self.d1.astype(np.float32) + d2_coeff_fp32 = self.d2.astype(np.float32) + d3_coeff_fp32 = self.d3.astype(np.float32) + + if self.calibrate_rgb: + R_rgb_fp32 = np.linalg.inv(R_rgb_fp32) + T_rgb_fp32[0] = -T_rgb_fp32[0] + T_rgb_fp32[1] = -T_rgb_fp32[1] + T_rgb_fp32[2] = -T_rgb_fp32[2] + + self.calib_data = [R1_fp32, R2_fp32, M1_fp32, M2_fp32, M3_fp32, R_fp32, T_fp32, R_rgb_fp32, T_rgb_fp32, d1_coeff_fp32, d2_coeff_fp32, d3_coeff_fp32] + + if 1: # Print matrices, to compare with device data + np.set_printoptions(suppress=True, precision=6) + print("\nR1 (left)"); print(R1_fp32) + print("\nR2 (right)"); print(R2_fp32) + print("\nM1 (left)"); print(M1_fp32) + print("\nM2 (right)"); print(M2_fp32) + print("\nR"); print(R_fp32) + print("\nT"); print(T_fp32) + print("\nM3 (rgb)"); print(M3_fp32) + print("\R (rgb)") + print(R_rgb_fp32) + print("\nT (rgb)") + print(T_rgb_fp32) + + if 0: # Print computed homography, to compare with device data + np.set_printoptions(suppress=True, precision=6) + for res_height in [800, 720, 400]: + m1 = np.copy(M1_fp32) + m2 = np.copy(M2_fp32) + if res_height == 720: + m1[1, 2] -= 40 + m2[1, 2] -= 40 + if res_height == 400: + m_scale = [[0.5, 0, 0], + [0, 0.5, 0], + [0, 0, 1]] + m1 = np.matmul(m_scale, m1) + m2 = np.matmul(m_scale, m2) + h1 = np.matmul(np.matmul(m2, R1_fp32), np.linalg.inv(m1)) + h2 = np.matmul(np.matmul(m2, R2_fp32), np.linalg.inv(m2)) + h1 = np.linalg.inv(h1) + h2 = np.linalg.inv(h2) + print('\nHomography H1, H2 for height =', res_height) + print(h1) + print() + print(h2) + + print("\tTook %i seconds to run image processing." % + (round(time.time() - start_time, 2))) + + self.create_save_mesh() + + if self.calibrate_rgb: + return self.test_epipolar_charuco_lr(filepath), self.test_epipolar_charuco_rgbr(filepath), self.calib_data + else: + return self.test_epipolar_charuco_lr(filepath), None, self.calib_data + + def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): + """ + Charuco base pose estimation. + """ + # print("POSE ESTIMATION STARTS:") + allCorners = [] + allIds = [] + all_marker_corners = [] + all_marker_ids = [] + all_recovered = [] + # decimator = 0 + # SUB PIXEL CORNER DETECTION CRITERION + criteria = (cv2.TERM_CRITERIA_EPS + + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) + count = 0 + for im in images: + print("=> Processing image {0}".format(im)) + frame = cv2.imread(im) + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + # gray = cv2.flip(gray, 0) # TODO(Sachin) : remove this later + # width = scale[1] + expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1]) + # print('expected height -------------------> ' + str(expected_height)) + # print('required height -------------------> ' + + # str(req_resolution)) + + if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]): + # print("scaling----------------------->") + 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) + # print('~~~~~~~~~~ Only resizing.... ~~~~~~~~~~~~~~~~') + 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)) + # print("destination resolution------>") + # print(dest_res) + gray = cv2.resize( + gray, dest_res, interpolation=cv2.INTER_CUBIC) + # print("scaled gray shape") + # print(gray.shape) + 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], :] + + # print("del height ??") + # print(del_height) + # print(gray.shape) + count += 1 + # self.parse_frame(gray, 'rgb_resized', + # 'rgb_resized_'+str(count)) + 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) + print('{0} number of Markers corners detected in the above image'.format( + len(marker_corners))) + if len(marker_corners) > 0: + # print(len(marker_corners)) + # SUB PIXEL DETECTION + # for corner in marker_corners: + # cv2.cornerSubPix(gray, corner, + # winSize = (5,5), + # zeroZone = (-1,-1), + # criteria = criteria) + res2 = cv2.aruco.interpolateCornersCharuco( + marker_corners, ids, gray, self.board) + + # if res2[1] is not None and res2[2] is not None and len(res2[1])>3 and decimator%1==0: + if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3: + + cv2.cornerSubPix(gray, res2[1], + winSize=(5, 5), + zeroZone=(-1, -1), + criteria=criteria) + allCorners.append(res2[1]) # Charco chess corners + allIds.append(res2[2]) # charuco chess corner id's + all_marker_corners.append(marker_corners) + all_marker_ids.append(ids) + all_recovered.append(recoverd) + else: + print("in else") + else: + print(im + " Not found") + # decimator+=1 + + imsize = gray.shape + return allCorners, allIds, all_marker_corners, all_marker_ids, imsize, all_recovered + + def calibrate_charuco3D(self, filepath): + self.objpoints = [] # 3d point in real world space + self.imgpoints_l = [] # 2d points in image plane. + self.imgpoints_r = [] # 2d points in image plane. + + # calcorners_l = [] # 2d points in image + # calcorners_r = [] # 2d points in image + # calids_l = [] # ids found in imag + # calids_r = [] # ids found in imag + + images_left = glob.glob(filepath + "/left/*") + images_right = glob.glob(filepath + "/right/*") + # images_rgb = glob.glob(filepath + "/rgb/*") + # print("Images left path------------------->") + # print(images_left) + images_left.sort() + images_right.sort() + # images_rgb.sort() + + assert len( + images_left) != 0, "ERROR: Images not read correctly, check directory" + assert len( + images_right) != 0, "ERROR: Images not read correctly, check directory" + # assert len( + # images_rgb) != 0, "ERROR: Images not read correctly, check directory" + + print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") + allCorners_l, allIds_l, _, _, imsize, _ = self.analyze_charuco( + images_left) + allCorners_r, allIds_r, _, _, imsize, _ = self.analyze_charuco( + images_right) + self.img_shape = imsize[::-1] + + # self.img_shape_rgb = imsize_rgb[::-1] + if self.cameraModel == 'perspective': + ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_camera_charuco( + allCorners_l, allIds_l, self.img_shape) + ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_camera_charuco( + allCorners_r, allIds_r, self.img_shape) + else: + ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_fisheye(allCorners_l, allIds_l, self.img_shape) + ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_fisheye(allCorners_r, allIds_r, self.img_shape) + # self.fisheye_undistort_visualizaation(images_left, self.M1, self.d1, self.img_shape) + # self.fisheye_undistort_visualizaation(images_right, self.M2, self.d2, self.img_shape) + + + print("~~~~~~~~~~~~~RMS error of left~~~~~~~~~~~~~~") + print(ret_l) + print(ret_r) + print(self.M1) + print(self.M2) + print(self.d1) + print(self.d2) + # if self.cameraModel == 'perspective': + ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) + # else: + # ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) + print("~~~~~~~~~~~~~RMS error of L-R~~~~~~~~~~~~~~") + print(ret) + """ + left_corners_sampled = [] + right_corners_sampled = [] + obj_pts = [] + one_pts = self.board.chessboardCorners + for i in range(len(allIds_l)): + left_sub_corners = [] + right_sub_corners = [] + obj_pts_sub = [] + # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: + # continue + for j in range(len(allIds_l[i])): + idx = np.where(allIds_r[i] == allIds_l[i][j]) + if idx[0].size == 0: + continue + left_sub_corners.append(allCorners_l[i][j]) + right_sub_corners.append(allCorners_r[i][idx]) + obj_pts_sub.append(one_pts[allIds_l[i][j]]) + + obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) + left_corners_sampled.append( + np.array(left_sub_corners, dtype=np.float32)) + right_corners_sampled.append( + np.array(right_sub_corners, dtype=np.float32)) + + self.objpoints = obj_pts + self.imgpoints_l = left_corners_sampled + self.imgpoints_r = right_corners_sampled + + flags = 0 + flags |= cv2.CALIB_USE_INTRINSIC_GUESS + flags |= cv2.CALIB_RATIONAL_MODEL + + stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + + cv2.TERM_CRITERIA_EPS, 100, 1e-5) + + ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = cv2.stereoCalibrate( + self.objpoints, self.imgpoints_l, self.imgpoints_r, + self.M1, self.d1, self.M2, self.d2, self.img_shape, + criteria=stereocalib_criteria, flags=flags) + print("<~~ ~~~~~~~~~~~ RMS of stereo ~~~~~~~~~~~ ~~>") + print('RMS error of stereo calibration of left-right is {0}'.format(ret)) """ + + # TODO(sachin): Fix rectify for Fisheye + if self.cameraModel == 'perspective': + + self.R1, self.R2, self.P1, self.P2, self.Q, validPixROI1, validPixROI2 = cv2.stereoRectify( + self.M1, + self.d1, + self.M2, + self.d2, + self.img_shape, self.R, self.T) + else: + self.R1, self.R2, self.P1, self.P2, self.Q = cv2.fisheye.stereoRectify(self.M1, + self.d1, + self.M2, + self.d2, + self.img_shape, self.R, self.T) + + self.H1 = np.matmul(np.matmul(self.M2, self.R1), + np.linalg.inv(self.M1)) + self.H2 = np.matmul(np.matmul(self.M2, self.R2), + np.linalg.inv(self.M2)) + + def fisheye_undistort_visualizaation(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': + 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) + + undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + cv2.imshow("undistorted", undistorted_img) + cv2.waitKey(0) + # cv2.destroyAllWindows() + + + def calibrate_camera_charuco(self, allCorners, allIds, imsize): + """ + Calibrates the camera using the dected corners. + """ + print("CAMERA CALIBRATION") + print(imsize) + if 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.]]) + + print("Camera Matrix initialization.............") + print(cameraMatrixInit) + + distCoeffsInit = np.zeros((5, 1)) + flags = (cv2.CALIB_USE_INTRINSIC_GUESS + + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO) + # flags = (cv2.CALIB_RATIONAL_MODEL) + (ret, camera_matrix, distortion_coefficients, + rotation_vectors, translation_vectors, + stdDeviationsIntrinsics, stdDeviationsExtrinsics, + perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended( + charucoCorners=allCorners, + charucoIds=allIds, + board=self.board, + imageSize=imsize, + cameraMatrix=cameraMatrixInit, + distCoeffs=distCoeffsInit, + flags=flags, + criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9)) + + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors + + def calibrate_fisheye(self, allCorners, allIds, imsize): + one_pts = self.board.chessboardCorners + obj_points = [] + for i in range(len(allIds)): + obj_pts_sub = [] + for j in range(len(allIds[i])): + 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]]) + + print("Camera Matrix initialization.............") + print(cameraMatrixInit) + flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + distCoeffsInit = np.zeros((4, 1)) + term_criteria = (cv2.TERM_CRITERIA_COUNT + + cv2.TERM_CRITERIA_EPS, 100, 1e-5) + + return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags = flags, criteria = term_criteria) + + def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r): + left_corners_sampled = [] + right_corners_sampled = [] + obj_pts = [] + one_pts = self.board.chessboardCorners + print('allIds_l') + print(len(allIds_l)) + print(len(allIds_r)) + print('allIds_l') + # print(allIds_l) + print('allIds_r') + # print(allIds_r) + + for i in range(len(allIds_l)): + left_sub_corners = [] + right_sub_corners = [] + obj_pts_sub = [] + # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: + # continue + for j in range(len(allIds_l[i])): + idx = np.where(allIds_r[i] == allIds_l[i][j]) + if idx[0].size == 0: + continue + left_sub_corners.append(allCorners_l[i][j]) + right_sub_corners.append(allCorners_r[i][idx]) + obj_pts_sub.append(one_pts[allIds_l[i][j]]) + + obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) + left_corners_sampled.append( + np.array(left_sub_corners, dtype=np.float32)) + right_corners_sampled.append( + np.array(right_sub_corners, dtype=np.float32)) + + stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + + cv2.TERM_CRITERIA_EPS, 100, 1e-5) + + if self.cameraModel == 'perspective': + flags = 0 + flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess + flags |= cv2.CALIB_RATIONAL_MODEL + + return cv2.stereoCalibrate( + obj_pts, left_corners_sampled, right_corners_sampled, + cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, + criteria=stereocalib_criteria, flags=flags) + elif self.cameraModel == 'fisheye': + print(len(obj_pts)) + print('obj_pts') + # print(obj_pts) + print(len(left_corners_sampled)) + print('left_corners_sampled') + # print(left_corners_sampled) + print(len(right_corners_sampled)) + print('right_corners_sampled') + # print(right_corners_sampled) + for i in range(len(obj_pts)): + print('---------------------') + print(i) + print(len(obj_pts[i])) + print(len(left_corners_sampled[i])) + print(len(right_corners_sampled[i])) + flags = 0 + # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(sACHIN): Try without intrinsic guess + return cv2.fisheye.stereoCalibrate( + obj_pts, left_corners_sampled, right_corners_sampled, + cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, + flags=flags, criteria=stereocalib_criteria), None, None + + def rgb_calibrate(self, filepath): + images_right = glob.glob(filepath + "/right/*") + images_rgb = glob.glob(filepath + "/rgb/*") + + images_rgb_pth = Path(filepath + "/rgb") + if not images_rgb_pth.exists(): + raise RuntimeError("RGB dataset folder not found!! To skip rgb calibration use -drgb argument") + + images_right.sort() + images_rgb.sort() + + allCorners_rgb_scaled, allIds_rgb_scaled, _, _, imsize_rgb_scaled, _ = self.analyze_charuco( + images_rgb, scale_req=True, req_resolution=(720, 1280)) + self.img_shape_rgb_scaled = imsize_rgb_scaled[::-1] + + ret_rgb_scaled, self.M3_scaled, self.d3_scaled, rvecs, tvecs = self.calibrate_camera_charuco( + allCorners_rgb_scaled, allIds_rgb_scaled, imsize_rgb_scaled[::-1]) + + allCorners_r_rgb, allIds_r_rgb, _, _, _, _ = self.analyze_charuco( + images_right, scale_req=True, req_resolution=(720, 1280)) + + print("RGB callleded RMS at 720") + print(ret_rgb_scaled) + print(imsize_rgb_scaled) + print('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~') + # print(self.M3_scaled) + + # sampling common detected corners + rgb_scaled_rgb_corners_sampled = [] + rgb_scaled_right_corners_sampled = [] + rgb_scaled_obj_pts = [] + rgb_pts = None + right_pts = None + one_pts = self.board.chessboardCorners + for i in range(len(allIds_rgb_scaled)): + rgb_sub_corners = [] + right_sub_corners = [] + obj_pts_sub = [] + # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: + # continue + for j in range(len(allIds_rgb_scaled[i])): + idx = np.where(allIds_r_rgb[i] == allIds_rgb_scaled[i][j]) + if idx[0].size == 0: + continue + rgb_sub_corners.append(allCorners_rgb_scaled[i][j]) + right_sub_corners.append(allCorners_r_rgb[i][idx]) + obj_pts_sub.append(one_pts[allIds_rgb_scaled[i][j]]) + + rgb_scaled_obj_pts.append( + np.array(obj_pts_sub, dtype=np.float32)) + rgb_scaled_rgb_corners_sampled.append( + np.array(rgb_sub_corners, dtype=np.float32)) + rgb_scaled_right_corners_sampled.append( + np.array(right_sub_corners, dtype=np.float32)) + if rgb_pts is None: + rgb_pts = np.array(rgb_sub_corners, dtype=np.float32) + right_pts = np.array(right_sub_corners, dtype=np.float32) + else: + np.vstack( + (rgb_pts, np.array(rgb_sub_corners, dtype=np.float32))) + np.vstack((right_pts, np.array( + right_sub_corners, dtype=np.float32))) + + self.objpoints_rgb_r = rgb_scaled_obj_pts + self.imgpoints_rgb = rgb_scaled_rgb_corners_sampled + self.imgpoints_rgb_right = rgb_scaled_right_corners_sampled + + flags = 0 + flags |= cv2.CALIB_FIX_INTRINSIC + flags |= cv2.CALIB_RATIONAL_MODEL + + + stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + + cv2.TERM_CRITERIA_EPS, 100, 1e-5) + + # print(M_RGB) + print('vs. intrinisics computed after scaling the image --->') + # self.M3, self.d3 + scale = 1920/1280 + print(scale) + scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]]) + self.M3 = np.matmul(scale_mat, self.M3_scaled) + self.d3 = self.d3_scaled + print(self.M3_scaled) + print(self.M3) + + self.M2_rgb = np.copy(self.M2) + self.M2_rgb[1, 2] -= 40 + self.d2_rgb = np.copy(self.d1) + + ret, _, _, _, _, self.R_rgb, self.T_rgb, E, F = cv2.stereoCalibrate( + self.objpoints_rgb_r, self.imgpoints_rgb, self.imgpoints_rgb_right, + self.M3_scaled, self.d3_scaled, self.M2_rgb, self.d2_rgb, self.img_shape_rgb_scaled, + criteria=stereocalib_criteria, flags=flags) + print("~~~~~~ Stereo calibration rgb-left RMS error ~~~~~~~~") + print(ret) + + # Rectification is only to test the epipolar + self.R1_rgb, self.R2_rgb, self.P1_rgb, self.P2_rgb, self.Q_rgb, validPixROI1, validPixROI2 = cv2.stereoRectify( + self.M3_scaled, + self.d3_scaled, + self.M2_rgb, + self.d2_rgb, + self.img_shape_rgb_scaled, self.R_rgb, self.T_rgb) + + def test_epipolar_charuco_lr(self, dataset_dir): + print("<-----------------Epipolar error of LEFT-right camera---------------->") + images_left = glob.glob(dataset_dir + '/left/*.png') + images_right = glob.glob(dataset_dir + '/right/*.png') + images_left.sort() + images_right.sort() + print("HU IHER") + assert len(images_left) != 0, "ERROR: Images not read correctly" + assert len(images_right) != 0, "ERROR: Images not read correctly" + criteria = (cv2.TERM_CRITERIA_EPS + + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) + + # if not use_homo: + mapx_l, mapy_l = cv2.initUndistortRectifyMap( + self.M1, self.d1, self.R1, self.P1, self.img_shape, cv2.CV_32FC1) + mapx_r, mapy_r = cv2.initUndistortRectifyMap( + self.M2, self.d2, self.R2, self.P2, self.img_shape, cv2.CV_32FC1) + print("Printing p1 and p2") + print(self.P1) + print(self.P2) + image_data_pairs = [] + for image_left, image_right in zip(images_left, images_right): + # read images + img_l = cv2.imread(image_left, 0) + img_r = cv2.imread(image_right, 0) + # 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)) + + # compute metrics + imgpoints_r = [] + imgpoints_l = [] + for i, image_data_pair in enumerate(image_data_pairs): + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + 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) + if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[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 = Path(images_right[i]) + name = img_pth.name + print("Image name {}".format(name)) + 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]) +# obj_pts_sub.append(one_pts[allIds_l[i][j]]) + +# obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) +# left_sub_corners_sampled.append(np.array(left_sub_corners, dtype=np.float32)) +# right_sub_corners_sampled.append(np.array(right_sub_corners, dtype=np.float32)) + + 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): + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + + print("Average Epipolar Error per image on host in " + img_pth.name + " : " + + str(epi_error_sum / len(corners_l))) + + epi_error_sum = 0 + for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + + avg_epipolar = epi_error_sum / len(imgpoints_r) + print("Average Epipolar Error: " + str(avg_epipolar)) + + if self.enable_rectification_disp: + self.display_rectification(image_data_pairs) + + return avg_epipolar + + def test_epipolar_charuco_rgbr(self, dataset_dir): + images_rgb = glob.glob(dataset_dir + '/rgb/*.png') + images_right = glob.glob(dataset_dir + '/right/*.png') + images_rgb.sort() + images_right.sort() + print("<-----------------Epipolar error of rgb-right camera---------------->") + assert len(images_rgb) != 0, "ERROR: Images not read correctly" + assert len(images_right) != 0, "ERROR: Images not read correctly" + # criteria for marker detection/corner detections + criteria = (cv2.TERM_CRITERIA_EPS + + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) + scale_width = 1280/self.img_shape_rgb_scaled[0] + print('scaled using {0}'.format(self.img_shape_rgb_scaled[0])) + + # if not use_homo: + mapx_rgb, mapy_rgb = cv2.initUndistortRectifyMap( + self.M3_scaled, self.d3_scaled, self.R1_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) + mapx_r, mapy_r = cv2.initUndistortRectifyMap( + self.M2_rgb, self.d2_rgb, self.R2_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) + + # self.H1_rgb = np.matmul(np.matmul(self.M2, self.R1_rgb), + # np.linalg.inv(M_rgb)) + # self.H2_r = np.matmul(np.matmul(self.M2, self.R2_rgb), + # np.linalg.inv(self.M2)) + + image_data_pairs = [] + count = 0 + for image_rgb, image_right in zip(images_rgb, images_right): + # read images + img_rgb = cv2.imread(image_rgb, 0) + img_r = cv2.imread(image_right, 0) + img_r = img_r[40: 760, :] + + dest_res = (int(img_rgb.shape[1] * scale_width), + int(img_rgb.shape[0] * scale_width)) + # print("RGB size ....") + # print(img_rgb.shape) + # print(dest_res) + + if img_rgb.shape[0] < 720: + raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format( + img_rgb.shape[0], req_resolution[0])) + del_height = (img_rgb.shape[0] - 720) // 2 + # print("del height ??") + # print(del_height) + img_rgb = img_rgb[del_height: del_height + 720, :] + # print("resized_shape") + # print(img_rgb.shape) + # self.parse_frame(img_rgb, "rectified_rgb_before", + # "rectified_"+str(count)) + + # warp right image + + # img_rgb = cv2.warpPerspective(img_rgb, self.H1_rgb, img_rgb.shape[::-1], + # cv2.INTER_CUBIC + + # cv2.WARP_FILL_OUTLIERS + + # cv2.WARP_INVERSE_MAP) + + # img_r = cv2.warpPerspective(img_r, self.H2_r, img_r.shape[::-1], + # cv2.INTER_CUBIC + + # cv2.WARP_FILL_OUTLIERS + + # cv2.WARP_INVERSE_MAP) + + img_rgb = cv2.remap(img_rgb, mapx_rgb, mapy_rgb, cv2.INTER_LINEAR) + img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR) + # self.parse_frame(img_rgb, "rectified_rgb", "rectified_"+str(count)) + image_data_pairs.append((img_rgb, img_r)) + count += 1 + + # compute metrics + imgpoints_r = [] + imgpoints_l = [] + for i, image_data_pair in enumerate(image_data_pairs): + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + 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) + if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[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 + 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): + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + img_pth = Path(images_right[i]) + # name = img_pth.name + print("Average Epipolar Error per image on host in " + img_pth.name + " : " + + str(epi_error_sum / len(corners_l))) + + epi_error_sum = 0 + for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): + epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + + avg_epipolar = epi_error_sum / len(imgpoints_r) + print("Average Epipolar Error of rgb_right: " + str(avg_epipolar)) + + if self.enable_rectification_disp: + self.display_rectification(image_data_pairs) + + return avg_epipolar + + def display_rectification(self, image_data_pairs): + print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") + for image_data_pair in image_data_pairs: + img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) + + # draw epipolar lines for debug purposes + 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 + + # show image + cv2.imshow('Stereo Pair', img_concat) + k = cv2.waitKey(0) + if k == 27: # Esc key to stop + break + + # os._exit(0) + # raise SystemExit() + + cv2.destroyWindow('Stereo Pair') + + def display_rectification(self, image_data_pairs): + print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") + for image_data_pair in image_data_pairs: + img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) + + # draw epipolar lines for debug purposes + 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 + + # show image + cv2.imshow('Stereo Pair', img_concat) + k = cv2.waitKey(0) + if k == 27: # Esc key to stop + break + + # os._exit(0) + # raise SystemExit() + + cv2.destroyWindow('Stereo Pair') + + def create_save_mesh(self): #, output_path): + + curr_path = Path(__file__).parent.resolve() + 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) + + map_x_l_fp32 = map_x_l.astype(np.float32) + map_y_l_fp32 = map_y_l.astype(np.float32) + map_x_r_fp32 = map_x_r.astype(np.float32) + map_y_r_fp32 = map_y_r.astype(np.float32) + + print("shape of maps") + print(map_x_l.shape) + print(map_y_l.shape) + print(map_x_r.shape) + print(map_y_r.shape) + + meshCellSize = 16 + mesh_left = [] + mesh_right = [] + + for y in range(map_x_l.shape[0] + 1): + if y % meshCellSize == 0: + row_left = [] + row_right = [] + for x in range(map_x_l.shape[1] + 1): + if x % meshCellSize == 0: + if y == map_x_l.shape[0] and x == map_x_l.shape[1]: + row_left.append(map_y_l[y - 1, x - 1]) + row_left.append(map_x_l[y - 1, x - 1]) + row_right.append(map_y_r[y - 1, x - 1]) + row_right.append(map_x_r[y - 1, x - 1]) + elif y == map_x_l.shape[0]: + row_left.append(map_y_l[y - 1, x]) + row_left.append(map_x_l[y - 1, x]) + row_right.append(map_y_r[y - 1, x]) + row_right.append(map_x_r[y - 1, x]) + elif x == map_x_l.shape[1]: + row_left.append(map_y_l[y, x - 1]) + row_left.append(map_x_l[y, x - 1]) + row_right.append(map_y_r[y, x - 1]) + row_right.append(map_x_r[y, x - 1]) + else: + row_left.append(map_y_l[y, x]) + row_left.append(map_x_l[y, x]) + row_right.append(map_y_r[y, x]) + row_right.append(map_x_r[y, x]) + if (map_x_l.shape[1] % meshCellSize) % 2 != 0: + row_left.append(0) + row_left.append(0) + row_right.append(0) + row_right.append(0) + + mesh_left.append(row_left) + mesh_right.append(row_right) + + mesh_left = np.array(mesh_left) + mesh_right = np.array(mesh_right) + left_mesh_fpath = str(curr_path) + '/../resources/left_mesh.calib' + right_mesh_fpath = str(curr_path) + '/../resources/right_mesh.calib' + mesh_left.tofile(left_mesh_fpath) + mesh_right.tofile(right_mesh_fpath) diff --git a/requirements.txt b/requirements.txt index 3ef16e7d9..5f988ce7e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,4 +10,5 @@ opencv-contrib-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machi --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -depthai==2.17.1.0 +# depthai==2.17.1.0 +depthai==2.17.2.0.dev+211dc1c0a135d5615dba75ebf2098ba8cdc05cd0 \ No newline at end of file diff --git a/resources/boards/ACME01.json b/resources/boards/ACME01.json deleted file mode 100644 index e94758047..000000000 --- a/resources/boards/ACME01.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "ACME01", - "revision": "V1.2", - "swap_left_and_right_cameras": false, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 - } -} - diff --git a/resources/boards/BW1097.json b/resources/boards/BW1097.json deleted file mode 100644 index 16c1546ca..000000000 --- a/resources/boards/BW1097.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-CM3", - "revision": "R2M2E3", - "swap_left_and_right_cameras": true, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 9.0, - "left_to_rgb_distance_cm": 2.0 - } -} - diff --git a/resources/boards/BW1098OBC.json b/resources/boards/BW1098OBC.json index 8a54bc8ad..672df86c2 100644 --- a/resources/boards/BW1098OBC.json +++ b/resources/boards/BW1098OBC.json @@ -3,10 +3,53 @@ { "name": "OAK-D", "revision": "R1M0E1", - "swap_left_and_right_cameras": true, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } } } + diff --git a/resources/boards/DM2097.json b/resources/boards/DM2097.json deleted file mode 100644 index 84a00b156..000000000 --- a/resources/boards/DM2097.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "board_config": - { - "name": "DM2097", - "revision": "R0M0E0", - "swap_left_and_right_cameras": false, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 9.0, - "left_to_rgb_distance_cm": 7.7 - } -} diff --git a/resources/boards/OAK-D-CM4-POE.json b/resources/boards/OAK-D-CM4-POE.json deleted file mode 100644 index b9c67c2e8..000000000 --- a/resources/boards/OAK-D-CM4-POE.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-CM4-POE", - "revision": "R2M1E2", - "swap_left_and_right_cameras": false, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 9.0, - "left_to_rgb_distance_cm": 7.7 - } -} diff --git a/resources/boards/OAK-D-CM4.json b/resources/boards/OAK-D-CM4.json deleted file mode 100644 index 4d976da08..000000000 --- a/resources/boards/OAK-D-CM4.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-CM4", - "revision": "R2M0E2", - "swap_left_and_right_cameras": true, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 9.0, - "left_to_rgb_distance_cm": 2.0 - } -} - diff --git a/resources/boards/OAK-D-IoT-40.json b/resources/boards/OAK-D-IoT-40.json deleted file mode 100644 index 23ba6912b..000000000 --- a/resources/boards/OAK-D-IoT-40.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-IoT-40", - "revision": "R2M2E2", - "swap_left_and_right_cameras": true, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 4.0, - "left_to_rgb_distance_cm": 3.0 - } -} diff --git a/resources/boards/OAK-D-LITE.json b/resources/boards/OAK-D-LITE.json deleted file mode 100644 index 03376e6f6..000000000 --- a/resources/boards/OAK-D-LITE.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-LITE", - "revision": "R0M0E0", - "left_fov_deg": 72.9, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 - } -} - diff --git a/resources/boards/OAK-D-PRO-W.json b/resources/boards/OAK-D-PRO-W.json deleted file mode 100644 index 6bf207525..000000000 --- a/resources/boards/OAK-D-PRO-W.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO-W", - "revision": "R3M1E3", - "swap_left_and_right_cameras": true, - "left_fov_deg": 127.0, - "rgb_fov_deg": 108.0, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 - } -} - diff --git a/resources/boards/OAK-D-PRO.json b/resources/boards/OAK-D-PRO.json deleted file mode 100644 index c73e744e9..000000000 --- a/resources/boards/OAK-D-PRO.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO", - "revision": "R3M1E3", - "swap_left_and_right_cameras": true, - "left_fov_deg": 71.86, - "rgb_fov_deg": 68.7938, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 - } -} - diff --git a/resources/boards/OAK-D-W.json b/resources/boards/OAK-D-W.json deleted file mode 100644 index e70e613da..000000000 --- a/resources/boards/OAK-D-W.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-W", - "revision": "R3M1E3", - "swap_left_and_right_cameras": true, - "left_fov_deg": 127.0, - "rgb_fov_deg": 108.0, - "left_to_right_distance_cm": 7.5, - "left_to_rgb_distance_cm": 3.75 - } -} - From 5113b6b7e63215a9509cb89ba941a99213d3574e Mon Sep 17 00:00:00 2001 From: Sachin Date: Tue, 9 Aug 2022 18:14:41 -0700 Subject: [PATCH 02/68] Working calibration with SYnc --- calibrate.py | 122 +++++++++++++++++++-------- depthai_helpers/calibration_utils.py | 2 + 2 files changed, 91 insertions(+), 33 deletions(-) diff --git a/calibrate.py b/calibrate.py index 83fe9e867..5debf08e9 100755 --- a/calibrate.py +++ b/calibrate.py @@ -1,19 +1,22 @@ #!/usr/bin/env python3 import argparse import json +from pydoc import render_doc import shutil import traceback from argparse import ArgumentParser from pathlib import Path import time from datetime import datetime, timedelta +from collections import deque import cv2 from cv2 import resize import depthai as dai import numpy as np +import copy -import depthai_helpers.calibration_utils_old as calibUtils +import depthai_helpers.calibration_utils as calibUtils font = cv2.FONT_HERSHEY_SIMPLEX debug = False @@ -169,7 +172,7 @@ def parse_args(): class HostSync: def __init__(self, deltaMilliSec): self.arrays = {} - # self.arraySize = arraySize + self.arraySize = 15 self.recentFrameTs = None self.deltaMilliSec = timedelta(milliseconds=deltaMilliSec) # self.synced = queue.Queue() @@ -179,23 +182,69 @@ def remove(self, t1): def add_msg(self, name, data, ts): if name not in self.arrays: - self.arrays[name] = [] + self.arrays[name] = deque(maxlen=self.arraySize) # Add msg to array - self.arrays[name].append({'data': data, 'timestamp': ts}) - if self.recentFrameTs == None or self.recentFrameTs < ts: + self.arrays[name].appendleft({'data': data, 'timestamp': ts}) + if self.recentFrameTs == None or self.recentFrameTs - ts: self.recentFrameTs = ts - - for name, arr in self.arrays.items(): - for i, obj in enumerate(arr): - if self.remove(obj['timestamp']): - arr.remove(obj) - else: break + print(len(self.arrays[name])) + # print(f'Added Msgs typ {name}') + print(ts) + # for name, arr in self.arrays.items(): + # for i, obj in enumerate(arr): + # if self.remove(obj['timestamp']): + # arr.remove(obj) + # else: break - + def clearQueues(self): + print('Clearing Queues...') + for name, msgList in self.arrays.items(): + self.arrays[name].clear() + print(len(self.arrays[name])) + def get_synced(self): synced = {} - - for name, arr in self.arrays.items(): + for name, msgList in self.arrays.items(): + # print('len(pivotM---------sgList)') + # print(len(pivotMsgList)) + + if len(msgList) != self.arraySize: + return False + + for name, pivotMsgList in self.arrays.items(): + print('len(pivotMsgList)') + print(len(pivotMsgList)) + pivotMsgListDuplicate = pivotMsgList + while pivotMsgListDuplicate: + currPivot = pivotMsgListDuplicate.popleft() + synced[name] = currPivot['data'] + + for subName, msgList in self.arrays.items(): + print(f'len of {subName}') + print(len(msgList)) + if name == subName: + continue + msgListDuplicate = msgList.copy() + while msgListDuplicate: + print(f'---len of dup {subName} is {len(msgListDuplicate)}') + currMsg = msgListDuplicate.popleft() + time_diff = abs(currMsg['timestamp'] - currPivot['timestamp']) + print(f'---Time diff is {time_diff} and delta is {self.deltaMilliSec}') + if time_diff < self.deltaMilliSec: + print(f'--------Adding {subName} to sync. Messages left is {len(msgListDuplicate)}') + synced[subName] = currMsg['data'] + break + print(f'Size of Synced is {len(synced)} amd array size is {len(self.arrays)}') + if len(synced) == len(self.arrays): + self.clearQueues() + return synced + + # raise SystemExit(1) + self.clearQueues() + return False + + + """ for name, arr in self.arrays.items(): for i, obj in enumerate(arr): time_diff = abs(obj['timestamp'] - self.recentFrameTs) print("Time diff for {0} is {1} milliseconds".format(name ,time_diff.total_seconds() * 1000)) @@ -213,7 +262,7 @@ def get_synced(self): arr.remove(obj) else: break return synced - return False + return False """ class Main: output_scale_factor = 0.5 @@ -393,7 +442,7 @@ def show(position, text): def show_failed_capture_frame(self): width, height = int( self.width * self.output_scale_factor), int(self.height * self.output_scale_factor) - info_frame = np.zeros((height, width, 3), np.uint8) + info_frame = np.zeros((self.height, self.width, 3), np.uint8) print("py: Capture failed, unable to find chessboard! Fix position and press spacebar again") def show(position, text): @@ -448,13 +497,14 @@ def capture_images_sync(self): curr_time = None self.display_name = "Image Window" - syncCollector = HostSync(20) + syncCollector = HostSync(60) while not finished: currImageList = {} for key in self.camera_queue.keys(): frameMsg = self.camera_queue[key].get() - print(f'key is {key}') + # print(f'Timestamp of {key} is {frameMsg.getTimestamp()}') + syncCollector.add_msg(key, frameMsg, frameMsg.getTimestamp()) gray_frame = None if frameMsg.getType() == dai.RawImgFrame.Type.RAW8: @@ -462,19 +512,17 @@ def capture_images_sync(self): else: gray_frame = cv2.cvtColor(frameMsg.getCvFrame(), cv2.COLOR_BGR2GRAY) currImageList[key] = gray_frame - print(gray_frame.shape) + # print(gray_frame.shape) resizeHeight = 0 resizeWidth = 0 for name, imgFrame in currImageList.items(): - print(f'original Shape of {name} is {imgFrame.shape}' ) + # print(f'original Shape of {name} is {imgFrame.shape}' ) currImageList[name] = cv2.resize( imgFrame, (0, 0), fx=self.output_scale_factor, fy=self.output_scale_factor) height, width = currImageList[name].shape - - print(f'resized Shape of {name} is {width}x{height}' ) widthRatio = resizeWidth / width heightRatio = resizeHeight / height @@ -495,7 +543,7 @@ def capture_images_sync(self): # if height > resizeHeight: # resizeHeight = height - print(f'Scale Shape is {resizeWidth}x{resizeHeight}' ) + # print(f'Scale Shape is {resizeWidth}x{resizeHeight}' ) combinedImage = None for name, imgFrame in currImageList.items(): @@ -504,7 +552,7 @@ def capture_images_sync(self): imgFrame = cv2.resize( imgFrame, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width) - print(f'final_scaledImageSize is {imgFrame.shape}') + # print(f'final_scaledImageSize is {imgFrame.shape}') if self.polygons is None: self.height, self.width = imgFrame.shape print(self.height, self.width) @@ -559,11 +607,14 @@ def capture_images_sync(self): cv2.imshow(self.display_name, combinedImage) tried = {} allPassed = True + if capturing: syncedMsgs = syncCollector.get_synced() if syncedMsgs == False: - continue # - for name, frameMsg in syncedMsgs: + for key in self.camera_queue.keys(): + self.camera_queue[key].getAll() + continue + for name, frameMsg in syncedMsgs.items(): tried[name] = self.parse_frame(frameMsg.getCvFrame(), name) allPassed = allPassed and tried[name] @@ -577,12 +628,14 @@ def capture_images_sync(self): self.images_captured += 1 self.images_captured_polygon += 1 + capturing = False else: self.show_failed_capture_frame() + capturing = False - print(f'self.images_captured_polygon {self.images_captured_polygon}') - print(f'self.current_polygon {self.current_polygon}') - print(f'len(self.polygons) {len(self.polygons)}') + # print(f'self.images_captured_polygon {self.images_captured_polygon}') + # print(f'self.current_polygon {self.current_polygon}') + # print(f'len(self.polygons) {len(self.polygons)}') if self.images_captured_polygon == self.args.count: self.images_captured_polygon = 0 @@ -805,6 +858,8 @@ def calibrate(self): try: # stereo_calib = StereoCalibration() + print("Starting image processingxxccx") + print(self.args.squaresX) status, result_config = stereo_calib.calibrate( self.board_config, self.dataset_path, @@ -828,13 +883,14 @@ def calibrate(self): reprojection_error_threshold = reprojection_error_threshold * cam_info['size'][1] / 720 if cam_info['name'] == 'rgb': - reprojection_error_threshold = 6 + reprojection_error_threshold = 3 print('Reprojection error threshold -> {}'.format(reprojection_error_threshold)) if cam_info['reprojection_error'] > reprojection_error_threshold: color = red error_text.append("high Reprojection Error") text = cam_info['name'] + ' Reprojection Error: ' + format(cam_info['reprojection_error'], '.6f') + print(text) # pygame_render_text(self.screen, text, (vis_x, vis_y), color, 30) calibration_handler.setDistortionCoefficients(stringToCam[camera], cam_info['dist_coeff']) @@ -842,7 +898,7 @@ def calibrate(self): calibration_handler.setFov(stringToCam[camera], cam_info['hfov']) if cam_info['hasAutofocus']: - calibration_handler.setLensPosition(stringToCam[camera], self.lensPosition[cam_info['name']]) + calibration_handler.setLensPosition(stringToCam[camera], self.focus_value) # log_list.append(self.focusSigma[cam_info['name']]) # log_list.append(cam_info['reprojection_error']) @@ -877,7 +933,7 @@ def calibrate(self): if len(error_text) == 0: print('Flashing Calibration data into ') - print(calib_dest_path) + # print(calib_dest_path) eeepromData = calibration_handler.getEepromData() print(f'EEPROM VERSION being flashed is -> {eeepromData.version}') @@ -937,7 +993,7 @@ def calibrate(self): print(error_text) for text in error_text: # text = error_text[0] - resImage = create_blank(900, 512, rgb_color=false) + resImage = create_blank(900, 512, rgb_color=red) cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2) cv2.imshow("Result Image", resImage) cv2.waitKey(0) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index dadf3821c..8767e4cbe 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -93,10 +93,12 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ """Function to calculate calibration for stereo camera.""" start_time = time.time() # init object data + print(f'squareX is {squaresX}') self.enable_rectification_disp = enable_disp_rectify self.cameraModel = camera_model self.data_path = filepath self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) + self.board = aruco.CharucoBoard_create( # 22, 16, squaresX, squaresY, From fcc4f4c715b838d277588a1c0746008d6f19d370 Mon Sep 17 00:00:00 2001 From: Sachin Date: Wed, 10 Aug 2022 09:08:22 -0700 Subject: [PATCH 03/68] added board config files --- resources/boards/BW1097.json | 54 +++++++++++++++++++ resources/boards/DM1092.json | 55 +++++++++++++++++++ resources/boards/DM1097.json | 54 +++++++++++++++++++ resources/boards/DM1098OAKD_WIFI.json | 55 +++++++++++++++++++ resources/boards/OAK-1-LITE-W.json | 14 +++++ resources/boards/OAK-1-LITE.json | 14 +++++ resources/boards/OAK-1-MAX.json | 14 +++++ resources/boards/OAK-1-POE.json | 16 ++++++ resources/boards/OAK-1-W.json | 14 +++++ resources/boards/OAK-1.json | 14 +++++ resources/boards/OAK-D-LITE.json | 56 ++++++++++++++++++++ resources/boards/OAK-D-POE-W.json | 55 +++++++++++++++++++ resources/boards/OAK-D-POE.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-POE-CUSTOM.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-POE.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-W-CUSTOM.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-W-POE-CUSTOM.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-W-POE.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO-W.json | 55 +++++++++++++++++++ resources/boards/OAK-D-PRO.json | 55 +++++++++++++++++++ resources/boards/OAK-D-S2.json | 55 +++++++++++++++++++ resources/boards/OAK-D-W.json | 55 +++++++++++++++++++ 22 files changed, 965 insertions(+) create mode 100644 resources/boards/BW1097.json create mode 100644 resources/boards/DM1092.json create mode 100644 resources/boards/DM1097.json create mode 100644 resources/boards/DM1098OAKD_WIFI.json create mode 100644 resources/boards/OAK-1-LITE-W.json create mode 100644 resources/boards/OAK-1-LITE.json create mode 100644 resources/boards/OAK-1-MAX.json create mode 100644 resources/boards/OAK-1-POE.json create mode 100644 resources/boards/OAK-1-W.json create mode 100644 resources/boards/OAK-1.json create mode 100644 resources/boards/OAK-D-LITE.json create mode 100644 resources/boards/OAK-D-POE-W.json create mode 100644 resources/boards/OAK-D-POE.json create mode 100644 resources/boards/OAK-D-PRO-POE-CUSTOM.json create mode 100644 resources/boards/OAK-D-PRO-POE.json create mode 100644 resources/boards/OAK-D-PRO-W-CUSTOM.json create mode 100644 resources/boards/OAK-D-PRO-W-POE-CUSTOM.json create mode 100644 resources/boards/OAK-D-PRO-W-POE.json create mode 100644 resources/boards/OAK-D-PRO-W.json create mode 100644 resources/boards/OAK-D-PRO.json create mode 100644 resources/boards/OAK-D-S2.json create mode 100644 resources/boards/OAK-D-W.json diff --git a/resources/boards/BW1097.json b/resources/boards/BW1097.json new file mode 100644 index 000000000..b7d6723b6 --- /dev/null +++ b/resources/boards/BW1097.json @@ -0,0 +1,54 @@ +{ + "board_config": + { + "name": "OAK-D-CM3", + "revision": "R2M2E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -9.0, + "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": 7, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} diff --git a/resources/boards/DM1092.json b/resources/boards/DM1092.json new file mode 100644 index 000000000..ac029a371 --- /dev/null +++ b/resources/boards/DM1092.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-IoT-40", + "revision": "R2M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -4, + "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": 3, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/DM1097.json b/resources/boards/DM1097.json new file mode 100644 index 000000000..1b302f5ea --- /dev/null +++ b/resources/boards/DM1097.json @@ -0,0 +1,54 @@ +{ + "board_config": + { + "name": "OAK-D-CM4", + "revision": "R2M0E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -9.0, + "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": 7, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} diff --git a/resources/boards/DM1098OAKD_WIFI.json b/resources/boards/DM1098OAKD_WIFI.json new file mode 100644 index 000000000..7f7d7a1de --- /dev/null +++ b/resources/boards/DM1098OAKD_WIFI.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-IoT-75", + "revision": "R0M0E0", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-1-LITE-W.json b/resources/boards/OAK-1-LITE-W.json new file mode 100644 index 000000000..24cac5786 --- /dev/null +++ b/resources/boards/OAK-1-LITE-W.json @@ -0,0 +1,14 @@ +{ + "board_config": + { + "name": "OAK-1-Lite-W", + "revision": "R3M1E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + } + } + } +} \ No newline at end of file diff --git a/resources/boards/OAK-1-LITE.json b/resources/boards/OAK-1-LITE.json new file mode 100644 index 000000000..ab97c8889 --- /dev/null +++ b/resources/boards/OAK-1-LITE.json @@ -0,0 +1,14 @@ +{ + "board_config": + { + "name": "OAK-1-LITE", + "revision": "R1M0E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + } + } + } +} \ No newline at end of file diff --git a/resources/boards/OAK-1-MAX.json b/resources/boards/OAK-1-MAX.json new file mode 100644 index 000000000..14be62a8a --- /dev/null +++ b/resources/boards/OAK-1-MAX.json @@ -0,0 +1,14 @@ +{ + "board_config": + { + "name": "OAK-1-MAX", + "revision": "R3M1E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + } + } + } +} \ No newline at end of file diff --git a/resources/boards/OAK-1-POE.json b/resources/boards/OAK-1-POE.json new file mode 100644 index 000000000..c682b0c97 --- /dev/null +++ b/resources/boards/OAK-1-POE.json @@ -0,0 +1,16 @@ +{ + "board_config": + { + "name": "OAK-1-POE", + "revision": "R2M1E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + } + } + + } +} + diff --git a/resources/boards/OAK-1-W.json b/resources/boards/OAK-1-W.json new file mode 100644 index 000000000..6dab92b13 --- /dev/null +++ b/resources/boards/OAK-1-W.json @@ -0,0 +1,14 @@ +{ + "board_config": + { + "name": "OAK-1-W", + "revision": "R3M1E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + } + } + } +} \ No newline at end of file diff --git a/resources/boards/OAK-1.json b/resources/boards/OAK-1.json new file mode 100644 index 000000000..2bbf8de9f --- /dev/null +++ b/resources/boards/OAK-1.json @@ -0,0 +1,14 @@ +{ + "board_config": + { + "name": "OAK-1", + "revision": "R3M1E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + } + } + } +} \ No newline at end of file diff --git a/resources/boards/OAK-D-LITE.json b/resources/boards/OAK-D-LITE.json new file mode 100644 index 000000000..483ca6c8d --- /dev/null +++ b/resources/boards/OAK-D-LITE.json @@ -0,0 +1,56 @@ +{ + "board_config": + { + "name": "OAK-D-LITE", + "revision": "R1M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 72.9, + "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": 72.9, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + + diff --git a/resources/boards/OAK-D-POE-W.json b/resources/boards/OAK-D-POE-W.json new file mode 100644 index 000000000..a81caf948 --- /dev/null +++ b/resources/boards/OAK-D-POE-W.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D POE-W", + "revision": "R3M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-POE.json b/resources/boards/OAK-D-POE.json new file mode 100644 index 000000000..b9d48c662 --- /dev/null +++ b/resources/boards/OAK-D-POE.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-POE", + "revision": "R3M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-POE-CUSTOM.json b/resources/boards/OAK-D-PRO-POE-CUSTOM.json new file mode 100644 index 000000000..502cab780 --- /dev/null +++ b/resources/boards/OAK-D-PRO-POE-CUSTOM.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-PRO-POE-CUSTOM", + "revision": "R3M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 71.86, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-POE.json b/resources/boards/OAK-D-PRO-POE.json new file mode 100644 index 000000000..f68487d30 --- /dev/null +++ b/resources/boards/OAK-D-PRO-POE.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-PRO-POE", + "revision": "R3M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-W-CUSTOM.json b/resources/boards/OAK-D-PRO-W-CUSTOM.json new file mode 100644 index 000000000..742417bcb --- /dev/null +++ b/resources/boards/OAK-D-PRO-W-CUSTOM.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D Pro-W", + "revision": "R3M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 127.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json b/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json new file mode 100644 index 000000000..7cb811ffa --- /dev/null +++ b/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-PRO-W-POE", + "revision": "R3M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 127.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-W-POE.json b/resources/boards/OAK-D-PRO-W-POE.json new file mode 100644 index 000000000..3f576b2c8 --- /dev/null +++ b/resources/boards/OAK-D-PRO-W-POE.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D Pro-W PoE", + "revision": "R3M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO-W.json b/resources/boards/OAK-D-PRO-W.json new file mode 100644 index 000000000..b50c56a3f --- /dev/null +++ b/resources/boards/OAK-D-PRO-W.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D Pro-W", + "revision": "R3M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-PRO.json b/resources/boards/OAK-D-PRO.json new file mode 100644 index 000000000..8a4cfe6f8 --- /dev/null +++ b/resources/boards/OAK-D-PRO.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-PRO", + "revision": "R3M1E3", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-S2.json b/resources/boards/OAK-D-S2.json new file mode 100644 index 000000000..9edbf341e --- /dev/null +++ b/resources/boards/OAK-D-S2.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-S2", + "revision": "R2M0E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 68.7938, + "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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + diff --git a/resources/boards/OAK-D-W.json b/resources/boards/OAK-D-W.json new file mode 100644 index 000000000..2d0e63a57 --- /dev/null +++ b/resources/boards/OAK-D-W.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "OAK-D-W", + "revision": "R3M2E2", + "cameras":{ + "CAM_A": { + "name": "rgb", + "hfov": 108.0, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 127.0, + "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": 127.0, + "type": "mono", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + From e52e7345ce4fbd05a5571c09432a794241c83160 Mon Sep 17 00:00:00 2001 From: Sachin Date: Mon, 15 Aug 2022 10:07:31 -0700 Subject: [PATCH 04/68] Updated the ar0234 res --- calibrate.py | 3 +- resources/boards/AR0234DEV.json | 55 +++++++++++++++++++++++++++++++++ 2 files changed, 57 insertions(+), 1 deletion(-) create mode 100644 resources/boards/AR0234DEV.json diff --git a/calibrate.py b/calibrate.py index 5debf08e9..113dcac33 100755 --- a/calibrate.py +++ b/calibrate.py @@ -71,7 +71,8 @@ 'IMX378' : dai.ColorCameraProperties.SensorResolution.THE_4_K, 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, 'OV9*82' : dai.ColorCameraProperties.SensorResolution.THE_800_P, - 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP + 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP, + 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P } def create_blank(width, height, rgb_color=(0, 0, 0)): diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json new file mode 100644 index 000000000..b1e9b0d16 --- /dev/null +++ b/resources/boards/AR0234DEV.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "AR0234DEV", + "revision": "R1M0E1", + "cameras":{ + "CAM_A": { + "name": "below", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "color", + "extrinsics": { + "to_cam": "CAM_C", + "specTranslation": { + "x": -4.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": 3.75, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + From 562d331be0d10428905b81a43fd358eb0fd08c99 Mon Sep 17 00:00:00 2001 From: Sachin Date: Mon, 15 Aug 2022 13:26:43 -0700 Subject: [PATCH 05/68] Updated mono in json --- resources/boards/AR0234DEV.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index b1e9b0d16..e2b7575d7 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -30,7 +30,7 @@ "CAM_C": { "name": "right", "hfov": 71.86, - "type": "mono", + "type": "color", "extrinsics": { "to_cam": "CAM_A", "specTranslation": { From d9105b11d526d5c9a02bbd462e25bdd9421e117a Mon Sep 17 00:00:00 2001 From: Sachin Date: Mon, 15 Aug 2022 13:40:01 -0700 Subject: [PATCH 06/68] Added Output Scale factor and removed the orientation check --- calibrate.py | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/calibrate.py b/calibrate.py index 113dcac33..cbdf9b6d2 100755 --- a/calibrate.py +++ b/calibrate.py @@ -156,7 +156,9 @@ def parse_args(): parser.add_argument("-d", "--debug", default=False, action="store_true", help="Enable debug logs.") parser.add_argument("-fac", "--factoryCalibration", default=False, action="store_true", 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.") + options = parser.parse_args() # Set some extra defaults, `-brd` would override them @@ -275,9 +277,10 @@ class Main: images_captured = 0 def __init__(self): - global debug + global debug, output_scale_factor self.args = parse_args() debug = self.args.debug + output_scale_factor = self.args.outputScaleFactor self.aruco_dictionary = cv2.aruco.Dictionary_get( cv2.aruco.DICT_4X4_1000) self.focus_value = self.args.rgbLensPosition @@ -624,8 +627,8 @@ def capture_images_sync(self): leftStereo = self.board_config['cameras'][self.board_config['stereo_config']['left_cam']]['name'] rightStereo = self.board_config['cameras'][self.board_config['stereo_config']['right_cam']]['name'] print(f'Left Camera of stereo is {leftStereo} and right Camera of stereo is {rightStereo}') - if not self.test_camera_orientation(syncedMsgs[leftStereo].getCvFrame(), syncedMsgs[rightStereo].getCvFrame()): - self.show_failed_orientation() + # if not self.test_camera_orientation(syncedMsgs[leftStereo].getCvFrame(), syncedMsgs[rightStereo].getCvFrame()): + # self.show_failed_orientation() self.images_captured += 1 self.images_captured_polygon += 1 @@ -650,16 +653,6 @@ def capture_images_sync(self): - - - - - - - - - - def capture_images(self): finished = False capturing = False From 48cdb67ece018e8e67aa62cfab1719a3d9298bd8 Mon Sep 17 00:00:00 2001 From: Sachin Date: Mon, 15 Aug 2022 13:49:10 -0700 Subject: [PATCH 07/68] Fixed the osf --- calibrate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/calibrate.py b/calibrate.py index cbdf9b6d2..86cb6017d 100755 --- a/calibrate.py +++ b/calibrate.py @@ -277,10 +277,10 @@ class Main: images_captured = 0 def __init__(self): - global debug, output_scale_factor + global debug self.args = parse_args() debug = self.args.debug - output_scale_factor = self.args.outputScaleFactor + self.output_scale_factor = self.args.outputScaleFactor self.aruco_dictionary = cv2.aruco.Dictionary_get( cv2.aruco.DICT_4X4_1000) self.focus_value = self.args.rgbLensPosition From f177b010b9c2bff4cbfa31a728d499dcebf625bb Mon Sep 17 00:00:00 2001 From: Sachin Date: Mon, 15 Aug 2022 15:53:44 -0700 Subject: [PATCH 08/68] Fixed capture issue with different names --- .gitignore | 2 +- calibrate.py | 15 +++++++-------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 0dc2c6715..ddcc135ce 100644 --- a/.gitignore +++ b/.gitignore @@ -30,7 +30,7 @@ share/python-wheels/ MANIFEST *.idea # DepthAI-Specific -dataset/ +dataset* .fw_cache/ depthai.calib mesh_left.calib diff --git a/calibrate.py b/calibrate.py index 86cb6017d..cffc0f462 100755 --- a/calibrate.py +++ b/calibrate.py @@ -190,9 +190,9 @@ def add_msg(self, name, data, ts): self.arrays[name].appendleft({'data': data, 'timestamp': ts}) if self.recentFrameTs == None or self.recentFrameTs - ts: self.recentFrameTs = ts - print(len(self.arrays[name])) + # print(len(self.arrays[name])) # print(f'Added Msgs typ {name}') - print(ts) + # print(ts) # for name, arr in self.arrays.items(): # for i, obj in enumerate(arr): # if self.remove(obj['timestamp']): @@ -649,8 +649,6 @@ def capture_images_sync(self): finished = True cv2.destroyAllWindows() break - - def capture_images(self): @@ -863,6 +861,7 @@ def calibrate(self): self.args.squaresY, self.args.cameraMode, self.args.rectifiedDisp) # Turn off enable disp rectify + calibration_handler = self.device.readCalibration2() error_text = [] @@ -1000,10 +999,10 @@ def run(self): try: if Path('dataset').exists(): shutil.rmtree('dataset/') - Path("dataset/left").mkdir(parents=True, exist_ok=True) - Path("dataset/right").mkdir(parents=True, exist_ok=True) - if not self.args.disableRgb: - Path("dataset/rgb").mkdir(parents=True, exist_ok=True) + 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) + except OSError: traceback.print_exc() print("An error occurred trying to create image dataset directories!") From 507ede94da8c003e589eabeb9bd3693fdd2a53d1 Mon Sep 17 00:00:00 2001 From: Sachin Date: Tue, 16 Aug 2022 09:27:56 -0700 Subject: [PATCH 09/68] updated links translation --- resources/boards/AR0234DEV.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index e2b7575d7..724a69e22 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -16,7 +16,7 @@ "extrinsics": { "to_cam": "CAM_C", "specTranslation": { - "x": -4.5, + "x": -7.5, "y": 0, "z": 0 }, @@ -34,8 +34,8 @@ "extrinsics": { "to_cam": "CAM_A", "specTranslation": { - "x": 3.75, - "y": 0, + "x": 7.5, + "y": 3.25, "z": 0 }, "rotation":{ From 46f8b1ffe53dd3a16efc3074f7da96e41a4fb0f1 Mon Sep 17 00:00:00 2001 From: Sachin Date: Wed, 17 Aug 2022 12:16:51 -0700 Subject: [PATCH 10/68] WIP structure and vertical Stereo calibration --- depthai_helpers/calibration_utils.py | 59 +++++++++++++++++++------ resources/boards/AR0234DEV copy.json | 66 ++++++++++++++++++++++++++++ resources/boards/AR0234DEVFIX.json | 56 +++++++++++++++++++++++ 3 files changed, 168 insertions(+), 13 deletions(-) create mode 100644 resources/boards/AR0234DEV copy.json create mode 100644 resources/boards/AR0234DEVFIX.json diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 8767e4cbe..ff977f9a5 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -157,15 +157,23 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ board_config['stereo_config']['rectification_left'] = extrinsics[3] board_config['stereo_config']['rectification_right'] = extrinsics[4] - left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] - left_cam_info['extrinsics']['translation'] = extrinsics[2] - left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] + """ for stereoObj in board_config['stereo_config']: + + if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1: + stereoObj['rectification_left'] = extrinsics[3] + stereoObj['rectification_right'] = extrinsics[4] """ print('<-------------Epipolar error of {} and {} ------------>'.format( left_cam_info['name'], right_cam_info['name'])) left_cam_info['extrinsics']['epipolar_error'] = self.test_epipolar_charuco( - left_path, right_path, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], extrinsics[3], extrinsics[4]) + left_path, right_path, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], extrinsics[2], extrinsics[3], extrinsics[4]) + + + left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] + left_cam_info['extrinsics']['translation'] = extrinsics[2] + left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] + return 1, board_config def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): @@ -501,7 +509,8 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # obj_pts, left_corners_sampled, right_corners_sampled, # cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, # criteria=stereocalib_criteria, flags=flags) - + # if np.absolute(T[1]) > 0.2: + R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix_l, distCoeff_l, @@ -547,21 +556,36 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz return [ret, R, T, R_l, R_r] - def display_rectification(self, image_data_pairs): + def display_rectification(self, image_data_pairs, isHorizontal): print( "Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") for image_data_pair in image_data_pairs: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + if isHorizontal: + img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + else: + img_concat = cv2.vconcat([image_data_pair[0], image_data_pair[1]]) img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) # draw epipolar lines for debug purposes + line_row = 0 - while line_row < img_concat.shape[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 + + + img_concat = cv2.resize( + img_concat, (0, 0), fx=0.4, fy=0.4) + # show image cv2.imshow('Stereo Pair', img_concat) k = cv2.waitKey(0) @@ -603,14 +627,17 @@ def scale_image(self, img, scaled_res): else: return img - def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, r_l, r_r): + def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r): images_left = glob.glob(left_img_pth + '/*.png') images_right = glob.glob(right_img_pth + '/*.png') images_left.sort() 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.2: + isHorizontal = False + scale = None scale_req = False frame_left_shape = cv2.imread(images_left[0], 0).shape @@ -739,7 +766,10 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, imgpoints_r.extend(corners_r) epi_error_sum = 0 for l_pt, r_pt in zip(corners_l, corners_r): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + 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]) print("Average Epipolar Error per image on host in " + img_pth.name + " : " + str(epi_error_sum / len(corners_l))) @@ -749,13 +779,16 @@ 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): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) + 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 is : " + str(avg_epipolar)) if self.enable_rectification_disp: - self.display_rectification(image_data_pairs) + self.display_rectification(image_data_pairs, isHorizontal) return avg_epipolar diff --git a/resources/boards/AR0234DEV copy.json b/resources/boards/AR0234DEV copy.json new file mode 100644 index 000000000..bf70c8cb9 --- /dev/null +++ b/resources/boards/AR0234DEV copy.json @@ -0,0 +1,66 @@ +{ + "board_config": + { + "name": "AR0234DEV", + "revision": "R1M0E1", + "cameras":{ + "CAM_A": { + "name": "below", + "hfov": 68.7938, + "type": "color" + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "color", + "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": "color", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 7.5, + "y": 3.25, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + } + }, + "stereo_config": + [ + { + "left_cam": "CAM_B", + "right_cam": "CAM_C", + "mode": "horizontal", + "main": 1 + }, + { + "left_cam": "CAM_B", + "right_cam": "CAM_A", + "mode": "vertical", + "main": 0 + } + ] + } +} + diff --git a/resources/boards/AR0234DEVFIX.json b/resources/boards/AR0234DEVFIX.json new file mode 100644 index 000000000..612ab2250 --- /dev/null +++ b/resources/boards/AR0234DEVFIX.json @@ -0,0 +1,56 @@ +{ + "board_config": + { + "name": "AR0234DEV", + "revision": "R1M0E1", + "cameras":{ + "CAM_C": { + "name": "right", + "hfov": 71.86, + "type": "color", + "extrinsics": { + "to_cam": "CAM_B", + "specTranslation": { + "x": 5.7, + "y": 0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_B": { + "name": "left", + "hfov": 71.86, + "type": "color", + "extrinsics": { + "to_cam": "CAM_A", + "specTranslation": { + "x": 0, + "y": 3.25, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_A": { + "name": "below", + "hfov": 68.7938, + "type": "color" + } + + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + From 03766f30f3b2884e2b51b2b00018a6a95c52f5ac Mon Sep 17 00:00:00 2001 From: saching13 Date: Fri, 19 Aug 2022 11:57:07 -0700 Subject: [PATCH 11/68] Fixed flashing issues --- calibrate.py | 10 ++++- requirements.txt | 1 + resources/boards/AR0234DEV copy.json | 66 ---------------------------- resources/boards/AR0234DEV.json | 25 ++++++----- resources/boards/AR0234DEVFIX.json | 56 ----------------------- 5 files changed, 23 insertions(+), 135 deletions(-) delete mode 100644 resources/boards/AR0234DEV copy.json delete mode 100644 resources/boards/AR0234DEVFIX.json diff --git a/calibrate.py b/calibrate.py index cffc0f462..86532fdfb 100755 --- a/calibrate.py +++ b/calibrate.py @@ -862,7 +862,15 @@ def calibrate(self): self.args.cameraMode, self.args.rectifiedDisp) # Turn off enable disp rectify - calibration_handler = self.device.readCalibration2() + calibration_handler = self.device.readCalibration() + try: + if self.empty_calibration(calibration_handler): + calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) + except: + print("Writing in except...") + calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) + + # calibration_handler.set error_text = [] for camera in result_config['cameras'].keys(): diff --git a/requirements.txt b/requirements.txt index 5f988ce7e..99bc66ac3 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,6 +6,7 @@ opencv-contrib-python==4.5.4.58 ; platform_machine != "aarch64" and platform_mac opencv-contrib-python==4.5.1.48 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version != "3.10" opencv-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" opencv-contrib-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" +scipy -e ./depthai_sdk --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" diff --git a/resources/boards/AR0234DEV copy.json b/resources/boards/AR0234DEV copy.json deleted file mode 100644 index bf70c8cb9..000000000 --- a/resources/boards/AR0234DEV copy.json +++ /dev/null @@ -1,66 +0,0 @@ -{ - "board_config": - { - "name": "AR0234DEV", - "revision": "R1M0E1", - "cameras":{ - "CAM_A": { - "name": "below", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 71.86, - "type": "color", - "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": "color", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 7.5, - "y": 3.25, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config": - [ - { - "left_cam": "CAM_B", - "right_cam": "CAM_C", - "mode": "horizontal", - "main": 1 - }, - { - "left_cam": "CAM_B", - "right_cam": "CAM_A", - "mode": "vertical", - "main": 0 - } - ] - } -} - diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index 724a69e22..612ab2250 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -4,19 +4,14 @@ "name": "AR0234DEV", "revision": "R1M0E1", "cameras":{ - "CAM_A": { - "name": "below", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", + "CAM_C": { + "name": "right", "hfov": 71.86, "type": "color", "extrinsics": { - "to_cam": "CAM_C", + "to_cam": "CAM_B", "specTranslation": { - "x": -7.5, + "x": 5.7, "y": 0, "z": 0 }, @@ -27,14 +22,14 @@ } } }, - "CAM_C": { - "name": "right", + "CAM_B": { + "name": "left", "hfov": 71.86, "type": "color", "extrinsics": { "to_cam": "CAM_A", "specTranslation": { - "x": 7.5, + "x": 0, "y": 3.25, "z": 0 }, @@ -44,7 +39,13 @@ "y": 0 } } + }, + "CAM_A": { + "name": "below", + "hfov": 68.7938, + "type": "color" } + }, "stereo_config":{ "left_cam": "CAM_B", diff --git a/resources/boards/AR0234DEVFIX.json b/resources/boards/AR0234DEVFIX.json deleted file mode 100644 index 612ab2250..000000000 --- a/resources/boards/AR0234DEVFIX.json +++ /dev/null @@ -1,56 +0,0 @@ -{ - "board_config": - { - "name": "AR0234DEV", - "revision": "R1M0E1", - "cameras":{ - "CAM_C": { - "name": "right", - "hfov": 71.86, - "type": "color", - "extrinsics": { - "to_cam": "CAM_B", - "specTranslation": { - "x": 5.7, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - }, - "CAM_B": { - "name": "left", - "hfov": 71.86, - "type": "color", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 0, - "y": 3.25, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - }, - "CAM_A": { - "name": "below", - "hfov": 68.7938, - "type": "color" - } - - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - From 84dd8995b9b30d8b8c947d4df7bcebbe477926ca Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 22 Aug 2022 09:06:37 -0700 Subject: [PATCH 12/68] updatec req condfitions --- requirements.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/requirements.txt b/requirements.txt index 99bc66ac3..fe1dd10b9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,8 +1,8 @@ requests==2.26.0 --extra-index-url https://www.piwheels.org/simple -opencv-python==4.5.4.58 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" +opencv-python==4.5.4.58 ; platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" opencv-python==4.5.1.48 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version != "3.10" -opencv-contrib-python==4.5.4.58 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" +opencv-contrib-python==4.5.4.58 ; platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" opencv-contrib-python==4.5.1.48 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version != "3.10" opencv-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" opencv-contrib-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" From 16f6e283114c6adafa38d38ff964cb4f11827a01 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 22 Aug 2022 09:20:38 -0700 Subject: [PATCH 13/68] added exception msg --- calibrate.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index 86532fdfb..0592d5caf 100755 --- a/calibrate.py +++ b/calibrate.py @@ -946,8 +946,9 @@ def calibrate(self): try: self.device.flashCalibration2(calibration_handler) is_write_succesful = True - except RuntimeError: + except RuntimeError as e: is_write_succesful = False + print(e) print("Writing in except...") is_write_succesful = self.device.flashCalibration2(calibration_handler) From 7b811174bf9b62cb9763dc075f22e2bdfa4f18b5 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 22 Aug 2022 09:36:53 -0700 Subject: [PATCH 14/68] removed try catch and writing board names --- calibrate.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/calibrate.py b/calibrate.py index 0592d5caf..560585c40 100755 --- a/calibrate.py +++ b/calibrate.py @@ -867,8 +867,7 @@ def calibrate(self): if self.empty_calibration(calibration_handler): calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) except: - print("Writing in except...") - calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) + pass # calibration_handler.set error_text = [] @@ -943,14 +942,14 @@ def calibrate(self): mx_serial_id = self.device.getDeviceInfo().getMxId() calib_dest_path = dest_path + '/' + mx_serial_id + '.json' calibration_handler.eepromToJsonFile(calib_dest_path) - try: - self.device.flashCalibration2(calibration_handler) - is_write_succesful = True - except RuntimeError as e: - is_write_succesful = False - print(e) - print("Writing in except...") - is_write_succesful = self.device.flashCalibration2(calibration_handler) + # try: + self.device.flashCalibration2(calibration_handler) + is_write_succesful = True + # except RuntimeError as e: + # is_write_succesful = False + # print(e) + # print("Writing in except...") + # is_write_succesful = self.device.flashCalibration2(calibration_handler) if self.args.factoryCalibration: try: From 1b356df6fe4e98145bfedde8c01aad238ce31272 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 22 Aug 2022 13:52:00 -0700 Subject: [PATCH 15/68] adding rect data --- depthai_helpers/calibration_utils.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index ff977f9a5..d8cb5b1b0 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -156,6 +156,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: board_config['stereo_config']['rectification_left'] = extrinsics[3] board_config['stereo_config']['rectification_right'] = extrinsics[4] + elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam: + print('Adding rect data ------------------->') + board_config['stereo_config']['rectification_left'] = extrinsics[4] + board_config['stereo_config']['rectification_right'] = extrinsics[3] """ for stereoObj in board_config['stereo_config']: From 64d5828d191d54de4c410622c9b4d56d049a7348 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 22 Aug 2022 14:13:56 -0700 Subject: [PATCH 16/68] Updated rectification flash --- calibrate.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index 560585c40..83fb938fa 100755 --- a/calibrate.py +++ b/calibrate.py @@ -929,7 +929,9 @@ def calibrate(self): if result_config['stereo_config']['left_cam'] == camera and result_config['stereo_config']['right_cam'] == cam_info['extrinsics']['to_cam']: calibration_handler.setStereoLeft(stringToCam[camera], result_config['stereo_config']['rectification_left']) calibration_handler.setStereoRight(stringToCam[cam_info['extrinsics']['to_cam']], result_config['stereo_config']['rectification_right']) - + elif result_config['stereo_config']['left_cam'] == cam_info['extrinsics']['to_cam'] and result_config['stereo_config']['right_cam'] == camera: + calibration_handler.setStereoRight(stringToCam[camera], result_config['stereo_config']['rectification_right']) + calibration_handler.setStereoLeft(stringToCam[cam_info['extrinsics']['to_cam']], result_config['stereo_config']['rectification_left']) if len(error_text) == 0: print('Flashing Calibration data into ') From c9bc18e83c28845eef141193381ab6ac0f37f008 Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 24 Aug 2022 13:28:29 -0700 Subject: [PATCH 17/68] Removed comments --- depthai_helpers/calibration_utils.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index d8cb5b1b0..82cbab524 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -157,7 +157,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ board_config['stereo_config']['rectification_left'] = extrinsics[3] board_config['stereo_config']['rectification_right'] = extrinsics[4] elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam: - print('Adding rect data ------------------->') board_config['stereo_config']['rectification_left'] = extrinsics[4] board_config['stereo_config']['rectification_right'] = extrinsics[3] @@ -269,6 +268,7 @@ def calibrate_intrinsics(self, image_files, hfov): # (Height, width) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] else: + print('Fisheye--------------------------------------------------') ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_fisheye( allCorners, allIds, imsize[::-1]) # (Height, width) @@ -445,9 +445,10 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): print("Camera Matrix initialization.............") print(cameraMatrixInit) flags = 0 + flags = cv2.fisheye.CALIB_CHECK_COND distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) + cv2.TERM_CRITERIA_EPS, 10000, 1e-5) return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) @@ -543,9 +544,17 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # print(len(left_corners_sampled[i])) # print(len(right_corners_sampled[i])) flags = 0 - flags |= cv2.CALIB_FIX_INTRINSIC - flags |= cv2.CALIB_RATIONAL_MODEL - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(SACHIN): Try without intrinsic guess + # flags |= cv2.CALIB_FIX_INTRINSIC + # flags |= cv2.CALIB_RATIONAL_MODEL + flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess + flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(sACHIN): Try without intrinsic guess + + print('Fisyeye stereo model..................') + print(len(cameraMatrix_l)) + print(len(cameraMatrix_r)) + print(len(distCoeff_l)) + print(len(distCoeff_r)) + ret, M1, d1, M2, d2, R, T, E, F = cv2.fisheye.stereoCalibrate( obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, From 38e6102ab536c14a8b8c79638737cdb7929b7917 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 8 Sep 2022 13:47:08 -0400 Subject: [PATCH 18/68] Added rotation to polygons --- calibrate.py | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index 83fb938fa..61f18a0aa 100755 --- a/calibrate.py +++ b/calibrate.py @@ -9,6 +9,7 @@ import time from datetime import datetime, timedelta from collections import deque +from scipy.spatial.transform import Rotation import cv2 from cv2 import resize @@ -563,8 +564,32 @@ def capture_images_sync(self): self.polygons = calibUtils.setPolygonCoordinates( self.height, self.width) + localPolygon = np.array([self.polygons[self.current_polygon]]) + print(localPolygon.shape) + print(localPolygon) + if self.images_captured_polygon == 1: + # perspectiveRotationMatrix = Rotation.from_euler('z', 45, degrees=True).as_matrix() + angle = 30. + theta = (angle/180.) * np.pi + perspectiveRotationMatrix = np.array([[np.cos(theta), -np.sin(theta)], + [np.sin(theta), np.cos(theta)]]) + + localPolygon = np.matmul(localPolygon, perspectiveRotationMatrix).astype(np.int32) + localPolygon[0][:, 1] += abs(localPolygon.min()) + if self.images_captured_polygon == 2: + # perspectiveRotationMatrix = Rotation.from_euler('z', -45, degrees=True).as_matrix() + angle = -30. + theta = (angle/180.) * np.pi + perspectiveRotationMatrix = np.array([[np.cos(theta), -np.sin(theta)], + [np.sin(theta), np.cos(theta)]]) + localPolygon = np.matmul(localPolygon, perspectiveRotationMatrix).astype(np.int32) + localPolygon[0][:, 1] += (height - abs(localPolygon[0][:, 1].max())) + localPolygon[0][:, 0] += abs(localPolygon[0][:, 1].min()) + + print(localPolygon) + print(localPolygon.shape) cv2.polylines( - imgFrame, np.array([self.polygons[self.current_polygon]]), + imgFrame, localPolygon, True, (0, 0, 255), 4) # TODO(Sachin): Add this back with proper alignment From 861f9e3e706d1c3eb0af8b7a673dd6aed5940a15 Mon Sep 17 00:00:00 2001 From: Sachin Guruswamy <43363595+saching13@users.noreply.github.com> Date: Mon, 12 Sep 2022 11:08:53 -0700 Subject: [PATCH 19/68] Polygon flex (#789) * Optimization and trace levels introduced WIP * tweaked parameters and updated tracelevel --- calibrate.py | 2 + depthai_helpers/calibration_utils.py | 206 +++++++++++++++++---------- resources/boards/AR0234DEV.json | 6 +- 3 files changed, 136 insertions(+), 78 deletions(-) diff --git a/calibrate.py b/calibrate.py index 61f18a0aa..f8998c76f 100755 --- a/calibrate.py +++ b/calibrate.py @@ -10,6 +10,7 @@ from datetime import datetime, timedelta from collections import deque from scipy.spatial.transform import Rotation +import traceback import cv2 from cv2 import resize @@ -1027,6 +1028,7 @@ def calibrate(self): cv2.waitKey(0) except Exception as e: print(e) + print(traceback.format_exc()) raise SystemExit(1) def run(self): diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 82cbab524..482956a29 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -5,13 +5,13 @@ import os import shutil import numpy as np -from scipy.spatial.transform import Rotation as R -import re +from scipy.spatial.transform import Rotation import time import json import cv2.aruco as aruco from pathlib import Path # Creates a set of 13 polygon coordinates +traceLevel = 0 def setPolygonCoordinates(height, width): @@ -84,6 +84,7 @@ def polygon_from_image_name(image_name): class StereoCalibration(object): + global traceLevel """Class to Calculate Calibration and Rectify a Stereo Camera.""" def __init__(self): @@ -98,7 +99,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ self.cameraModel = camera_model self.data_path = filepath self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) - + self.board = aruco.CharucoBoard_create( # 22, 16, squaresX, squaresY, @@ -122,10 +123,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ cam_info['reprojection_error'] = ret print( '<------------Camera Name: {} ------------>'.format(cam_info['name'])) - print("Reprojection error of {0}: {1}".format(cam_info['name'], ret)) - print("intrinsics of {0}: \n {1}".format(cam_info['name'], intrinsics)) - # print(intrinsics) - # print(ret) + print("Reprojection error of {0}: {1}".format( + cam_info['name'], ret)) + print("Estimated intrinsics of {0}: \n {1}".format( + cam_info['name'], intrinsics)) for camera in board_config['cameras'].keys(): left_cam_info = board_config['cameras'][camera] @@ -145,7 +146,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ translation = np.array( [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32) - rotation = R.from_euler( + rotation = Rotation.from_euler( 'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32) extrinsics = self.calibrate_extrinsics(left_path, right_path, left_cam_info['intrinsics'], left_cam_info[ @@ -156,9 +157,13 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: board_config['stereo_config']['rectification_left'] = extrinsics[3] board_config['stereo_config']['rectification_right'] = extrinsics[4] + board_config['stereo_config']['p_left'] = extrinsics[5] + board_config['stereo_config']['p_right'] = extrinsics[6] elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam: board_config['stereo_config']['rectification_left'] = extrinsics[4] board_config['stereo_config']['rectification_right'] = extrinsics[3] + board_config['stereo_config']['p_left'] = extrinsics[6] + board_config['stereo_config']['p_right'] = extrinsics[5] """ for stereoObj in board_config['stereo_config']: @@ -169,14 +174,22 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ print('<-------------Epipolar error of {} and {} ------------>'.format( left_cam_info['name'], right_cam_info['name'])) left_cam_info['extrinsics']['epipolar_error'] = self.test_epipolar_charuco( - left_path, right_path, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], extrinsics[2], extrinsics[3], extrinsics[4]) - + left_path, + right_path, + left_cam_info['intrinsics'], + left_cam_info['dist_coeff'], + right_cam_info['intrinsics'], + right_cam_info['dist_coeff'], + extrinsics[2], # Translation between left and right Cameras + extrinsics[3], # Left Rectification rotation + extrinsics[4], # Right Rectification rotation + extrinsics[5], # Left Rectification Intrinsics + extrinsics[6]) # Right Rectification Intrinsics left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] left_cam_info['extrinsics']['translation'] = extrinsics[2] left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] - return 1, board_config def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): @@ -192,7 +205,7 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): # decimator = 0 # SUB PIXEL CORNER DETECTION CRITERION criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) count = 0 for im in images: # print("=> Processing image {0}".format(im)) @@ -228,8 +241,9 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): gray, self.aruco_dictionary) marker_corners, ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(gray, self.board, marker_corners, ids, rejectedCorners=rejectedImgPoints) - print('{0} number of Markers corners detected in the image {1}'.format( - len(marker_corners), img_pth.name)) + if 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: res2 = cv2.aruco.interpolateCornersCharuco( marker_corners, ids, gray, self.board) @@ -266,6 +280,10 @@ def calibrate_intrinsics(self, image_files, hfov): ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_charuco( allCorners, allIds, imsize[::-1], hfov) # (Height, width) + if traceLevel == 2: + self.fisheye_undistort_visualizaation( + image_files, camera_matrix, distortion_coefficients, imsize[::-1]) + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] else: print('Fisheye--------------------------------------------------') @@ -316,7 +334,8 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu 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( + 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)) @@ -353,10 +372,11 @@ def scale_intrinsics(self, intrinsics, originalShape, destShape): * scale - destShape[0]) / 2 scaled_intrinsics[0][2] -= (originalShape[1] * scale - destShape[1]) / 2 - print('original_intrinsics') - print(intrinsics) - print('scaled_intrinsics') - print(scaled_intrinsics) + if traceLevel == 1: + print('original_intrinsics') + print(intrinsics) + print('scaled_intrinsics') + print(scaled_intrinsics) return scaled_intrinsics @@ -366,8 +386,9 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): img = cv2.imread(im) # h, w = img.shape[:2] if self.cameraModel == 'perspective': + kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0.1) map1, map2 = cv2.initUndistortRectifyMap( - K, D, np.eye(3), K, img_size, cv2.CV_32FC1) + K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1) else: map1, map2 = cv2.fisheye.initUndistortRectifyMap( K, D, np.eye(3), K, img_size, cv2.CV_32FC1) @@ -375,8 +396,11 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): undistorted_img = cv2.remap( img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) cv2.imshow("undistorted", undistorted_img) - cv2.waitKey(0) - # cv2.destroyAllWindows() + 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 def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): """ @@ -405,13 +429,15 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): cameraMatrixInit = np.array([[3819.8801, 0.0, 1912.8375], [0.0, 3819.8801, 1135.3433], [0.0, 0.0, 1.]]) """ - - print("Camera Matrix initialization.............") - print(cameraMatrixInit) + if traceLevel == 1: + print( + f'Camera Matrix initialization with HFOV of {hfov} is.............') + print(cameraMatrixInit) distCoeffsInit = np.zeros((5, 1)) - flags = (cv2.CALIB_USE_INTRINSIC_GUESS + - cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO) + flags = (cv2.CALIB_USE_INTRINSIC_GUESS + + cv2.CALIB_RATIONAL_MODEL) + # flags = (cv2.CALIB_RATIONAL_MODEL) (ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, @@ -425,8 +451,9 @@ 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)) - print('Per View Errors...') - print(perViewErrors) + if 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): @@ -495,66 +522,72 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS # print(flags) - flags |= cv2.CALIB_FIX_INTRINSIC + # flags |= cv2.CALIB_FIX_INTRINSIC # flags |= cv2.CALIB_USE_INTRINSIC_GUESS flags |= cv2.CALIB_RATIONAL_MODEL # print(flags) - print('Printing Extrinsics guesses...') - print(r_in) - print(t_in) + if 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}') + + # ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate( # obj_pts, left_corners_sampled, right_corners_sampled, # cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, # criteria=stereocalib_criteria, flags=flags) # if np.absolute(T[1]) > 0.2: - + 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 # self.P_l = P_l # self.P_r = P_r + 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] + print(f'P_l is \n {P_l}') + print(f'P_r is \n {P_r}') + + return [ret, R, T, R_l, R_r, P_l, P_r] elif self.cameraModel == 'fisheye': - # print(len(obj_pts)) - # print('obj_pts') - # print(obj_pts) - # print(len(left_corners_sampled)) - # print('left_corners_sampled') - # print(left_corners_sampled) - # print(len(right_corners_sampled)) - # print('right_corners_sampled') - # print(right_corners_sampled) - # for i in range(len(obj_pts)): - # print('---------------------') - # print(i) - # print(len(obj_pts[i])) - # print(len(left_corners_sampled[i])) - # print(len(right_corners_sampled[i])) flags = 0 # flags |= cv2.CALIB_FIX_INTRINSIC # flags |= cv2.CALIB_RATIONAL_MODEL - flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess - flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(sACHIN): Try without intrinsic guess + # TODO(sACHIN): Try without intrinsic guess + flags |= cv2.CALIB_USE_INTRINSIC_GUESS + # TODO(sACHIN): Try without intrinsic guess + flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC print('Fisyeye stereo model..................') print(len(cameraMatrix_l)) print(len(cameraMatrix_r)) print(len(distCoeff_l)) print(len(distCoeff_r)) - + ret, M1, d1, M2, d2, R, T, E, F = cv2.fisheye.stereoCalibrate( obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, @@ -566,7 +599,7 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz cameraMatrix_r, distCoeff_r, imsize, R, T) - + return [ret, R, T, R_l, R_r] def display_rectification(self, image_data_pairs, isHorizontal): @@ -574,9 +607,11 @@ def display_rectification(self, image_data_pairs, isHorizontal): "Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") for image_data_pair in image_data_pairs: if isHorizontal: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) + img_concat = cv2.hconcat( + [image_data_pair[0], image_data_pair[1]]) else: - img_concat = cv2.vconcat([image_data_pair[0], image_data_pair[1]]) + img_concat = cv2.vconcat( + [image_data_pair[0], image_data_pair[1]]) img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) # draw epipolar lines for debug purposes @@ -595,10 +630,9 @@ def display_rectification(self, image_data_pairs, isHorizontal): (0, 255, 0), 1) line_col += 40 - img_concat = cv2.resize( - img_concat, (0, 0), fx=0.4, fy=0.4) - + img_concat, (0, 0), fx=0.4, fy=0.4) + # show image cv2.imshow('Stereo Pair', img_concat) k = cv2.waitKey(0) @@ -640,7 +674,7 @@ def scale_image(self, img, scaled_res): else: return img - def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r): + 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') images_left.sort() @@ -648,9 +682,9 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, 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.2: - isHorizontal = False - + if np.absolute(t[1]) > 0.2: + isHorizontal = False + scale = None scale_req = False frame_left_shape = cv2.imread(images_left[0], 0).shape @@ -665,7 +699,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, 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] @@ -673,7 +707,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, 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( + 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)) @@ -681,14 +716,23 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) + p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) + p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) - # print('Scaled Res :{}'.format(scaled_res)) + # TODO(Sachin): Observe Images by adding visualization + # TODO(Sachin): Check if the stetch is only in calibration Images + + kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_rp, d_l, scaled_res[::-1], 0) + kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_rp, d_r, scaled_res[::-1], 0) + mapx_l, mapy_l = cv2.initUndistortRectifyMap( - M_lp, d_l, r_l, M_rp, scaled_res[::-1], cv2.CV_32FC1) + 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, M_rp, scaled_res[::-1], cv2.CV_32FC1) + 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): @@ -716,7 +760,18 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR) image_data_pairs.append((img_l, img_r)) - + + if traceLevel == 2: + cv2.imshow("undistorted-Left", img_l) + cv2.imshow("undistorted-right", img_r) + # 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 + if traceLevel == 2: + cv2.destroyWindow("undistorted-left") + cv2.destroyWindow("undistorted-right") # compute metrics imgpoints_r = [] imgpoints_l = [] @@ -787,7 +842,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, print("Average Epipolar Error per image on host in " + img_pth.name + " : " + str(epi_error_sum / len(corners_l))) else: - print('Numer of corners is in left -> {} and right -> {}'.format(len(marker_corners_l), len(marker_corners_r))) + print('Numer of corners is in left -> {} and right -> {}'.format( + len(marker_corners_l), len(marker_corners_r))) return -1 epi_error_sum = 0 diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index 612ab2250..8315ef811 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -6,7 +6,7 @@ "cameras":{ "CAM_C": { "name": "right", - "hfov": 71.86, + "hfov": 140, "type": "color", "extrinsics": { "to_cam": "CAM_B", @@ -24,7 +24,7 @@ }, "CAM_B": { "name": "left", - "hfov": 71.86, + "hfov": 140, "type": "color", "extrinsics": { "to_cam": "CAM_A", @@ -42,7 +42,7 @@ }, "CAM_A": { "name": "below", - "hfov": 68.7938, + "hfov": 140, "type": "color" } From 00acebbc216b9e93719ffcaa1a910e39b3d4a5c7 Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 14 Sep 2022 16:19:03 -0400 Subject: [PATCH 20/68] Modified the board file --- resources/boards/AR0234DEV.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index 8315ef811..47825dfee 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -11,7 +11,7 @@ "extrinsics": { "to_cam": "CAM_B", "specTranslation": { - "x": 5.7, + "x": -7.5, "y": 0, "z": 0 }, @@ -30,7 +30,7 @@ "to_cam": "CAM_A", "specTranslation": { "x": 0, - "y": 3.25, + "y": -3.5, "z": 0 }, "rotation":{ From 63b5adb61d05e72235a4c7be985b915723688db4 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 15 Sep 2022 17:17:45 -0400 Subject: [PATCH 21/68] added debugs --- calibrate.py | 2 +- depthai_helpers/calibration_utils.py | 21 ++++++++++++++++----- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/calibrate.py b/calibrate.py index f8998c76f..ec5a8b373 100755 --- a/calibrate.py +++ b/calibrate.py @@ -118,7 +118,7 @@ def parse_args(): ''' parser = ArgumentParser( epilog=epilog_text, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("-c", "--count", default=1, type=int, required=False, + parser.add_argument("-c", "--count", default=3, type=int, required=False, help="Number of images per polygon to capture. Default: 1.") parser.add_argument("-s", "--squareSizeCm", type=float, required=True, help="Square size of calibration pattern used in centimeters. Default: 2.0cm.") diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 482956a29..7f6a874e6 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -11,7 +11,7 @@ import cv2.aruco as aruco from pathlib import Path # Creates a set of 13 polygon coordinates -traceLevel = 0 +traceLevel = 1 def setPolygonCoordinates(height, width): @@ -612,7 +612,7 @@ def display_rectification(self, image_data_pairs, isHorizontal): else: img_concat = cv2.vconcat( [image_data_pair[0], image_data_pair[1]]) - img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) + # img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) # draw epipolar lines for debug purposes @@ -775,6 +775,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # compute metrics imgpoints_r = [] imgpoints_l = [] + # new_imagePairs = [] for i, image_data_pair in enumerate(image_data_pairs): # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) marker_corners_l, ids_l, rejectedImgPoints = cv2.aruco.detectMarkers( @@ -820,7 +821,12 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, criteria=criteria) # termination criteria - img_pth = Path(images_right[i]) + 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])): @@ -838,13 +844,18 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, 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 Error per image on host in " + img_pth.name + " : " + - str(epi_error_sum / len(corners_l))) + print("Average Epipolar 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))) return -1 + lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) + rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) + image_data_pairs[i] = (lText, rText) + epi_error_sum = 0 for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): From 7372329ac37eb40ec06da25a33e96a8fc354f821 Mon Sep 17 00:00:00 2001 From: saching13 Date: Fri, 16 Sep 2022 21:28:41 -0400 Subject: [PATCH 22/68] tweaked intrinsics in use --- depthai_helpers/calibration_utils.py | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 7f6a874e6..19eab5c93 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -372,7 +372,7 @@ def scale_intrinsics(self, intrinsics, originalShape, destShape): * scale - destShape[0]) / 2 scaled_intrinsics[0][2] -= (originalShape[1] * scale - destShape[1]) / 2 - if traceLevel == 1: + if traceLevel == 2: print('original_intrinsics') print(intrinsics) print('scaled_intrinsics') @@ -568,8 +568,8 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) print(f'R_R Euler angles in XYZ {r_euler}') - print(f'P_l is \n {P_l}') - print(f'P_r is \n {P_r}') + # print(f'P_l is \n {P_l}') + # print(f'P_r is \n {P_r}') return [ret, R, T, R_l, R_r, P_l, P_r] @@ -716,18 +716,24 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) - p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) - p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) + # p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) + # p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) # TODO(Sachin): Observe Images by adding visualization # TODO(Sachin): Check if the stetch is only in calibration Images - - kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_rp, d_l, scaled_res[::-1], 0) - kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_rp, d_r, scaled_res[::-1], 0) - + print('Original intrinsics ....') + print(M_lp) + print(M_rp) + kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_lp, d_l, scaled_res[::-1], 0) + # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_lp, d_r, scaled_res[::-1], 0) + kScaledR = kScaledL + print('Intrinsics from the getOptimalNewCameraMatrix....') + print(kScaledL) + print(kScaledR) + 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( From 085b4a6f51c756757deaea11414a0d25d103b525 Mon Sep 17 00:00:00 2001 From: saching13 Date: Sat, 17 Sep 2022 00:15:47 -0400 Subject: [PATCH 23/68] iterative method added but commented --- depthai_helpers/calibration_utils.py | 190 ++++++++++++++++++++++++++- 1 file changed, 188 insertions(+), 2 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 19eab5c93..da5cb25ab 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -10,6 +10,7 @@ import json import cv2.aruco as aruco from pathlib import Path +from collections import deque # Creates a set of 13 polygon coordinates traceLevel = 1 @@ -673,6 +674,138 @@ def scale_image(self, img, scaled_res): 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') @@ -732,8 +865,61 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, kScaledR = kScaledL print('Intrinsics from the getOptimalNewCameraMatrix....') print(kScaledL) - print(kScaledR) - + # print(kScaledR) + oldEpipolarError = None + epQueue = deque() + movePos = True + # increments = 1 + if 0: + while True: + + epError = self.sgdEpipolar(images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal) + + if oldEpipolarError is None: + epQueue.append((epError, kScaledR)) + oldEpipolarError = epError + kScaledR[0][0] += 1 + kScaledR[1][1] += 1 + continue + if movePos: + if epError < oldEpipolarError: + epQueue.append((epError, kScaledR)) + oldEpipolarError = epError + kScaledR[0][0] += 1 + kScaledR[1][1] += 1 + else: + movePos = False + startPos = epQueue.popleft() + oldEpipolarError = startPos[0] + kScaledR = startPos[1] + epQueue.appendleft((oldEpipolarError, kScaledR)) + kScaledR[0][0] -= 1 + kScaledR[1][1] -= 1 + else: + if epError < oldEpipolarError: + epQueue.appendleft((epError, kScaledR)) + oldEpipolarError = epError + kScaledR[0][0] -= 1 + kScaledR[1][1] -= 1 + else: + break + oldEpipolarError = None + while epQueue: + currEp, currK = epQueue.popleft() + if oldEpipolarError is None: + oldEpipolarError = currEp + kScaledR = currK + else: + currEp, currK = epQueue.popleft() + if currEp < oldEpipolarError: + oldEpipolarError = currEp + kScaledR = currK + + + print('Lets find the best epipolar Error') + + + 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( From 6d3964a673f6be54f19ea67fd7bf11349279a1d4 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 29 Sep 2022 20:29:13 -0400 Subject: [PATCH 24/68] cleanedup image sizes and better ep errors --- calibrate.py | 10 ++- depthai_helpers/calibration_utils.py | 112 +++++++++++++++++---------- 2 files changed, 77 insertions(+), 45 deletions(-) diff --git a/calibrate.py b/calibrate.py index ec5a8b373..82118e058 100755 --- a/calibrate.py +++ b/calibrate.py @@ -872,7 +872,7 @@ def calibrate(self): print("Starting image processing") stereo_calib = calibUtils.StereoCalibration() dest_path = str(Path('resources').absolute()) - self.args.cameraMode = 'perspective' # hardcoded for now + # self.args.cameraMode = 'perspective' # hardcoded for now try: # stereo_calib = StereoCalibration() @@ -893,7 +893,11 @@ def calibrate(self): if self.empty_calibration(calibration_handler): calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) except: - pass + print('Device closed in exception..' ) + self.device.close() + print(e) + print(traceback.format_exc()) + raise SystemExit(1) # calibration_handler.set error_text = [] @@ -1027,6 +1031,8 @@ def calibrate(self): cv2.imshow("Result Image", resImage) cv2.waitKey(0) except Exception as e: + self.device.close() + print('Device closed in exception..' ) print(e) print(traceback.format_exc()) raise SystemExit(1) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index da5cb25ab..f5e62c9ff 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -120,7 +120,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ images_path, cam_info['hfov']) cam_info['intrinsics'] = intrinsics cam_info['dist_coeff'] = dist_coeff - cam_info['size'] = size + cam_info['size'] = size # (Width, height) cam_info['reprojection_error'] = ret print( '<------------Camera Name: {} ------------>'.format(cam_info['name'])) @@ -128,7 +128,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ cam_info['name'], ret)) print("Estimated intrinsics of {0}: \n {1}".format( cam_info['name'], intrinsics)) - + for camera in board_config['cameras'].keys(): left_cam_info = board_config['cameras'][camera] if 'extrinsics' in left_cam_info: @@ -209,7 +209,8 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) count = 0 for im in images: - # print("=> Processing image {0}".format(im)) + if traceLevel == 2: + print("=> Processing image {0}".format(im)) img_pth = Path(im) frame = cv2.imread(im) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) @@ -267,8 +268,8 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): print(im + " Not found") raise RuntimeError("Failed to detect markers in the image") - imsize = gray.shape - return allCorners, allIds, all_marker_corners, all_marker_ids, imsize, all_recovered + # imsize = gray.shape[::-1] + return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered def calibrate_intrinsics(self, image_files, hfov): image_files = glob.glob(image_files + "/*") @@ -279,19 +280,24 @@ 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( - allCorners, allIds, imsize[::-1], hfov) + allCorners, allIds, imsize, hfov) # (Height, width) - if traceLevel == 2: + if traceLevel == 3: self.fisheye_undistort_visualizaation( - image_files, camera_matrix, distortion_coefficients, imsize[::-1]) + image_files, camera_matrix, distortion_coefficients, imsize) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] + 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[::-1]) + allCorners, allIds, imsize) + if traceLevel == 3: + self.fisheye_undistort_visualizaation( + image_files, camera_matrix, distortion_coefficients, imsize) + + # (Height, width) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize[::-1] + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): self.objpoints = [] # 3d point in real world space @@ -314,7 +320,7 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu scale = None scale_req = False - frame_left_shape = cv2.imread(images_left[0], 0).shape + 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 @@ -353,8 +359,8 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco( images_right, scale_req, scaled_res) - print(f'Image size of right side :{imsize_r}') - print(f'Image size of left side :{imsize_l}') + print(f'Image size of right side (w, h):{imsize_r}') + print(f'Image size of left side (w, h):{imsize_l}') assert imsize_r == imsize_l, "Left and right resolution scaling is wrong" @@ -362,16 +368,16 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu 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 = 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] + scaled_intrinsics[1][2] -= (originalShape[0] # c_y - along height of the image * scale - destShape[0]) / 2 - scaled_intrinsics[0][2] -= (originalShape[1] + scaled_intrinsics[0][2] -= (originalShape[1] # c_x width of the image * scale - destShape[1]) / 2 if traceLevel == 2: print('original_intrinsics') @@ -387,7 +393,9 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): img = cv2.imread(im) # h, w = img.shape[:2] if self.cameraModel == 'perspective': - kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0.1) + 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), kScaled, img_size, cv2.CV_32FC1) else: @@ -397,8 +405,9 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): undistorted_img = cv2.remap( img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) cv2.imshow("undistorted", undistorted_img) - print(f'image path - {im}') - print(f'Image Undistorted Size {undistorted_img.shape}') + if 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 @@ -469,14 +478,17 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): cameraMatrixInit = np.array([[500, 0.0, 643.9126], [0.0, 500, 387.56018], [0.0, 0.0, 1.0]]) - + print("Camera Matrix initialization.............") print(cameraMatrixInit) flags = 0 - flags = cv2.fisheye.CALIB_CHECK_COND + # 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 + distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 10000, 1e-5) + cv2.TERM_CRITERIA_EPS, 50000, 1e-9) return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) @@ -497,8 +509,8 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz left_sub_corners = [] right_sub_corners = [] obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue + #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: + # continue for j in range(len(allIds_l[i])): idx = np.where(allIds_r[i] == allIds_l[i][j]) if idx[0].size == 0: @@ -523,7 +535,7 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS # print(flags) - # flags |= cv2.CALIB_FIX_INTRINSIC + flags |= cv2.CALIB_FIX_INTRINSIC # flags |= cv2.CALIB_USE_INTRINSIC_GUESS flags |= cv2.CALIB_RATIONAL_MODEL # print(flags) @@ -573,22 +585,30 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # print(f'P_r is \n {P_r}') 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 + # flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess - flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - - print('Fisyeye stereo model..................') - print(len(cameraMatrix_l)) - print(len(cameraMatrix_r)) - print(len(distCoeff_l)) - print(len(distCoeff_r)) - + # 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: + 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, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, @@ -859,17 +879,21 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # TODO(Sachin): Check if the stetch is only in calibration Images print('Original intrinsics ....') print(M_lp) + # print(d_l) print(M_rp) - kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_lp, d_l, scaled_res[::-1], 0) - # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_lp, d_r, scaled_res[::-1], 0) + # print(d_r) + + print(f'Width and height is {scaled_res[::-1]}') + # 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) kScaledR = kScaledL print('Intrinsics from the getOptimalNewCameraMatrix....') print(kScaledL) - # print(kScaledR) + print(kScaledR) oldEpipolarError = None epQueue = deque() movePos = True - # increments = 1 if 0: while True: @@ -953,7 +977,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 == 2: + if traceLevel == 3: cv2.imshow("undistorted-Left", img_l) cv2.imshow("undistorted-right", img_r) # print(f'image path - {im}') @@ -961,7 +985,7 @@ 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 == 2: + if traceLevel == 3: cv2.destroyWindow("undistorted-left") cv2.destroyWindow("undistorted-right") # compute metrics @@ -981,7 +1005,9 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, marker_corners_r, ids_r, rejectedCorners=rejectedImgPoints) - + print(f'Marekrs length r is {len(marker_corners_r)}') + print(f'Marekrs length l is {len(marker_corners_l)}') + cv2.imshow("undistorted-rleft", image_data_pair[0]) res2_l = cv2.aruco.interpolateCornersCharuco( marker_corners_l, ids_l, image_data_pair[0], self.board) res2_r = cv2.aruco.interpolateCornersCharuco( From 1067cb2946e7d3d9d0e3b400c70ef65ac0087120 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 13 Oct 2022 08:04:15 -0400 Subject: [PATCH 25/68] Fixed config error --- calibrate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/calibrate.py b/calibrate.py index 82118e058..db1bc239a 100755 --- a/calibrate.py +++ b/calibrate.py @@ -891,8 +891,8 @@ def calibrate(self): calibration_handler = self.device.readCalibration() try: if self.empty_calibration(calibration_handler): - calibration_handler.setBoardInfo(self.board_config['board_config']['name'], self.board_config['board_config']['revision']) - except: + calibration_handler.setBoardInfo(self.board_config['name'], self.board_config['revision']) + except Exception as e: print('Device closed in exception..' ) self.device.close() print(e) From c45218859d5588058d9f658102578f08e294a88f Mon Sep 17 00:00:00 2001 From: saching13 Date: Tue, 13 Dec 2022 15:37:51 +0530 Subject: [PATCH 26/68] Tweaked replay --- calibrate.py | 27 +++++++++++++++++---------- depthai_helpers/calibration_utils.py | 9 +++++---- requirements.txt | 2 +- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/calibrate.py b/calibrate.py index db1bc239a..3489c56f0 100755 --- a/calibrate.py +++ b/calibrate.py @@ -66,13 +66,13 @@ camToMonoRes = { 'OV7251' : dai.MonoCameraProperties.SensorResolution.THE_480_P, - 'OV9*82' : dai.MonoCameraProperties.SensorResolution.THE_800_P, + 'OV9282' : dai.MonoCameraProperties.SensorResolution.THE_800_P, } camToRgbRes = { 'IMX378' : dai.ColorCameraProperties.SensorResolution.THE_4_K, 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, - 'OV9*82' : 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 } @@ -310,7 +310,7 @@ def __init__(self): # "OAK-D-Lite Calibration is not supported on main yet. Please use `lite_calibration` branch to calibrate your OAK-D-Lite!!") self.device = dai.Device() - cameraProperties = self.device.getConnectedCameraProperties() + cameraProperties = self.device.getConnectedCameraFeatures() for properties in cameraProperties: for in_cam in self.board_config['cameras'].keys(): cam_info = self.board_config['cameras'][in_cam] @@ -321,13 +321,8 @@ def __init__(self): # self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check() break - pipeline = self.create_pipeline() - self.device.startPipeline(pipeline) - - self.camera_queue = {} - for config_cam in self.board_config['cameras']: - cam = self.board_config['cameras'][config_cam] - self.camera_queue[cam['name']] = self.device.getOutputQueue(cam['name'], 1, False) + + """ cameraProperties = self.device.getConnectedCameraProperties() for properties in cameraProperties: @@ -341,6 +336,15 @@ def __init__(self): # if not self.args.disableRgb: # self.rgb_camera_queue = self.device.getOutputQueue("rgb", 30, True) + def startPipeline(self): + pipeline = self.create_pipeline() + self.device.startPipeline(pipeline) + + self.camera_queue = {} + for config_cam in self.board_config['cameras']: + cam = self.board_config['cameras'][config_cam] + self.camera_queue[cam['name']] = self.device.getOutputQueue(cam['name'], 1, False) + def is_markers_found(self, frame): marker_corners, _, _ = cv2.aruco.detectMarkers( frame, self.aruco_dictionary) @@ -384,6 +388,8 @@ def create_pipeline(self): xout = pipeline.createXLinkOut() cam_node.setBoardSocket(stringToCam[cam_id]) + sensorName = cam_info['sensorName'] + print(f'Sensor name is {sensorName}') cam_node.setResolution(camToRgbRes[cam_info['sensorName']]) cam_node.setFps(fps) @@ -1050,6 +1056,7 @@ def run(self): traceback.print_exc() print("An error occurred trying to create image dataset directories!") raise SystemExit(1) + self.startPipeline() self.show_info_frame() self.capture_images_sync() self.dataset_path = str(Path("dataset").absolute()) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index f5e62c9ff..8d13e88e3 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -411,6 +411,8 @@ def fisheye_undistort_visualizaation(self, img_list, K, D, img_size): k = cv2.waitKey(0) if k == 27: # Esc key to stop break + cv2.destroyWindow("undistorted") + def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): """ @@ -652,7 +654,7 @@ def display_rectification(self, image_data_pairs, isHorizontal): line_col += 40 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) @@ -885,7 +887,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, print(f'Width and height is {scaled_res[::-1]}') # print(d_r) - kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_l, d_l, scaled_res[::-1], 0) + kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) kScaledR = kScaledL print('Intrinsics from the getOptimalNewCameraMatrix....') @@ -977,7 +979,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 traceLevel == 4: cv2.imshow("undistorted-Left", img_l) cv2.imshow("undistorted-right", img_r) # print(f'image path - {im}') @@ -1007,7 +1009,6 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, rejectedCorners=rejectedImgPoints) print(f'Marekrs length r is {len(marker_corners_r)}') print(f'Marekrs length l is {len(marker_corners_l)}') - cv2.imshow("undistorted-rleft", image_data_pair[0]) res2_l = cv2.aruco.interpolateCornersCharuco( marker_corners_l, ids_l, image_data_pair[0], self.board) res2_r = cv2.aruco.interpolateCornersCharuco( diff --git a/requirements.txt b/requirements.txt index fe1dd10b9..9102662f6 100644 --- a/requirements.txt +++ b/requirements.txt @@ -12,4 +12,4 @@ scipy pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ # depthai==2.17.1.0 -depthai==2.17.2.0.dev+211dc1c0a135d5615dba75ebf2098ba8cdc05cd0 \ No newline at end of file +depthai===2.19.0.0 \ No newline at end of file From 029aff0f7c625e17fe501b328e06b7d2a9a38c66 Mon Sep 17 00:00:00 2001 From: saching13 Date: Tue, 20 Dec 2022 12:16:38 +0530 Subject: [PATCH 27/68] Added optional rect from projection matrix --- depthai_helpers/calibration_utils.py | 30 +++++++++++++++++----------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 8d13e88e3..f72ab47a2 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -12,8 +12,8 @@ from pathlib import Path from collections import deque # Creates a set of 13 polygon coordinates -traceLevel = 1 - +traceLevel = 0 +rectProjectionMode = 0 def setPolygonCoordinates(height, width): horizontal_shift = width//4 @@ -870,9 +870,12 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) - - # p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) - # p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) + if rectProjectionMode: + p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) + p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) + print('Projection intrinsics ....') + print(p_lp) + print(p_rp) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) @@ -881,16 +884,19 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # TODO(Sachin): Check if the stetch is only in calibration Images print('Original intrinsics ....') print(M_lp) - # print(d_l) print(M_rp) - # print(d_r) print(f'Width and height is {scaled_res[::-1]}') # print(d_r) - kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) + # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) + # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0) # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) - kScaledR = kScaledL - print('Intrinsics from the getOptimalNewCameraMatrix....') + kScaledR = kScaledL = M_r + if rectProjectionMode: + kScaledL = p_lp + kScaledR = p_rp + + print('Intrinsics from the getOptimalNewCameraMatrix/Original ....') print(kScaledL) print(kScaledR) oldEpipolarError = None @@ -1071,8 +1077,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, print('Numer of corners is in left -> {} and right -> {}'.format( len(marker_corners_l), len(marker_corners_r))) return -1 - lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) - rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) + lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA) + rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA) image_data_pairs[i] = (lText, rText) From 6f186a6ecded35f7350d702ac861a419d8c65ed5 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 20 Feb 2023 12:54:45 -0800 Subject: [PATCH 28/68] updated depthai version --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 9102662f6..7d483eb9c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -12,4 +12,4 @@ scipy pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ # depthai==2.17.1.0 -depthai===2.19.0.0 \ No newline at end of file +depthai===2.20.2.0 \ No newline at end of file From 9deb126adad5598f4dfde470e0791355c80b18c3 Mon Sep 17 00:00:00 2001 From: alex-luxonis Date: Tue, 21 Feb 2023 01:48:07 +0200 Subject: [PATCH 29/68] Add `-ab/--antibanding {50|60|off}` option. Implement `-fps` option --- calibrate.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/calibrate.py b/calibrate.py index 3489c56f0..dde067393 100755 --- a/calibrate.py +++ b/calibrate.py @@ -77,6 +77,12 @@ 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P } +antibandingOpts = { + 'off': dai.CameraControl.AntiBandingMode.OFF, + '50': dai.CameraControl.AntiBandingMode.MAINS_50_HZ, + '60': dai.CameraControl.AntiBandingMode.MAINS_60_HZ, +} + def create_blank(width, height, rgb_color=(0, 0, 0)): """Create new image(numpy array) filled with certain color in RGB""" # Create black blank image @@ -151,8 +157,6 @@ def parse_args(): required=False, help="Choose between perspective and Fisheye") parser.add_argument("-rlp", "--rgbLensPosition", default=135, 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.") @@ -160,6 +164,10 @@ 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('-ab', '--antibanding', default='50', choices={'off', '50', '60'}, + help="Set antibanding/antiflicker algo for lights that flicker at mains frequency. Default: %(default)s [Hz]") options = parser.parse_args() @@ -370,7 +378,7 @@ def test_camera_orientation(self, frame_l, frame_r): def create_pipeline(self): pipeline = dai.Pipeline() - fps = 10 + fps = self.args.framerate for cam_id in self.board_config['cameras']: cam_info = self.board_config['cameras'][cam_id] if cam_info['type'] == 'mono': @@ -406,6 +414,8 @@ def create_pipeline(self): controlIn = pipeline.createXLinkIn() controlIn.setStreamName(cam_info['name'] + '-control') controlIn.out.link(cam_node.inputControl) + + cam_node.initialControl.setAntiBandingMode(antibandingOpts[self.args.antibanding]) xout.input.setBlocking(False) xout.input.setQueueSize(1) From 691f68015c7fde9a4b699140b69f7f6fe27e5d84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nejc=20Jezer=C5=A1ek?= Date: Wed, 22 Feb 2023 17:40:16 +0100 Subject: [PATCH 30/68] Peripheral calibration - fisheye camera model (#952) * Make sure the same points are present on all images * Use initUndistortRectifyMap from fisheye calib --- depthai_helpers/calibration_utils.py | 78 ++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 21 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index f72ab47a2..4c42b79a2 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -10,6 +10,7 @@ import json import cv2.aruco as aruco from pathlib import Path +from functools import reduce from collections import deque # Creates a set of 13 polygon coordinates traceLevel = 0 @@ -530,7 +531,7 @@ 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, 1000, 1e-9) if self.cameraModel == 'perspective': flags = 0 @@ -588,10 +589,28 @@ 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': + # make sure all images have the same points + common_ids = reduce(np.intersect1d, allIds_l + allIds_r) + common_obj_points = self.board.chessboardCorners[common_ids.reshape(-1,1)] + all_common_obj_points = [] + all_common_left_img_points = [] + all_common_right_img_points = [] + for left_ids, left_img_points, right_ids, right_img_points in zip(allIds_l, allCorners_l, allIds_r, allCorners_r): + common_left_img_points = left_img_points[np.in1d(left_ids, common_ids)] + common_right_img_points = right_img_points[np.in1d(right_ids, common_ids)] + + all_common_left_img_points.append(common_left_img_points) + all_common_right_img_points.append(common_right_img_points) + all_common_obj_points.append(common_obj_points) + flags = 0 - flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - flags |= cv2.fisheye.CALIB_CHECK_COND + # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + # flags |= cv2.fisheye.CALIB_CHECK_COND flags |= cv2.fisheye.CALIB_FIX_SKEW + flags |= cv2.fisheye.CALIB_FIX_INTRINSIC + flags |= cv2.fisheye.CALIB_FIX_K1 + flags |= cv2.fisheye.CALIB_FIX_K2 + flags |= cv2.fisheye.CALIB_FIX_K3 # flags |= cv2.CALIB_FIX_INTRINSIC # flags |= cv2.CALIB_RATIONAL_MODEL # TODO(sACHIN): Try without intrinsic guess @@ -611,8 +630,8 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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( + all_common_obj_points, all_common_left_img_points, all_common_right_img_points, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, flags=flags, criteria=stereocalib_criteria), None, None @@ -621,9 +640,9 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz distCoeff_l, cameraMatrix_r, distCoeff_r, - imsize, R, T) + imsize, R, T, flags=0) - 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): print( @@ -698,11 +717,17 @@ def scale_image(self, img, scaled_res): 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) - + 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 = [] imagesCount = 0 @@ -952,11 +977,16 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, - 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) - + 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): @@ -1103,10 +1133,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) From abc566e6611bc63f8e7653c028ec55ec1d828e42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nejc=20Jezer=C5=A1ek?= Date: Thu, 23 Feb 2023 14:32:53 +0100 Subject: [PATCH 31/68] Fix fiheye calibration rotated rectified image --- depthai_helpers/calibration_utils.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 4c42b79a2..eae9f5a52 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -635,7 +635,7 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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( + R_l, R_r, P_l, P_r, Q, _, _ = cv2.stereoRectify( cameraMatrix_l, distCoeff_l, cameraMatrix_r, @@ -861,9 +861,7 @@ 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.2: - isHorizontal = False + isHorizontal = np.absolute(t[0]) > np.absolute(t[1]) scale = None scale_req = False From d1054c574fd705cb10c928ef31d603b9d5155ad8 Mon Sep 17 00:00:00 2001 From: saching13 Date: Fri, 3 Mar 2023 09:05:59 -0800 Subject: [PATCH 32/68] new lines from corner to corners --- depthai_helpers/calibration_utils.py | 189 ++++++++++++++++++++------- depthai_helpers/stereoRectify.py | 94 +++++++++++++ resources/boards/AR0234DEV.json | 10 +- 3 files changed, 241 insertions(+), 52 deletions(-) create mode 100644 depthai_helpers/stereoRectify.py diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index eae9f5a52..ff18c035f 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -12,10 +12,12 @@ from pathlib import Path from functools import reduce from collections import deque +from depthai_helpers.stereoRectify import stereoRectify # Creates a set of 13 polygon coordinates traceLevel = 0 rectProjectionMode = 0 +colors = [(0, 255 , 0), (0, 0, 255)] def setPolygonCoordinates(height, width): horizontal_shift = width//4 vertical_shift = height//4 @@ -478,15 +480,17 @@ 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]]) + cameraMatrixInit = np.array([[907.84859625, 0.0 , 995.15888273], + [ 0.0 , 889.29269629, 627.49748034], + [ 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_USE_INTRINSIC_GUESS + # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW distCoeffsInit = np.zeros((4, 1)) @@ -546,23 +550,18 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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( @@ -606,12 +605,12 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz flags = 0 # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # flags |= cv2.fisheye.CALIB_CHECK_COND - flags |= cv2.fisheye.CALIB_FIX_SKEW + # flags |= cv2.fisheye.CALIB_FIX_SKEW flags |= cv2.fisheye.CALIB_FIX_INTRINSIC flags |= cv2.fisheye.CALIB_FIX_K1 flags |= cv2.fisheye.CALIB_FIX_K2 flags |= cv2.fisheye.CALIB_FIX_K3 - # flags |= cv2.CALIB_FIX_INTRINSIC + flags |= cv2.fisheye.CALIB_FIX_K4 # flags |= cv2.CALIB_RATIONAL_MODEL # TODO(sACHIN): Try without intrinsic guess # flags |= cv2.CALIB_USE_INTRINSIC_GUESS @@ -635,42 +634,127 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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.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]) + + if 0: + if not isHorizontal: + rotated_k_l = cameraMatrix_l.copy() + rotated_k_r = cameraMatrix_r.copy() + rotated_k_l[0][0] = cameraMatrix_l[1][1] # swap fx and fy + rotated_k_r[0][0] = cameraMatrix_r[1][1] # swap fx and fy + rotated_k_l[1][1] = cameraMatrix_l[0][0] # swap fx and fy + rotated_k_r[1][1] = cameraMatrix_r[0][0] # swap fx and fy + + rotated_k_l[0][2] = cameraMatrix_l[1][2] # swap optical center x and y + rotated_k_r[0][2] = cameraMatrix_r[1][2] # swap optical center x and y + rotated_k_l[1][2] = cameraMatrix_l[0][2] # swap optical center x and y + rotated_k_r[1][2] = cameraMatrix_r[0][2] # swap optical center x and y + + T_mod = T.copy() + T_mod[0] = T[1] + T_mod[1] = T[0] + + r = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True) + R_mod = r.as_matrix() + print(f' Image size is {imsize} and modified iamge size is {imsize[::-1]}') + R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( + rotated_k_l, + distCoeff_l, + rotated_k_r, + distCoeff_r, + imsize[::-1], R_mod, T_mod, flags=0) + # TODO revier things back to original style for Rotation and translation + r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) + R_l = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True).as_matrix() + + r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) + R_r = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True).as_matrix() + + temp = P_l[0][0] + P_l[0][0] = P_l[1][1] + P_l[1][1] = temp + temp = P_r[0][0] + P_r[0][0] = P_r[1][1] + P_r[1][1] = temp + + temp = P_l[0][2] + P_l[0][2] = P_l[1][2] + P_l[1][2] = temp + temp = P_r[0][2] + P_r[0][2] = P_r[1][2] + P_r[1][2] = temp + + temp = P_l[0][3] + P_l[0][3] = P_l[1][3] + P_l[1][3] = temp + temp = P_r[0][3] + P_r[0][3] = P_r[1][3] + P_r[1][3] = temp + else: + R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( + cameraMatrix_l, + distCoeff_l, + cameraMatrix_r, + distCoeff_r, + imsize, R, T, flags=0) + R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, - imsize, R, T, flags=0) + imsize, R, T) # , alpha=0.1 + + if 0: + R_l, R_r = stereoRectify(R, T) + + 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, 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: + 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]]) + 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.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) # draw epipolar lines for debug purposes - 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 + # 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 img_concat = cv2.resize( img_concat, (0, 0), fx=0.8, fy=0.8) @@ -1021,12 +1105,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: - cv2.destroyWindow("undistorted-left") + if traceLevel == 4: + cv2.destroyWindow("undistorted-Left") cv2.destroyWindow("undistorted-right") # compute metrics imgpoints_r = [] imgpoints_l = [] + image_epipolar_color = [] # new_imagePairs = [] for i, image_data_pair in enumerate(image_data_pairs): # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) @@ -1089,17 +1174,24 @@ 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) + 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: - epi_error_sum += abs(l_pt[0][0] - r_pt[0][0]) + curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0]) + if curr_epipolar_error >= 1: + corner_epipolar_color.append(1) + else: + 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 + " : " + + print("Epipolar Error per image on host in " + img_pth_right.name + " : " + str(localError)) else: print('Numer of corners is in left -> {} and right -> {}'.format( @@ -1111,17 +1203,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 diff --git a/depthai_helpers/stereoRectify.py b/depthai_helpers/stereoRectify.py new file mode 100644 index 000000000..0671fffed --- /dev/null +++ b/depthai_helpers/stereoRectify.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +import numpy as np +import math + +np.set_printoptions(suppress=True) + +def rotationMatrixToEulerAngles(R) : + + sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) + + singular = sy < 1e-6 + + if not singular : + x = math.atan2(R[2,1] , R[2,2]) + y = math.atan2(-R[2,0], sy) + z = math.atan2(R[1,0], R[0,0]) + else : + x = math.atan2(-R[1,2], R[1,1]) + y = math.atan2(-R[2,0], sy) + z = 0 + + return np.array([x, y, z]) + +def eulerAnglesToRotationMatrix(theta) : + + R_x = np.array([[1, 0, 0 ], + [0, math.cos(theta[0]), -math.sin(theta[0]) ], + [0, math.sin(theta[0]), math.cos(theta[0]) ] + ]) + + R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], + [0, 1, 0 ], + [-math.sin(theta[1]), 0, math.cos(theta[1]) ] + ]) + + R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], + [math.sin(theta[2]), math.cos(theta[2]), 0], + [0, 0, 1] + ]) + + R = (R_z @ ( R_y @ R_x )) + + return R + +def stereoRectify(R, T): + T = T.flatten() + print(f'Shape of R: {R.shape}') + print(f'Shape of T: {T.shape}') + om = rotationMatrixToEulerAngles(R) + om = om * -0.5 + r_r = eulerAnglesToRotationMatrix(om) + t = r_r @ T + + idx = 0 if abs(t[0]) > abs(t[1]) else 1 + + c = t[idx] + nt = np.linalg.norm(t) + uu = np.zeros(t.shape) + uu[idx] = 1 if c > 0 else -1 + print(f'Shape of t: {t.shape}') + print(f'Shape of uu: {uu.shape}') + + ww = np.cross(t, uu) + nw = np.linalg.norm(ww) + + if nw > 0: + scale = math.acos(abs(c)/nt)/nw + ww = ww * scale + + wR = eulerAnglesToRotationMatrix(ww) + R1 = wR @ np.transpose(r_r) + R2 = wR @ r_r + + return R1, R2 + +refR1 = np.array([[ 0.99994761, -0.00390605, -0.0094629 ], + [ 0.00391631, 0.99999177, 0.00106613 ], + [ 0.00945866, -0.00110314, 0.99995464 ]]) +refR2 = np.array([[ 0.99984103, 0.0033367, 0.01751487 ], + [ -0.0033177, 0.99999386, -0.00111375 ], + [ -0.01751848, 0.00105546, 0.99984598 ]]) + + +R = np.array([[ 0.99960995, -0.00720377, -0.02698262 ], + [ 0.00726279, 0.99997145, 0.00208996 ], + [ 0.02696679, -0.00228512, 0.99963373 ]]) +T = np.array([-7.51322365, -0.02507336, -0.13161406]) + +R1, R2 = stereoRectify(R, T) +print("refR1-R1:") +print(refR1-R1) +print("refR2-R2:") +print(refR2-R2) diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json index 47825dfee..9e1390f9c 100644 --- a/resources/boards/AR0234DEV.json +++ b/resources/boards/AR0234DEV.json @@ -1,11 +1,11 @@ { "board_config": { - "name": "AR0234DEV", + "name": "FFC-3P", "revision": "R1M0E1", "cameras":{ "CAM_C": { - "name": "right", + "name": "left", "hfov": 140, "type": "color", "extrinsics": { @@ -23,7 +23,7 @@ } }, "CAM_B": { - "name": "left", + "name": "right", "hfov": 140, "type": "color", "extrinsics": { @@ -48,8 +48,8 @@ }, "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" + "left_cam": "CAM_C", + "right_cam": "CAM_B" } } } From 9db8704155724d4bc65432968d78a3b5fae15731 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 23 Mar 2023 14:04:30 -0700 Subject: [PATCH 33/68] update calibration handler with cameraModel --- calibrate.py | 3 +- depthai_helpers/stereoRectify.py | 94 -------------------------------- 2 files changed, 2 insertions(+), 95 deletions(-) delete mode 100644 depthai_helpers/stereoRectify.py diff --git a/calibrate.py b/calibrate.py index dde067393..14d029b9a 100755 --- a/calibrate.py +++ b/calibrate.py @@ -942,7 +942,8 @@ def calibrate(self): 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.args.cameraMode != 'perspective': + calibration_handler.setCameraType(stringToCam[camera], dai.CameraModel.Fisheye) if cam_info['hasAutofocus']: calibration_handler.setLensPosition(stringToCam[camera], self.focus_value) diff --git a/depthai_helpers/stereoRectify.py b/depthai_helpers/stereoRectify.py deleted file mode 100644 index 0671fffed..000000000 --- a/depthai_helpers/stereoRectify.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import math - -np.set_printoptions(suppress=True) - -def rotationMatrixToEulerAngles(R) : - - sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) - - singular = sy < 1e-6 - - if not singular : - x = math.atan2(R[2,1] , R[2,2]) - y = math.atan2(-R[2,0], sy) - z = math.atan2(R[1,0], R[0,0]) - else : - x = math.atan2(-R[1,2], R[1,1]) - y = math.atan2(-R[2,0], sy) - z = 0 - - return np.array([x, y, z]) - -def eulerAnglesToRotationMatrix(theta) : - - R_x = np.array([[1, 0, 0 ], - [0, math.cos(theta[0]), -math.sin(theta[0]) ], - [0, math.sin(theta[0]), math.cos(theta[0]) ] - ]) - - R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], - [0, 1, 0 ], - [-math.sin(theta[1]), 0, math.cos(theta[1]) ] - ]) - - R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], - [math.sin(theta[2]), math.cos(theta[2]), 0], - [0, 0, 1] - ]) - - R = (R_z @ ( R_y @ R_x )) - - return R - -def stereoRectify(R, T): - T = T.flatten() - print(f'Shape of R: {R.shape}') - print(f'Shape of T: {T.shape}') - om = rotationMatrixToEulerAngles(R) - om = om * -0.5 - r_r = eulerAnglesToRotationMatrix(om) - t = r_r @ T - - idx = 0 if abs(t[0]) > abs(t[1]) else 1 - - c = t[idx] - nt = np.linalg.norm(t) - uu = np.zeros(t.shape) - uu[idx] = 1 if c > 0 else -1 - print(f'Shape of t: {t.shape}') - print(f'Shape of uu: {uu.shape}') - - ww = np.cross(t, uu) - nw = np.linalg.norm(ww) - - if nw > 0: - scale = math.acos(abs(c)/nt)/nw - ww = ww * scale - - wR = eulerAnglesToRotationMatrix(ww) - R1 = wR @ np.transpose(r_r) - R2 = wR @ r_r - - return R1, R2 - -refR1 = np.array([[ 0.99994761, -0.00390605, -0.0094629 ], - [ 0.00391631, 0.99999177, 0.00106613 ], - [ 0.00945866, -0.00110314, 0.99995464 ]]) -refR2 = np.array([[ 0.99984103, 0.0033367, 0.01751487 ], - [ -0.0033177, 0.99999386, -0.00111375 ], - [ -0.01751848, 0.00105546, 0.99984598 ]]) - - -R = np.array([[ 0.99960995, -0.00720377, -0.02698262 ], - [ 0.00726279, 0.99997145, 0.00208996 ], - [ 0.02696679, -0.00228512, 0.99963373 ]]) -T = np.array([-7.51322365, -0.02507336, -0.13161406]) - -R1, R2 = stereoRectify(R, T) -print("refR1-R1:") -print(refR1-R1) -print("refR2-R2:") -print(refR2-R2) From 4d9e428def889874acb23a4fafd674c73bb1cf10 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 24 Mar 2023 04:06:02 +0100 Subject: [PATCH 34/68] WIP vermeer --- calibrate.py | 107 ++++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 98 insertions(+), 9 deletions(-) diff --git a/calibrate.py b/calibrate.py index dde067393..c20d3a81f 100755 --- a/calibrate.py +++ b/calibrate.py @@ -11,6 +11,7 @@ from collections import deque from scipy.spatial.transform import Rotation import traceback +import itertools import cv2 from cv2 import resize @@ -74,7 +75,9 @@ 'IMX214' : dai.ColorCameraProperties.SensorResolution.THE_4_K, 'OV9782' : dai.ColorCameraProperties.SensorResolution.THE_800_P, 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP, - 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P + 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, + 'ar0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, + 'imx412' : dai.ColorCameraProperties.SensorResolution.THE_1080_P, } antibandingOpts = { @@ -277,6 +280,91 @@ def get_synced(self): return synced return False """ + +class MessageSync: + def __init__(self, num_queues, min_diff_timestamp, max_num_messages=4, min_queue_depth=3): + self.num_queues = num_queues + self.min_diff_timestamp = min_diff_timestamp + self.max_num_messages = max_num_messages + # self.queues = [deque() for _ in range(num_queues)] + self.queues = dict() + self.queue_depth = min_queue_depth + # self.earliest_ts = {} + + def add_msg(self, name, msg): + if name not in self.queues: + self.queues[name] = deque(maxlen=self.max_num_messages) + self.queues[name].append(msg) + # if msg.getTimestampDevice() < self.earliest_ts: + # self.earliest_ts = {name: msg.getTimestampDevice()} + + # print('Queues: ', end='') + # for name in self.queues.keys(): + # print('\t: ', name, end='') + # print(self.queues[name], end=', ') + # print() + # print() + + def get_synced(self): + + # Atleast 3 messages should be buffered + min_len = min([len(queue) for queue in self.queues.values()]) + if min_len == 0: + print('Status:', 'exited due to min len == 0', self.queues) + return None + + # initializing list of list + queue_lengths = [] + for name in self.queues.keys(): + queue_lengths.append(range(0, len(self.queues[name]))) + permutations = list(itertools.product(*queue_lengths)) + # print ("All possible permutations are : " + str(permutations)) + + # Return a best combination after being atleast 3 messages deep for all queues + min_ts_diff = None + for indicies in permutations: + tmp = {} + i = 0 + for n in self.queues.keys(): + tmp[n] = indicies[i] + i = i + 1 + indicies = tmp + + acc_diff = 0.0 + min_ts = None + for name in indicies.keys(): + msg = self.queues[name][indicies[name]] + if min_ts is None: + min_ts = msg.getTimestampDevice().total_seconds() + for name in indicies.keys(): + msg = self.queues[name][indicies[name]] + acc_diff = acc_diff + abs(min_ts - msg.getTimestampDevice().total_seconds()) + + # Mark minimum + if min_ts_diff is None or (acc_diff < min_ts_diff['ts'] and abs(acc_diff - min_ts_diff['ts']) > 0.0001): + min_ts_diff = {'ts': acc_diff, 'indicies': indicies.copy()} + print('new minimum:', min_ts_diff, 'min required:', self.min_diff_timestamp) + + if min_ts_diff['ts'] < self.min_diff_timestamp: + # Check if atleast 5 messages deep + min_queue_depth = None + for name in indicies.keys(): + if min_queue_depth is None or indicies[name] < min_queue_depth: + min_queue_depth = indicies[name] + if min_queue_depth >= self.queue_depth: + # Retrieve and pop the others + synced = {} + for name in indicies.keys(): + synced[name] = self.queues[name][min_ts_diff['indicies'][name]] + # pop out the older messages + for i in range(0, min_ts_diff['indicies'][name]+1): + self.queues[name].popleft() + + print('Returning synced messages with error:', min_ts_diff['ts'], min_ts_diff['indicies']) + return synced + + # print(' + class Main: output_scale_factor = 0.5 polygons = None @@ -415,7 +503,7 @@ def create_pipeline(self): controlIn.setStreamName(cam_info['name'] + '-control') controlIn.out.link(cam_node.inputControl) - cam_node.initialControl.setAntiBandingMode(antibandingOpts[self.args.antibanding]) + # cam_node.initialControl.setAntiBandingMode(antibandingOpts[self.args.antibanding]) xout.input.setBlocking(False) xout.input.setQueueSize(1) @@ -519,7 +607,7 @@ def capture_images_sync(self): curr_time = None self.display_name = "Image Window" - syncCollector = HostSync(60) + syncCollector = MessageSync(len(self.camera_queue), 10) while not finished: currImageList = {} for key in self.camera_queue.keys(): @@ -527,7 +615,7 @@ def capture_images_sync(self): # print(f'Timestamp of {key} is {frameMsg.getTimestamp()}') - syncCollector.add_msg(key, frameMsg, frameMsg.getTimestamp()) + syncCollector.add_msg(key, frameMsg) gray_frame = None if frameMsg.getType() == dai.RawImgFrame.Type.RAW8: gray_frame = frameMsg.getCvFrame() @@ -577,13 +665,13 @@ def capture_images_sync(self): # print(f'final_scaledImageSize is {imgFrame.shape}') if self.polygons is None: self.height, self.width = imgFrame.shape - print(self.height, self.width) + # print(self.height, self.width) self.polygons = calibUtils.setPolygonCoordinates( self.height, self.width) localPolygon = np.array([self.polygons[self.current_polygon]]) - print(localPolygon.shape) - print(localPolygon) + # print(localPolygon.shape) + # print(localPolygon) if self.images_captured_polygon == 1: # perspectiveRotationMatrix = Rotation.from_euler('z', 45, degrees=True).as_matrix() angle = 30. @@ -603,8 +691,8 @@ def capture_images_sync(self): localPolygon[0][:, 1] += (height - abs(localPolygon[0][:, 1].max())) localPolygon[0][:, 0] += abs(localPolygon[0][:, 1].min()) - print(localPolygon) - print(localPolygon.shape) + # print(localPolygon) + # print(localPolygon.shape) cv2.polylines( imgFrame, localPolygon, True, (0, 0, 255), 4) @@ -661,6 +749,7 @@ def capture_images_sync(self): self.camera_queue[key].getAll() continue for name, frameMsg in syncedMsgs.items(): + print(f"Time stamp of {name} is {frameMsg.getTimestamp()}") tried[name] = self.parse_frame(frameMsg.getCvFrame(), name) allPassed = allPassed and tried[name] From 3be4d44a652da491c433bd6c51c446392da8204a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nejc=20Jezer=C5=A1ek?= Date: Tue, 28 Mar 2023 18:06:07 +0200 Subject: [PATCH 35/68] Fix fisheye calibration --- depthai_helpers/calibration_utils.py | 34 +++++----------------------- 1 file changed, 6 insertions(+), 28 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index ff18c035f..890ae853e 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -12,7 +12,6 @@ from pathlib import Path from functools import reduce from collections import deque -from depthai_helpers.stereoRectify import stereoRectify # Creates a set of 13 polygon coordinates traceLevel = 0 rectProjectionMode = 0 @@ -588,19 +587,11 @@ 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': - # make sure all images have the same points - common_ids = reduce(np.intersect1d, allIds_l + allIds_r) - common_obj_points = self.board.chessboardCorners[common_ids.reshape(-1,1)] - all_common_obj_points = [] - all_common_left_img_points = [] - all_common_right_img_points = [] - for left_ids, left_img_points, right_ids, right_img_points in zip(allIds_l, allCorners_l, allIds_r, allCorners_r): - common_left_img_points = left_img_points[np.in1d(left_ids, common_ids)] - common_right_img_points = right_img_points[np.in1d(right_ids, common_ids)] - - all_common_left_img_points.append(common_left_img_points) - all_common_right_img_points.append(common_right_img_points) - all_common_obj_points.append(common_obj_points) + # 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 = 0 # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC @@ -619,18 +610,8 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW if 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( - all_common_obj_points, all_common_left_img_points, all_common_right_img_points, + 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 @@ -709,9 +690,6 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz cameraMatrix_r, distCoeff_r, imsize, R, T) # , alpha=0.1 - - if 0: - R_l, R_r = stereoRectify(R, T) r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) print(f'R_L Euler angles in XYZ {r_euler}') From e0ae7d77214d053089634be0380038659affb2ae Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 11 Apr 2023 20:13:18 +0200 Subject: [PATCH 36/68] Fixes for RVC3 --- calibrate.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/calibrate.py b/calibrate.py index 34490c192..9988061f2 100755 --- a/calibrate.py +++ b/calibrate.py @@ -76,8 +76,6 @@ 'OV9782' : dai.ColorCameraProperties.SensorResolution.THE_800_P, 'IMX582' : dai.ColorCameraProperties.SensorResolution.THE_12_MP, 'AR0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, - 'ar0234' : dai.ColorCameraProperties.SensorResolution.THE_1200_P, - 'imx412' : dai.ColorCameraProperties.SensorResolution.THE_1080_P, } antibandingOpts = { @@ -486,7 +484,7 @@ def create_pipeline(self): cam_node.setBoardSocket(stringToCam[cam_id]) sensorName = cam_info['sensorName'] print(f'Sensor name is {sensorName}') - cam_node.setResolution(camToRgbRes[cam_info['sensorName']]) + cam_node.setResolution(camToRgbRes[cam_info['sensorName'].upper()]) cam_node.setFps(fps) xout.setStreamName(cam_info['name']) @@ -744,7 +742,7 @@ def capture_images_sync(self): if capturing: syncedMsgs = syncCollector.get_synced() - if syncedMsgs == False: + if syncedMsgs == False or syncedMsgs == None: for key in self.camera_queue.keys(): self.camera_queue[key].getAll() continue From 9645a5e2ca81903efaa5a28a67db105f6294c2e7 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 18 Apr 2023 15:28:05 +0200 Subject: [PATCH 37/68] Add mouse trigger, save calibration, custom dataset options --- calibrate.py | 35 +++++++++++++++++++++++++++++++---- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/calibrate.py b/calibrate.py index 9988061f2..e64d4a8a5 100755 --- a/calibrate.py +++ b/calibrate.py @@ -169,6 +169,16 @@ def parse_args(): help="FPS to set for all cameras. Default: %(default)s") parser.add_argument('-ab', '--antibanding', default='50', choices={'off', '50', '60'}, help="Set antibanding/antiflicker algo for lights that flicker at mains frequency. Default: %(default)s [Hz]") + parser.add_argument('-scp', '--saveCalibPath', type=str, default="", + help="Save calibration file to this path") + parser.add_argument('-dst', '--datasetPath', type=str, default="", + help="Path to dataset used for processing images") + parser.add_argument('-mdmp', '--minDetectedMarkersPercent', type=float, default=0.5, + help="Minimum percentage of detected markers to consider a frame valid") + parser.add_argument('-mt', '--mouseTrigger', default=False, action="store_true", + help="Enable mouse trigger for image capture") + parser.add_argument('-nic', '--noInitCalibration', default=False, action="store_true", + help="Don't take the board calibration for initialization but start with an empty one") options = parser.parse_args() @@ -430,6 +440,10 @@ def __init__(self): # if not self.args.disableRgb: # self.rgb_camera_queue = self.device.getOutputQueue("rgb", 30, True) + def mouse_event_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + self.mouseTrigger = True + def startPipeline(self): pipeline = self.create_pipeline() self.device.startPipeline(pipeline) @@ -443,7 +457,8 @@ def is_markers_found(self, frame): marker_corners, _, _ = cv2.aruco.detectMarkers( frame, self.aruco_dictionary) print("Markers count ... {}".format(len(marker_corners))) - return not (len(marker_corners) < self.args.squaresX*self.args.squaresY / 4) + num_all_markers = (self.args.squaresX-1)*(self.args.squaresY-1) / 2 + return not (len(marker_corners) < (num_all_markers * self.args.minDetectedMarkersPercent)) def test_camera_orientation(self, frame_l, frame_r): marker_corners_l, id_l, _ = cv2.aruco.detectMarkers( @@ -606,6 +621,7 @@ def capture_images_sync(self): self.display_name = "Image Window" syncCollector = MessageSync(len(self.camera_queue), 10) + self.mouseTrigger = False while not finished: currImageList = {} for key in self.camera_queue.keys(): @@ -715,10 +731,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 == True: start_timer = True prev_time = time.time() timer = self.args.captureDelay + self.mouseTrigger = False if start_timer == True: curr_time = time.time() @@ -735,6 +752,9 @@ def capture_images_sync(self): (image_shape[1]//2, image_shape[0]//2), font, 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 = {} @@ -990,8 +1010,10 @@ def calibrate(self): self.args.squaresY, self.args.cameraMode, self.args.rectifiedDisp) # Turn off enable disp rectify - - calibration_handler = self.device.readCalibration() + if self.args.noInitCalibration: + calibration_handler = dai.CalibrationHandler() + else: + calibration_handler = self.device.readCalibration() try: if self.empty_calibration(calibration_handler): calibration_handler.setBoardInfo(self.board_config['name'], self.board_config['revision']) @@ -1078,6 +1100,8 @@ def calibrate(self): mx_serial_id = self.device.getDeviceInfo().getMxId() calib_dest_path = dest_path + '/' + mx_serial_id + '.json' calibration_handler.eepromToJsonFile(calib_dest_path) + if self.args.saveCalibPath: + calibration_handler.eepromToJsonFile(self.args.saveCalibPath) # try: self.device.flashCalibration2(calibration_handler) is_write_succesful = True @@ -1158,6 +1182,9 @@ def run(self): self.show_info_frame() self.capture_images_sync() self.dataset_path = str(Path("dataset").absolute()) + if self.args.datasetPath: + print("Using dataset path: {}".format(self.args.datasetPath)) + self.dataset_path = self.args.datasetPath if 'process' in self.args.mode: self.calibrate() print('py: DONE.') From 185c2f715bc64403d97977d1ec73ee0221814541 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Tue, 18 Apr 2023 18:56:36 +0200 Subject: [PATCH 38/68] Update depthai version --- requirements.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/requirements.txt b/requirements.txt index 7d483eb9c..cf55a7230 100644 --- a/requirements.txt +++ b/requirements.txt @@ -11,5 +11,4 @@ scipy --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -# depthai==2.17.1.0 -depthai===2.20.2.0 \ No newline at end of file +depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file From 5f7e74628c6d319b4dccd48182849079df4b45c2 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 19 Apr 2023 02:23:53 +0200 Subject: [PATCH 39/68] Add options for dataset custom path --- calibrate.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/calibrate.py b/calibrate.py index e64d4a8a5..49b65406b 100755 --- a/calibrate.py +++ b/calibrate.py @@ -408,6 +408,8 @@ def __init__(self): len(calibUtils.setPolygonCoordinates(1000, 600)) if debug: print("Using Arguments=", self.args) + if self.args.datasetPath: + Path(self.args.datasetPath).mkdir(parents=True, exist_ok=True) # if self.args.board.upper() == 'OAK-D-LITE': # raise Exception( @@ -529,8 +531,10 @@ 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) - print("py: Saved image as: " + str(filename)) + path = Path(self.args.datasetPath) / stream_name / filename + path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(path), frame) + print("py: Saved image as: " + str(path)) return True def show_info_frame(self): @@ -1101,6 +1105,7 @@ def calibrate(self): calib_dest_path = dest_path + '/' + mx_serial_id + '.json' calibration_handler.eepromToJsonFile(calib_dest_path) if self.args.saveCalibPath: + Path(self.args.saveCalibPath).parent.mkdir(parents=True, exist_ok=True) calibration_handler.eepromToJsonFile(self.args.saveCalibPath) # try: self.device.flashCalibration2(calibration_handler) From 6d74171e2fdfd44d979b25f42933e8674fb2a96c Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Wed, 19 Apr 2023 20:33:00 +0200 Subject: [PATCH 40/68] Add an option to manually specify the number of charucos --- calibrate.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index 49b65406b..c9769109e 100755 --- a/calibrate.py +++ b/calibrate.py @@ -175,6 +175,7 @@ def parse_args(): help="Path to dataset used for processing images") parser.add_argument('-mdmp', '--minDetectedMarkersPercent', type=float, default=0.5, help="Minimum percentage of detected markers to consider a frame valid") + parser.add_argument('-nm', '--numMarkers', type=int, default=None, help="Number of markers in the board") parser.add_argument('-mt', '--mouseTrigger', default=False, action="store_true", help="Enable mouse trigger for image capture") parser.add_argument('-nic', '--noInitCalibration', default=False, action="store_true", @@ -459,7 +460,10 @@ def is_markers_found(self, frame): marker_corners, _, _ = cv2.aruco.detectMarkers( frame, self.aruco_dictionary) print("Markers count ... {}".format(len(marker_corners))) - num_all_markers = (self.args.squaresX-1)*(self.args.squaresY-1) / 2 + if not self.args.numMarkers: + num_all_markers = (self.args.squaresX-1)*(self.args.squaresY-1) / 2 + else: + num_all_markers = self.args.numMarkers return not (len(marker_corners) < (num_all_markers * self.args.minDetectedMarkersPercent)) def test_camera_orientation(self, frame_l, frame_r): From cfff50fdbb66b055d6a5318708dde1d4636d0b0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nejc=20Jezer=C5=A1ek?= Date: Wed, 26 Apr 2023 13:54:50 +0200 Subject: [PATCH 41/68] Fixed default dataset path --- calibrate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index c9769109e..b549409cd 100755 --- a/calibrate.py +++ b/calibrate.py @@ -171,7 +171,7 @@ def parse_args(): help="Set antibanding/antiflicker algo for lights that flicker at mains frequency. Default: %(default)s [Hz]") parser.add_argument('-scp', '--saveCalibPath', type=str, default="", help="Save calibration file to this path") - parser.add_argument('-dst', '--datasetPath', type=str, default="", + parser.add_argument('-dst', '--datasetPath', type=str, default="dataset", help="Path to dataset used for processing images") parser.add_argument('-mdmp', '--minDetectedMarkersPercent', type=float, default=0.5, help="Minimum percentage of detected markers to consider a frame valid") From ed1a21d35c37efd1c4e5b5a5c8cae8bae47eed92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nejc=20Jezer=C5=A1ek?= Date: Wed, 3 May 2023 08:52:13 +0200 Subject: [PATCH 42/68] Support for GRAY8 img type, fix scaling issues --- calibrate.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index b549409cd..8f4480928 100755 --- a/calibrate.py +++ b/calibrate.py @@ -639,7 +639,7 @@ def capture_images_sync(self): syncCollector.add_msg(key, frameMsg) gray_frame = None - if frameMsg.getType() == dai.RawImgFrame.Type.RAW8: + if frameMsg.getType() in [dai.RawImgFrame.Type.RAW8, dai.RawImgFrame.Type.GRAY8] : gray_frame = frameMsg.getCvFrame() else: gray_frame = cv2.cvtColor(frameMsg.getCvFrame(), cv2.COLOR_BGR2GRAY) @@ -726,6 +726,13 @@ def capture_images_sync(self): # self.current_polygon + 1, self.images_captured, self.total_images, name), # (0, 700), cv2.FONT_HERSHEY_TRIPLEX, 1.0, (255, 0, 0)) + + height, width = imgFrame.shape + # TO-DO: fix the rooquick and dirty fix: if the resized image is higher than the target resolution, crop it + if height > resizeHeight: + height_offset = (height - resizeHeight)//2 + imgFrame = imgFrame[height_offset:height_offset+resizeHeight, :] + height, width = imgFrame.shape height_offset = (resizeHeight - height)//2 width_offset = (resizeWidth - width)//2 From 316526a25345203a6f3da90f8daecfdca0032bf2 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Fri, 12 May 2023 15:16:14 +0200 Subject: [PATCH 43/68] Lower the threshold for synced frames --- calibrate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrate.py b/calibrate.py index 8f4480928..b1162a313 100755 --- a/calibrate.py +++ b/calibrate.py @@ -628,7 +628,7 @@ def capture_images_sync(self): curr_time = None self.display_name = "Image Window" - syncCollector = MessageSync(len(self.camera_queue), 10) + syncCollector = MessageSync(len(self.camera_queue), 0.003) # 3ms tolerance self.mouseTrigger = False while not finished: currImageList = {} From 3a337969efeedac07e9d0c33ab742fb0cbc760af Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 15 May 2023 13:51:50 +0200 Subject: [PATCH 44/68] Remove the CALIB_CHECK_COND --- depthai_helpers/calibration_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 890ae853e..0177d68ce 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -490,7 +490,7 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): # flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW + flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_FIX_SKEW distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + From bc9f90d9f45a0dc51117559ecd739730c7ffa3a6 Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 17 May 2023 14:52:18 -0700 Subject: [PATCH 45/68] Cleared old opencv dependencies versioning --- requirements.txt | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/requirements.txt b/requirements.txt index cf55a7230..cf7cfb4ee 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,14 +1,9 @@ requests==2.26.0 --extra-index-url https://www.piwheels.org/simple -opencv-python==4.5.4.58 ; platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" -opencv-python==4.5.1.48 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version != "3.10" -opencv-contrib-python==4.5.4.58 ; platform_machine != "armv6l" and platform_machine != "armv7l" and python_version == "3.10" -opencv-contrib-python==4.5.1.48 ; platform_machine != "aarch64" and platform_machine != "armv6l" and platform_machine != "armv7l" and python_version != "3.10" -opencv-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" -opencv-contrib-python==4.4.0.46 ; platform_machine == "armv6l" or platform_machine == "armv7l" +opencv-contrib-python==4.5.4.58 scipy --e ./depthai_sdk ---extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ -pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" ---extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file +# -e ./depthai_sdk +# --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ +# pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" +# --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ +# depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file From 53445f2a08a9bc51c3a604a35592f01fa22f8789 Mon Sep 17 00:00:00 2001 From: Sachin Guruswamy <43363595+saching13@users.noreply.github.com> Date: Fri, 26 May 2023 19:07:47 -0700 Subject: [PATCH 46/68] Peripheral calibration rvc3 (#1040) * if fisheye changed the requiement to need all the corners on the board * added coverage window * added charuco cornes and coverage visualization * small improvements * added marker visualization for capture * removed merge datasets * reprojection error and cover image * removed unused name --- calibrate.py | 161 ++++++++++++++++++++------- depthai_helpers/calibration_utils.py | 62 +++++++++-- reproject_error_fisheye.py | 76 +++++++++++++ resources/boards/TESTVER_NO_RGB.json | 55 +++++++++ 4 files changed, 306 insertions(+), 48 deletions(-) create mode 100644 reproject_error_fisheye.py create mode 100644 resources/boards/TESTVER_NO_RGB.json diff --git a/calibrate.py b/calibrate.py index b1162a313..75403b3c5 100755 --- a/calibrate.py +++ b/calibrate.py @@ -12,6 +12,7 @@ from scipy.spatial.transform import Rotation import traceback import itertools +import math import cv2 from cv2 import resize @@ -416,6 +417,14 @@ def __init__(self): # raise Exception( # "OAK-D-Lite Calibration is not supported on main yet. Please use `lite_calibration` branch to calibrate your OAK-D-Lite!!") + if self.args.cameraMode != "perspective": + self.args.minDetectedMarkersPercent = 1 + + self.coverageImages ={} + for cam_id in self.board_config['cameras']: + name = self.board_config['cameras'][cam_id]['name'] + self.coverageImages[name] = None + self.device = dai.Device() cameraProperties = self.device.getConnectedCameraFeatures() for properties in cameraProperties: @@ -428,7 +437,13 @@ def __init__(self): # self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check() break - + self.charuco_board = cv2.aruco.CharucoBoard_create( + self.args.squaresX, self.args.squaresY, + self.args.squareSizeCm, + self.args.markerSizeCm, + self.aruco_dictionary) + + """ cameraProperties = self.device.getConnectedCameraProperties() @@ -461,11 +476,45 @@ def is_markers_found(self, frame): frame, self.aruco_dictionary) print("Markers count ... {}".format(len(marker_corners))) if not self.args.numMarkers: - num_all_markers = (self.args.squaresX-1)*(self.args.squaresY-1) / 2 + num_all_markers = math.floor(self.args.squaresX * self.args.squaresY / 2) else: num_all_markers = self.args.numMarkers + print(f'Total mariers needed -> {(num_all_markers * self.args.minDetectedMarkersPercent)}') return not (len(marker_corners) < (num_all_markers * self.args.minDetectedMarkersPercent)) + def detect_markers_corners(self, frame): + marker_corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(frame, self.aruco_dictionary) + marker_corners, ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(frame, self.charuco_board, + marker_corners, ids, + rejectedCorners=rejectedImgPoints) + if len(marker_corners) <= 0: + return marker_corners, ids, None, None + ret, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco(marker_corners, ids, frame, self.charuco_board) + return marker_corners, ids, charuco_corners, charuco_ids + + def draw_markers(self, frame): + marker_corners, ids, charuco_corners, charuco_ids = self.detect_markers_corners(frame) + if charuco_ids is not None and len(charuco_ids) > 0: + return cv2.aruco.drawDetectedCornersCharuco(frame, charuco_corners, charuco_ids, (0, 255, 0)); + return frame + + def draw_corners(self, frame, displayframe, color): + marker_corners, ids, charuco_corners, charuco_ids = self.detect_markers_corners(frame) + for corner in charuco_corners: + corner_int = (int(corner[0][0]), int(corner[0][1])) + cv2.circle(displayframe, corner_int, 8, color, -1) + height, width = displayframe.shape[:2] + start_point = (0, 0) # top of the image + end_point = (0, height) + + color = (0, 0, 0) # blue in BGR + thickness = 4 + + # Draw the line on the image + cv2.line(displayframe, start_point, end_point, color, thickness) + return displayframe + # return cv2.aruco.drawDetectedCornersCharuco(displayframe, charuco_corners) + def test_camera_orientation(self, frame_l, frame_r): marker_corners_l, id_l, _ = cv2.aruco.detectMarkers( frame_l, self.aruco_dictionary) @@ -574,7 +623,12 @@ def show_failed_capture_frame(self): width, height = int( self.width * self.output_scale_factor), int(self.height * self.output_scale_factor) info_frame = np.zeros((self.height, self.width, 3), np.uint8) - print("py: Capture failed, unable to find chessboard! Fix position and press spacebar again") + if self.args.cameraMode != "perspective": + print("py: Capture failed, unable to find full board! Fix position and press spacebar again") + else: + print("py: Capture failed, unable to find chessboard! Fix position and press spacebar again") + + def show(position, text): cv2.putText(info_frame, text, position, @@ -638,12 +692,12 @@ def capture_images_sync(self): # print(f'Timestamp of {key} is {frameMsg.getTimestamp()}') syncCollector.add_msg(key, frameMsg) - gray_frame = None + color_frame = None if frameMsg.getType() in [dai.RawImgFrame.Type.RAW8, dai.RawImgFrame.Type.GRAY8] : - gray_frame = frameMsg.getCvFrame() + color_frame = cv2.cvtColor(frameMsg.getCvFrame(), cv2.COLOR_GRAY2BGR) else: - gray_frame = cv2.cvtColor(frameMsg.getCvFrame(), cv2.COLOR_BGR2GRAY) - currImageList[key] = gray_frame + color_frame = frameMsg.getCvFrame() + currImageList[key] = color_frame # print(gray_frame.shape) resizeHeight = 0 @@ -651,19 +705,18 @@ def capture_images_sync(self): for name, imgFrame in currImageList.items(): # print(f'original Shape of {name} is {imgFrame.shape}' ) - currImageList[name] = cv2.resize( - imgFrame, (0, 0), fx=self.output_scale_factor, fy=self.output_scale_factor) + + currImageList[name] = cv2.resize(self.draw_markers(imgFrame), + (0, 0), + fx=self.output_scale_factor, + fy=self.output_scale_factor) - height, width = currImageList[name].shape + height, width, _ = currImageList[name].shape widthRatio = resizeWidth / width heightRatio = resizeHeight / height - # if widthRatio > 1.0 and heightRatio > 1.0 and widthRatio < 1.2 and heightRatio < 1.2: - # continue - - if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0): resizeWidth = width resizeHeight = height @@ -678,15 +731,16 @@ def capture_images_sync(self): # print(f'Scale Shape is {resizeWidth}x{resizeHeight}' ) combinedImage = None + combinedCoverageImage = None for name, imgFrame in currImageList.items(): - height, width = imgFrame.shape + height, width, _ = imgFrame.shape if width > resizeWidth and height > resizeHeight: imgFrame = cv2.resize( imgFrame, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width) # print(f'final_scaledImageSize is {imgFrame.shape}') if self.polygons is None: - self.height, self.width = imgFrame.shape + self.height, self.width, _ = imgFrame.shape # print(self.height, self.width) self.polygons = calibUtils.setPolygonCoordinates( self.height, self.width) @@ -713,35 +767,34 @@ def capture_images_sync(self): localPolygon[0][:, 1] += (height - abs(localPolygon[0][:, 1].max())) localPolygon[0][:, 0] += abs(localPolygon[0][:, 1].min()) - # print(localPolygon) - # print(localPolygon.shape) cv2.polylines( imgFrame, localPolygon, True, (0, 0, 255), 4) - # TODO(Sachin): Add this back with proper alignment - # cv2.putText( - # imgFrame, - # "Polygon Position: {}. Captured {} of {} {} images".format( - # self.current_polygon + 1, self.images_captured, self.total_images, name), - # (0, 700), cv2.FONT_HERSHEY_TRIPLEX, 1.0, (255, 0, 0)) - - - height, width = imgFrame.shape + height, width, _ = imgFrame.shape # TO-DO: fix the rooquick and dirty fix: if the resized image is higher than the target resolution, crop it if height > resizeHeight: height_offset = (height - resizeHeight)//2 imgFrame = imgFrame[height_offset:height_offset+resizeHeight, :] - height, width = imgFrame.shape + height, width, _ = imgFrame.shape height_offset = (resizeHeight - height)//2 width_offset = (resizeWidth - width)//2 - subImage = np.pad(imgFrame, ((height_offset, height_offset), (width_offset,width_offset)), 'constant', constant_values=0) + subImage = np.pad(imgFrame, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0) + if self.coverageImages[name] is not None: + currCoverImage = cv2.resize(self.coverageImages[name], (0, 0), fx=self.output_scale_factor, fy=self.output_scale_factor) + padding = ((height_offset, height_offset), (width_offset,width_offset), (0, 0)) + subCoverageImage = np.pad(currCoverImage, padding, 'constant', constant_values=0) + if combinedCoverageImage is None: + combinedCoverageImage = subCoverageImage + else: + combinedCoverageImage = np.hstack((combinedCoverageImage, subCoverageImage)) + + if combinedImage is None: combinedImage = subImage else: combinedImage = np.hstack((combinedImage, subImage)) - key = cv2.waitKey(1) if key == 27 or key == ord("q"): print("py: Calibration has been interrupted!") @@ -752,6 +805,7 @@ def capture_images_sync(self): timer = self.args.captureDelay self.mouseTrigger = False + display_image = combinedImage if start_timer == True: curr_time = time.time() if curr_time - prev_time >= 1: @@ -763,15 +817,19 @@ def capture_images_sync(self): print('Start capturing...') image_shape = combinedImage.shape - cv2.putText(combinedImage, str(timer), + + cv2.putText(display_image, str(timer), (image_shape[1]//2, image_shape[0]//2), font, - 7, (0, 255, 255), + 7, (0, 0, 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) + cv2.imshow(self.display_name, display_image) + if combinedCoverageImage is not None: + cv2.imshow("Coverage-Image", combinedCoverageImage) + tried = {} allPassed = True @@ -780,13 +838,21 @@ def capture_images_sync(self): if syncedMsgs == False or syncedMsgs == None: for key in self.camera_queue.keys(): self.camera_queue[key].getAll() - continue + continue for name, frameMsg in syncedMsgs.items(): print(f"Time stamp of {name} is {frameMsg.getTimestamp()}") + if self.coverageImages[name] is None: + coverageShape = frameMsg.getCvFrame().shape + self.coverageImages[name] = np.ones(coverageShape, np.uint8) * 255 + tried[name] = self.parse_frame(frameMsg.getCvFrame(), name) + print(f'Status of {name} is {tried[name]}') allPassed = allPassed and tried[name] if allPassed: + color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255))) + for name, frameMsg in syncedMsgs.items(): + self.coverageImages[name] = self.draw_corners(frameMsg.getCvFrame(), self.coverageImages[name], color) if not self.images_captured: leftStereo = self.board_config['cameras'][self.board_config['stereo_config']['left_cam']]['name'] rightStereo = self.board_config['cameras'][self.board_config['stereo_config']['right_cam']]['name'] @@ -815,7 +881,7 @@ def capture_images_sync(self): break - def capture_images(self): + """ def capture_images(self): finished = False capturing = False captured_left = False @@ -1004,7 +1070,7 @@ def capture_images(self): 7, (0, 255, 255), 4, cv2.LINE_AA) cv2.imshow(self.display_name, combine_img) - frame_list.clear() + frame_list.clear() """ def calibrate(self): print("Starting image processing") @@ -1038,15 +1104,19 @@ def calibrate(self): print(e) print(traceback.format_exc()) raise SystemExit(1) - + + target_file = open('./dataset/target_info.txt', 'w') # calibration_handler.set error_text = [] + + target_file.write(f'Marker Size: {self.args.markerSizeCm} cm\n') + target_file.write(f'Square Size: {self.args.squareSizeCm} cm\n') + target_file.write(f'Number of squaresX: {self.args.squaresX}\n') + target_file.write(f'Number of squaresY: {self.args.squaresY}\n') for camera in result_config['cameras'].keys(): cam_info = result_config['cameras'][camera] # log_list.append(self.ccm_selected[cam_info['name']]) - - color = green reprojection_error_threshold = 1.0 if cam_info['size'][1] > 720: print(cam_info['size'][1]) @@ -1061,6 +1131,9 @@ def calibrate(self): error_text.append("high Reprojection Error") text = cam_info['name'] + ' Reprojection Error: ' + format(cam_info['reprojection_error'], '.6f') print(text) + text = cam_info['name'] + '-reprojection: {}\n'.format(cam_info['reprojection_error'], '.6f') + target_file.write(text) + # pygame_render_text(self.screen, text, (vis_x, vis_y), color, 30) calibration_handler.setDistortionCoefficients(stringToCam[camera], cam_info['dist_coeff']) @@ -1068,7 +1141,7 @@ def calibrate(self): calibration_handler.setFov(stringToCam[camera], cam_info['hfov']) if self.args.cameraMode != 'perspective': calibration_handler.setCameraType(stringToCam[camera], dai.CameraModel.Fisheye) - if cam_info['hasAutofocus']: + if 'hasAutofocus' in cam_info and cam_info['hasAutofocus']: calibration_handler.setLensPosition(stringToCam[camera], self.focus_value) # log_list.append(self.focusSigma[cam_info['name']]) @@ -1090,6 +1163,9 @@ def calibrate(self): color = red error_text.append("Epiploar validation failed between " + left_cam + " and " + right_cam) + text = cam_info['name'] + " and " + right_cam + ' epipolar_error: {}\n'.format(cam_info['extrinsics']['epipolar_error'], '.6f') + target_file.write(text) + # log_list.append(cam_info['extrinsics']['epipolar_error']) # text = left_cam + "-" + right_cam + ' Avg Epipolar error: ' + format(cam_info['extrinsics']['epipolar_error'], '.6f') # pygame_render_text(self.screen, text, (vis_x, vis_y), color, 30) @@ -1103,6 +1179,7 @@ def calibrate(self): elif result_config['stereo_config']['left_cam'] == cam_info['extrinsics']['to_cam'] and result_config['stereo_config']['right_cam'] == camera: calibration_handler.setStereoRight(stringToCam[camera], result_config['stereo_config']['rectification_right']) calibration_handler.setStereoLeft(stringToCam[cam_info['extrinsics']['to_cam']], result_config['stereo_config']['rectification_left']) + target_file.close() if len(error_text) == 0: print('Flashing Calibration data into ') @@ -1113,7 +1190,9 @@ def calibrate(self): eeepromData.version = 7 print(f'EEPROM VERSION being flashed is -> {eeepromData.version}') mx_serial_id = self.device.getDeviceInfo().getMxId() - calib_dest_path = dest_path + '/' + mx_serial_id + '.json' + date_time_string = datetime.now().strftime("_%m_%d_%y_%H_%M") + file_name = mx_serial_id + date_time_string + calib_dest_path = dest_path + '/' + file_name + '.json' calibration_handler.eepromToJsonFile(calib_dest_path) if self.args.saveCalibPath: Path(self.args.saveCalibPath).parent.mkdir(parents=True, exist_ok=True) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 0177d68ce..a30961c3e 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -112,14 +112,15 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ # parameters = aruco.DetectorParameters_create() assert mrk_size != None, "ERROR: marker size not set" - + combinedCoverageImage = None for camera in board_config['cameras'].keys(): cam_info = board_config['cameras'][camera] print( '<------------Calibrating {} ------------>'.format(cam_info['name'])) images_path = filepath + '/' + cam_info['name'] - ret, intrinsics, dist_coeff, _, _, size = self.calibrate_intrinsics( + ret, intrinsics, dist_coeff, _, _, size, coverageImage = self.calibrate_intrinsics( images_path, cam_info['hfov']) + # coverageImages[cam_info['name']] = coverageImage cam_info['intrinsics'] = intrinsics cam_info['dist_coeff'] = dist_coeff cam_info['size'] = size # (Width, height) @@ -130,6 +131,21 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ cam_info['name'], ret)) print("Estimated intrinsics of {0}: \n {1}".format( cam_info['name'], intrinsics)) + + coverage_name = cam_info['name'] + print_text = f'Coverage Image of {coverage_name} with reprojection error of {ret}' + cv2.putText(coverageImage, print_text, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 0), 2) + # coverageImages[coverage_name] = coverageImage + + if combinedCoverageImage is None: + combinedCoverageImage = coverageImage + else: + combinedCoverageImage = np.hstack((combinedCoverageImage, coverageImage)) + + combinedCoverageImage = cv2.resize(combinedCoverageImage, (0, 0), fx=0.7, fy=0.7) + cv2.imshow('coverage image', combinedCoverageImage) + cv2.waitKey(0) + cv2.destroyAllWindows() for camera in board_config['cameras'].keys(): left_cam_info = board_config['cameras'][camera] @@ -195,6 +211,23 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ return 1, board_config + def draw_corners(self, charuco_corners, displayframe): + for corners in charuco_corners: + color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255))) + for corner in corners: + corner_int = (int(corner[0][0]), int(corner[0][1])) + cv2.circle(displayframe, corner_int, 4, color, -1) + height, width = displayframe.shape[:2] + start_point = (0, 0) # top of the image + end_point = (0, height) + + color = (0, 0, 0) # blue in BGR + thickness = 4 + + # Draw the line on the image + cv2.line(displayframe, start_point, end_point, color, thickness) + return displayframe + def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): """ Charuco base pose estimation. @@ -280,6 +313,15 @@ def calibrate_intrinsics(self, image_files, hfov): image_files) != 0, "ERROR: Images not read correctly, check directory" allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files) + # print(imsize) + # coverageShape = (imsize[0], imsize[1], 3) + print(imsize) + print(imsize[-1]) + print(imsize[::-1]) + coverageImage = np.ones(imsize[::-1], np.uint8) * 255 + coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR) + coverageImage = self.draw_corners(allCorners, coverageImage) + if self.cameraModel == 'perspective': ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_charuco( allCorners, allIds, imsize, hfov) @@ -288,7 +330,7 @@ def calibrate_intrinsics(self, image_files, hfov): self.fisheye_undistort_visualizaation( image_files, camera_matrix, distortion_coefficients, imsize) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage else: print('Fisheye--------------------------------------------------') ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_fisheye( @@ -296,10 +338,11 @@ def calibrate_intrinsics(self, image_files, hfov): if traceLevel == 3: self.fisheye_undistort_visualizaation( image_files, camera_matrix, distortion_coefficients, imsize) - + print('Fisheye rotation vector', rotation_vectors[0]) + print('Fisheye translation vector', translation_vectors[0]) # (Height, width) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize + return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): self.objpoints = [] # 3d point in real world space @@ -977,6 +1020,11 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0) # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) kScaledR = kScaledL = M_r + + if self.cameraModel != 'perspective': + kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1) + kScaledL = kScaledR + if rectProjectionMode: kScaledL = p_lp kScaledR = p_rp @@ -1104,8 +1152,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, marker_corners_r, ids_r, rejectedCorners=rejectedImgPoints) - print(f'Marekrs length r is {len(marker_corners_r)}') - print(f'Marekrs length l is {len(marker_corners_l)}') + # print(f'Marekrs length r is {len(marker_corners_r)}') + # print(f'Marekrs length l is {len(marker_corners_l)}') res2_l = cv2.aruco.interpolateCornersCharuco( marker_corners_l, ids_l, image_data_pair[0], self.board) res2_r = cv2.aruco.interpolateCornersCharuco( diff --git a/reproject_error_fisheye.py b/reproject_error_fisheye.py new file mode 100644 index 000000000..1d9e7c059 --- /dev/null +++ b/reproject_error_fisheye.py @@ -0,0 +1,76 @@ +import glob +import cv2 +import numpy as np +import cv2.aruco as aruco +import depthai as dai +from pathlib import Path + +def detect_markers_corners(frame): + marker_corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(frame, aruco_dictionary) + marker_corners, ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(frame, charuco_board, + marker_corners, ids, + rejectedCorners=rejectedImgPoints) + if len(marker_corners) <= 0: + return marker_corners, ids, None, None + ret, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco(marker_corners, ids, frame, charuco_board) + return marker_corners, ids, charuco_corners, charuco_ids + +size = (1920, 1200) + +# Load the images +calibration_handler = dai.CalibrationHandler('./resources/1844301021621CF500_05_26_23_16_31.json') +images = glob.glob('./dataset_cam7_may_18_merged/left/*.png') + +# Prepare for camera calibration +k = np.array(calibration_handler.getCameraIntrinsics(calibration_handler.getStereoLeftCameraId(), size[0], size[1])) +D_left = np.array(calibration_handler.getDistortionCoefficients(calibration_handler.getStereoLeftCameraId())) +d = np.array(calibration_handler.getDistortionCoefficients(calibration_handler.getStereoLeftCameraId())) +r = np.eye(3, dtype=np.float32) + +d_zero = np.zeros((14, 1), dtype=np.float32) +M_focal = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(k, d, size, np.eye(3), fov_scale=1.1) +mapXL, mapYL = cv2.fisheye.initUndistortRectifyMap(k, d[:4], r, M_focal, size, cv2.CV_32FC1) + +aruco_dictionary = aruco.Dictionary_get(cv2.aruco.DICT_4X4_1000) +charuco_board = aruco.CharucoBoard_create( + 11, + 8, + 6.0, + 4.6, + aruco_dictionary) +checkCorners3D = charuco_board.chessboardCorners +rvec = np.array([[0.0], + [0.0], + [0.0]], dtype=np.float32) + +tvec = np.array([[0.0], + [0.0], + [0.0]], dtype=np.float32) +# count = 0 +for im_name in images: + ## Undistort it first + path = Path(im_name) + img = cv2.imread(im_name) + img_rect = cv2.remap(img , mapXL, mapYL, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + + marker_corners, ids, charuco_corners, charuco_ids = detect_markers_corners(img_rect) + ret, rvec, tvec = aruco.estimatePoseCharucoBoard(charuco_corners, charuco_ids, charuco_board, M_focal, d_zero, rvec, tvec) + + """ if len(charuco_ids) != len(checkCorners3D): + print('charuco_ids shape -> ', charuco_ids.shape) + print('charuco_corners shape -> ', charuco_corners.shape) + print('checkCorners3D shape -> ', checkCorners3D.shape) + raise ValueError("Number of ids does not match number of original corners") """ + + points_2d, _ = cv2.fisheye.projectPoints(checkCorners3D[None], rvec, tvec, k, d) + undistorted_points_2d = cv2.fisheye.undistortPoints(points_2d, k, d, R = r, P = M_focal) + # print("undistorted_points_2d shape -> ", undistorted_points_2d.shape) + # print("charuco_corners shape -> ", charuco_corners.shape) + error = 0 + for i in range(len(charuco_ids)): + error_vec = charuco_corners[i][0] - undistorted_points_2d[0][charuco_ids[i]] + error += np.linalg.norm(error_vec) + # print(im_name) + print(f'Reprojection error is {error / len(charuco_ids)} avg of {len(charuco_ids)} of points in file {path.name} ') + + diff --git a/resources/boards/TESTVER_NO_RGB.json b/resources/boards/TESTVER_NO_RGB.json new file mode 100644 index 000000000..a55e1908f --- /dev/null +++ b/resources/boards/TESTVER_NO_RGB.json @@ -0,0 +1,55 @@ +{ + "board_config": + { + "name": "TESTVER", + "revision": "R0M0E0", + "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", + "extrinsics": { + "to_cam": "CAM_D", + "specTranslation": { + "x": 0, + "y": -4.0, + "z": 0 + }, + "rotation":{ + "r": 0, + "p": 0, + "y": 0 + } + } + }, + "CAM_D": { + "name": "vertical", + "hfov": 71.86, + "type": "color" + } + }, + "stereo_config":{ + "left_cam": "CAM_B", + "right_cam": "CAM_C" + } + } +} + From e4e7bea4350c3b2b434b17549447f92666e13f00 Mon Sep 17 00:00:00 2001 From: saching13 Date: Mon, 29 May 2023 16:38:10 -0700 Subject: [PATCH 47/68] fixed the requirements.txt --- install_requirements.py | 4 ---- requirements.txt | 4 ++-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/install_requirements.py b/install_requirements.py index 609d189a9..50ef8e5c7 100755 --- a/install_requirements.py +++ b/install_requirements.py @@ -27,10 +27,6 @@ if sys.version_info[0] != 3: raise RuntimeError("Demo script requires Python 3 to run (detected: Python {})".format(sys.version_info[0])) -if platform.machine() == "arm64" and platform.system() == "Darwin": - err_str = "There are no prebuilt wheels for M1 processors. Please open the following link for a solution - https://discuss.luxonis.com/d/69-running-depthai-on-apple-m1-based-macs" - raise RuntimeError(err_str) - is_pi = thisPlatform.startswith("arm") prebuiltWheelsPythonVersion = [7,9] if is_pi and sys.version_info[1] not in prebuiltWheelsPythonVersion: diff --git a/requirements.txt b/requirements.txt index cf7cfb4ee..2bc003bc3 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,5 +5,5 @@ scipy # -e ./depthai_sdk # --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ # pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" -# --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -# depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file +--extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ +depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file From 8601c5170c80a989e413e1d9f86cd92e81fc3aaa Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 31 May 2023 13:22:15 -0700 Subject: [PATCH 48/68] saving converge files --- .gitignore | 2 +- calibrate.py | 2 +- depthai_helpers/calibration_utils.py | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index ddcc135ce..9e276b04f 100644 --- a/.gitignore +++ b/.gitignore @@ -30,7 +30,7 @@ share/python-wheels/ MANIFEST *.idea # DepthAI-Specific -dataset* +*dataset* .fw_cache/ depthai.calib mesh_left.calib diff --git a/calibrate.py b/calibrate.py index 75403b3c5..69bdf52df 100755 --- a/calibrate.py +++ b/calibrate.py @@ -1105,7 +1105,7 @@ def calibrate(self): print(traceback.format_exc()) raise SystemExit(1) - target_file = open('./dataset/target_info.txt', 'w') + target_file = open(self.dataset_path + '/target_info.txt', 'w') # calibration_handler.set error_text = [] diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index a30961c3e..8b94a9092 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -136,7 +136,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ print_text = f'Coverage Image of {coverage_name} with reprojection error of {ret}' cv2.putText(coverageImage, print_text, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 0), 2) # coverageImages[coverage_name] = coverageImage - + coverage_file_path = filepath + '/' + coverage_name + '_coverage.png' + cv2.imwrite(coverage_file_path, coverageImage) if combinedCoverageImage is None: combinedCoverageImage = coverageImage else: From d5da79f5a27c7e1ecf9f81791e32cf82f0231200 Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 14 Jun 2023 14:13:58 +0200 Subject: [PATCH 49/68] modified calibration parameters --- calibrate.py | 4 ++-- depthai_helpers/calibration_utils.py | 8 +++----- requirements.txt | 2 +- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/calibrate.py b/calibrate.py index 69bdf52df..64ce0a286 100755 --- a/calibrate.py +++ b/calibrate.py @@ -479,7 +479,7 @@ def is_markers_found(self, frame): num_all_markers = math.floor(self.args.squaresX * self.args.squaresY / 2) else: num_all_markers = self.args.numMarkers - print(f'Total mariers needed -> {(num_all_markers * self.args.minDetectedMarkersPercent)}') + print(f'Total markers needed -> {(num_all_markers * self.args.minDetectedMarkersPercent)}') return not (len(marker_corners) < (num_all_markers * self.args.minDetectedMarkersPercent)) def detect_markers_corners(self, frame): @@ -641,7 +641,7 @@ def show(position, text): # cv2.imshow("left", info_frame) # cv2.imshow("right", info_frame) cv2.imshow(self.display_name, info_frame) - cv2.waitKey(2000) + cv2.waitKey(1000) def show_failed_orientation(self): width, height = int( diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 8b94a9092..109b9adec 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -530,12 +530,10 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): 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_CHECK_COND flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_FIX_SKEW - + flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + # flags |= cv2.fisheye.CALIB_FIX_SKEW distCoeffsInit = np.zeros((4, 1)) term_criteria = (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 50000, 1e-9) diff --git a/requirements.txt b/requirements.txt index 2bc003bc3..cd212f3bb 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,4 +6,4 @@ scipy # --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ # pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -depthai===2.19.1.0.dev0+399fffe13be71aeadf3385200ffa3bba9b71490d \ No newline at end of file +depthai===2.19.1.0.dev0+0f2e70d9067fec82e709cd1543621cb76a01a44c \ No newline at end of file From 469d6dfceaeaae2cafa4404edc4a120802aed2bd Mon Sep 17 00:00:00 2001 From: saching13 Date: Wed, 14 Jun 2023 14:15:48 +0200 Subject: [PATCH 50/68] added json for no vertical testing --- resources/boards/TESTVER_NO_VERTICAL.json | 37 +++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 resources/boards/TESTVER_NO_VERTICAL.json diff --git a/resources/boards/TESTVER_NO_VERTICAL.json b/resources/boards/TESTVER_NO_VERTICAL.json new file mode 100644 index 000000000..01fcc28f8 --- /dev/null +++ b/resources/boards/TESTVER_NO_VERTICAL.json @@ -0,0 +1,37 @@ +{ + "board_config": + { + "name": "TESTVER", + "revision": "R0M0E0", + "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" + } + } +} + From d7af1adfeae1b1170fa1afb547ba9b54b99262c3 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 23 Jun 2023 00:20:00 +0200 Subject: [PATCH 51/68] Changes --- calibrate.py | 22 +++++++++++++------ depthai_helpers/calibration_utils.py | 17 +++++++++----- .../integrations/depthai_pipeline_graph | 1 + resources/depthai_boards | 1 + 4 files changed, 28 insertions(+), 13 deletions(-) create mode 160000 depthai_sdk/src/depthai_sdk/integrations/depthai_pipeline_graph create mode 160000 resources/depthai_boards diff --git a/calibrate.py b/calibrate.py index 64ce0a286..4cd5269de 100755 --- a/calibrate.py +++ b/calibrate.py @@ -181,6 +181,8 @@ def parse_args(): help="Enable mouse trigger for image capture") parser.add_argument('-nic', '--noInitCalibration', default=False, action="store_true", help="Don't take the board calibration for initialization but start with an empty one") + parser.add_argument('-dismin', '--displayMinimum', default=False, action="store_true", + help="Display the new minimum every second while taking pictures") options = parser.parse_args() @@ -315,7 +317,7 @@ def add_msg(self, name, msg): # print() # print() - def get_synced(self): + def get_synced(self, displayMinimum): # Atleast 3 messages should be buffered min_len = min([len(queue) for queue in self.queues.values()]) @@ -353,7 +355,8 @@ def get_synced(self): # Mark minimum if min_ts_diff is None or (acc_diff < min_ts_diff['ts'] and abs(acc_diff - min_ts_diff['ts']) > 0.0001): min_ts_diff = {'ts': acc_diff, 'indicies': indicies.copy()} - print('new minimum:', min_ts_diff, 'min required:', self.min_diff_timestamp) + if displayMinimum: + print('new minimum:', min_ts_diff, 'min required:', self.min_diff_timestamp) if min_ts_diff['ts'] < self.min_diff_timestamp: # Check if atleast 5 messages deep @@ -388,6 +391,7 @@ def __init__(self): global debug self.args = parse_args() debug = self.args.debug + displayMinimum=self.args.displayMinimum self.output_scale_factor = self.args.outputScaleFactor self.aruco_dictionary = cv2.aruco.Dictionary_get( cv2.aruco.DICT_4X4_1000) @@ -680,7 +684,7 @@ def capture_images_sync(self): timer = self.args.captureDelay prev_time = None curr_time = None - + displayMinimum= self.args.displayMinimum self.display_name = "Image Window" syncCollector = MessageSync(len(self.camera_queue), 0.003) # 3ms tolerance self.mouseTrigger = False @@ -689,7 +693,7 @@ def capture_images_sync(self): for key in self.camera_queue.keys(): frameMsg = self.camera_queue[key].get() - # print(f'Timestamp of {key} is {frameMsg.getTimestamp()}') + #print(f'Timestamp of {key} is {frameMsg.getTimestamp()}') syncCollector.add_msg(key, frameMsg) color_frame = None @@ -703,6 +707,7 @@ def capture_images_sync(self): resizeHeight = 0 resizeWidth = 0 for name, imgFrame in currImageList.items(): + self.coverageImages[name]=None # print(f'original Shape of {name} is {imgFrame.shape}' ) @@ -832,9 +837,8 @@ def capture_images_sync(self): tried = {} allPassed = True - if capturing: - syncedMsgs = syncCollector.get_synced() + syncedMsgs = syncCollector.get_synced(displayMinimum) if syncedMsgs == False or syncedMsgs == None: for key in self.camera_queue.keys(): self.camera_queue[key].getAll() @@ -1095,9 +1099,13 @@ def calibrate(self): calibration_handler = dai.CalibrationHandler() else: calibration_handler = self.device.readCalibration() + board_keys = self.board_config.keys() try: if self.empty_calibration(calibration_handler): - calibration_handler.setBoardInfo(self.board_config['name'], self.board_config['revision']) + if "name" in board_keys and "revision" in board_keys: + calibration_handler.setBoardInfo(self.board_config['name'], self.board_config['revision']) + else: + calibration_handler.setBoardInfo(str(self.device.getDeviceName()), str(self.args.revision)) except Exception as e: print('Device closed in exception..' ) self.device.close() diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 109b9adec..3dbb4542b 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -112,8 +112,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ # parameters = aruco.DetectorParameters_create() assert mrk_size != None, "ERROR: marker size not set" - combinedCoverageImage = None for camera in board_config['cameras'].keys(): + combinedCoverageImage = None cam_info = board_config['cameras'][camera] print( '<------------Calibrating {} ------------>'.format(cam_info['name'])) @@ -792,7 +792,7 @@ def display_rectification(self, image_data_pairs, images_corners_l, images_corne def scale_image(self, img, scaled_res): expected_height = img.shape[0]*(scaled_res[1]/img.shape[1]) - # print("Expected Height: {}".format(expected_height)) + 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]: @@ -1137,27 +1137,32 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, imgpoints_r = [] imgpoints_l = [] image_epipolar_color = [] - # new_imagePairs = [] + # new_imagePairs = []) for i, image_data_pair in enumerate(image_data_pairs): + cv2.imshow("Image", image_data_pair[1]) + cv2.imshow("Image", image_data_pair[1]) + cv2.waitKey(0) # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) marker_corners_l, ids_l, rejectedImgPoints = cv2.aruco.detectMarkers( image_data_pair[0], self.aruco_dictionary) + print(marker_corners_l) 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) + print(marker_corners_r) marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, marker_corners_r, ids_r, rejectedCorners=rejectedImgPoints) - # print(f'Marekrs length r is {len(marker_corners_r)}') - # print(f'Marekrs length l is {len(marker_corners_l)}') + print(f'Marekrs length r is {len(marker_corners_r)}') + print(f'Marekrs length l is {len(marker_corners_l)}') 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) - + print("Done") img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) line_row = 0 diff --git a/depthai_sdk/src/depthai_sdk/integrations/depthai_pipeline_graph b/depthai_sdk/src/depthai_sdk/integrations/depthai_pipeline_graph new file mode 160000 index 000000000..dd1c2a495 --- /dev/null +++ b/depthai_sdk/src/depthai_sdk/integrations/depthai_pipeline_graph @@ -0,0 +1 @@ +Subproject commit dd1c2a4956d1948753360029064b016d6df62554 diff --git a/resources/depthai_boards b/resources/depthai_boards new file mode 160000 index 000000000..e9dfca057 --- /dev/null +++ b/resources/depthai_boards @@ -0,0 +1 @@ +Subproject commit e9dfca0572a264cc93cb556c926e73a2609cdcae From 4c0419716fa9731757996154ba0aecfc6f1bb970 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 23 Jun 2023 01:10:09 +0200 Subject: [PATCH 52/68] Adding_display_option --- calibrate.py | 7 +- depthai_helpers/calibration_utils.py | 133 +++++++++++++-------------- 2 files changed, 71 insertions(+), 69 deletions(-) diff --git a/calibrate.py b/calibrate.py index 4cd5269de..f5b20e405 100755 --- a/calibrate.py +++ b/calibrate.py @@ -183,6 +183,8 @@ def parse_args(): help="Don't take the board calibration for initialization but start with an empty one") parser.add_argument('-dismin', '--displayMinimum', default=False, action="store_true", help="Display the new minimum every second while taking pictures") + parser.add_argument('-disall', '--printer', default=False, action="store_true", + help="Display all the information in calibration.") options = parser.parse_args() @@ -1094,7 +1096,8 @@ def calibrate(self): self.args.squaresX, self.args.squaresY, self.args.cameraMode, - self.args.rectifiedDisp) # Turn off enable disp rectify + self.args.rectifiedDisp, # Turn off enable disp rectify + self.args.printer) if self.args.noInitCalibration: calibration_handler = dai.CalibrationHandler() else: @@ -1127,7 +1130,7 @@ def calibrate(self): # log_list.append(self.ccm_selected[cam_info['name']]) reprojection_error_threshold = 1.0 if cam_info['size'][1] > 720: - print(cam_info['size'][1]) + #print(cam_info['size'][1]) reprojection_error_threshold = reprojection_error_threshold * cam_info['size'][1] / 720 if cam_info['name'] == 'rgb': diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 3dbb4542b..80abc3b99 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -93,11 +93,12 @@ class StereoCalibration(object): def __init__(self): """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): + def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, display_all): """Function to calculate calibration for stereo camera.""" start_time = time.time() # init object data - print(f'squareX is {squaresX}') + if display_all: + print(f'squareX is {squaresX}') self.enable_rectification_disp = enable_disp_rectify self.cameraModel = camera_model self.data_path = filepath @@ -125,11 +126,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ cam_info['dist_coeff'] = dist_coeff cam_info['size'] = size # (Width, height) cam_info['reprojection_error'] = ret - print( - '<------------Camera Name: {} ------------>'.format(cam_info['name'])) print("Reprojection error of {0}: {1}".format( cam_info['name'], ret)) - print("Estimated intrinsics of {0}: \n {1}".format( + if display_all: + print("Estimated intrinsics of {0}: \n {1}".format( cam_info['name'], intrinsics)) coverage_name = cam_info['name'] @@ -170,7 +170,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ 'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32) extrinsics = self.calibrate_extrinsics(left_path, right_path, left_cam_info['intrinsics'], left_cam_info[ - 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation) + 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, display_all) if extrinsics[0] == -1: return -1, extrinsics[1] @@ -204,8 +204,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ extrinsics[3], # Left Rectification rotation extrinsics[4], # Right Rectification rotation extrinsics[5], # Left Rectification Intrinsics - extrinsics[6]) # Right Rectification Intrinsics - + extrinsics[6], # Right Rectification Intrinsics + display_all) left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] left_cam_info['extrinsics']['translation'] = extrinsics[2] left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] @@ -316,9 +316,9 @@ def calibrate_intrinsics(self, image_files, hfov): allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files) # print(imsize) # coverageShape = (imsize[0], imsize[1], 3) - print(imsize) - print(imsize[-1]) - print(imsize[::-1]) + #print(imsize) + #print(imsize[-1]) + #print(imsize[::-1]) coverageImage = np.ones(imsize[::-1], np.uint8) * 255 coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR) coverageImage = self.draw_corners(allCorners, coverageImage) @@ -345,7 +345,7 @@ def calibrate_intrinsics(self, image_files, hfov): # (Height, width) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage - def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): + def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation, display_all): self.objpoints = [] # 3d point in real world space self.imgpoints_l = [] # 2d points in image plane. self.imgpoints_r = [] # 2d points in image plane. @@ -386,12 +386,12 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu # 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)) + if display_all: + 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)) # scaled_res = (scaled_height, ) M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) @@ -404,14 +404,14 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu # print("~~~~~~~~~~~ POSE ESTIMATION RIGHT CAMERA ~~~~~~~~~~~~~") allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco( images_right, scale_req, scaled_res) - - print(f'Image size of right side (w, h):{imsize_r}') - print(f'Image size of left side (w, h):{imsize_l}') + if display_all: + print(f'Image size of right side (w, h): {imsize_r}') + print(f'Image size of left side (w, h): {imsize_l}') assert imsize_r == imsize_l, "Left and right resolution scaling is wrong" 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) + allCorners_l, allIds_l, allCorners_r, allIds_r, imsize_r, M_lp, d_l, M_rp, d_r, guess_translation, guess_rotation, display_all) def scale_intrinsics(self, intrinsics, originalShape, destShape): scale = destShape[1] / originalShape[1] # scale on width @@ -466,8 +466,8 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov): """ 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) + print("INTRINSIC CALIBRATION") + #print(imsize) cameraMatrixInit = np.array([[f, 0.0, imsize[0]/2], [0.0, f, imsize[1]/2], [0.0, 0.0, 1.0]]) @@ -540,7 +540,7 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) - def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in): + def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, display_all): left_corners_sampled = [] right_corners_sampled = [] obj_pts = [] @@ -596,13 +596,13 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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} degs') + print(f'Reprojection error is {ret}') + if display_all: + print('Printing Extrinsics res...') + print(R) + print(T) + print(f'Euler angles in XYZ {r_euler} degs') # ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate( @@ -620,9 +620,11 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz # self.P_l = P_l # self.P_r = P_r r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) - print(f'R_L Euler angles in XYZ {r_euler}') + if display_all: + 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}') + if display_all: + print(f'R_R Euler angles in XYZ {r_euler}') # print(f'P_l is \n {P_l}') # print(f'P_r is \n {P_r}') @@ -656,13 +658,13 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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 - - 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') + print(f'Reprojection error is {ret}') + if display_all: + print('Printing Extrinsics res...') + print(R) + print(T) + print(f'Euler angles in XYZ {r_euler} degs') isHorizontal = np.absolute(T[0]) > np.absolute(T[1]) if 0: @@ -734,9 +736,11 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz 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}') + if display_all: + 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}') + if display_all: + print(f'R_R Euler angles in XYZ {r_euler}') return [ret, R, T, R_l, R_r, P_l, P_r] @@ -790,9 +794,10 @@ def display_rectification(self, image_data_pairs, images_corners_l, images_corne cv2.destroyWindow('Stereo Pair') - def scale_image(self, img, scaled_res): + def scale_image(self, img, scaled_res, display_all): expected_height = img.shape[0]*(scaled_res[1]/img.shape[1]) - print("Expected Height: {}".format(expected_height)) + if display_all: + 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]: @@ -820,7 +825,7 @@ def scale_image(self, img, scaled_res): 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): + def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal, display_all): if self.cameraModel == 'perspective': mapx_l, mapy_l = cv2.initUndistortRectifyMap( M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1) @@ -844,8 +849,8 @@ def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_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 = self.scale_image(img_l, scaled_res, display_all) + img_r = self.scale_image(img_r, scaled_res, display_all) # print(img_l.shape) # print(img_r.shape) @@ -958,7 +963,7 @@ def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, 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): + 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, display_all): images_left = glob.glob(left_img_pth + '/*.png') images_right = glob.glob(right_img_pth + '/*.png') images_left.sort() @@ -988,11 +993,11 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # 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)) + if display_all: + 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)) M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) @@ -1012,8 +1017,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, print('Original intrinsics ....') print(M_lp) print(M_rp) - - print(f'Width and height is {scaled_res[::-1]}') + if display_all: + print(f'Width and height is {scaled_res[::-1]}') # print(d_r) # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0) @@ -1037,7 +1042,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, if 0: while True: - epError = self.sgdEpipolar(images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal) + epError = self.sgdEpipolar(images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal, display_all) if oldEpipolarError is None: epQueue.append((epError, kScaledR)) @@ -1080,7 +1085,7 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, kScaledR = currK - print('Lets find the best epipolar Error') + #print('Lets find the best epipolar Error') @@ -1101,8 +1106,8 @@ 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 = self.scale_image(img_l, scaled_res, display_all) + img_r = self.scale_image(img_r, scaled_res, display_all) # print(img_l.shape) # print(img_r.shape) @@ -1139,20 +1144,15 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, image_epipolar_color = [] # new_imagePairs = []) for i, image_data_pair in enumerate(image_data_pairs): - cv2.imshow("Image", image_data_pair[1]) - cv2.imshow("Image", image_data_pair[1]) - cv2.waitKey(0) # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) marker_corners_l, ids_l, rejectedImgPoints = cv2.aruco.detectMarkers( image_data_pair[0], self.aruco_dictionary) - print(marker_corners_l) 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) - print(marker_corners_r) marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, marker_corners_r, ids_r, rejectedCorners=rejectedImgPoints) @@ -1162,7 +1162,6 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, 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) - print("Done") img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) line_row = 0 @@ -1220,9 +1219,9 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, epi_error_sum += curr_epipolar_error localError = epi_error_sum / len(corners_l) image_epipolar_color.append(corner_epipolar_color) - - print("Epipolar Error per image on host in " + img_pth_right.name + " : " + - str(localError)) + if display_all: + print("Epipolar 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))) From 3bc52a94e87b253a5556f757d4e77d81b2a9a830 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 23 Jun 2023 01:22:19 +0200 Subject: [PATCH 53/68] Adding_display_option --- calibrate.py | 3 --- depthai_helpers/calibration_utils.py | 12 ++++++------ 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/calibrate.py b/calibrate.py index f5b20e405..fc4e8408c 100755 --- a/calibrate.py +++ b/calibrate.py @@ -1084,10 +1084,7 @@ def calibrate(self): dest_path = str(Path('resources').absolute()) # self.args.cameraMode = 'perspective' # hardcoded for now try: - # stereo_calib = StereoCalibration() - print("Starting image processingxxccx") - print(self.args.squaresX) status, result_config = stereo_calib.calibrate( self.board_config, self.dataset_path, diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 80abc3b99..823e9ee25 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -1015,8 +1015,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # TODO(Sachin): Observe Images by adding visualization # TODO(Sachin): Check if the stetch is only in calibration Images print('Original intrinsics ....') - print(M_lp) - print(M_rp) + print(f"L {M_lp}") + print(f"R: {M_rp}") if display_all: print(f'Width and height is {scaled_res[::-1]}') # print(d_r) @@ -1034,8 +1034,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, kScaledR = p_rp print('Intrinsics from the getOptimalNewCameraMatrix/Original ....') - print(kScaledL) - print(kScaledR) + print(f"L: {kScaledL}") + print(f"R: {kScaledR}") oldEpipolarError = None epQueue = deque() movePos = True @@ -1156,8 +1156,8 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, marker_corners_r, ids_r, _, _ = cv2.aruco.refineDetectedMarkers(image_data_pair[1], self.board, marker_corners_r, ids_r, rejectedCorners=rejectedImgPoints) - print(f'Marekrs length r is {len(marker_corners_r)}') - print(f'Marekrs length l is {len(marker_corners_l)}') + print(f'Marekrs length for pair {i} is: L {len(marker_corners_l)} | R {len(marker_corners_r)} ') + #print(f'Marekrs length l is {len(marker_corners_l)}') res2_l = cv2.aruco.interpolateCornersCharuco( marker_corners_l, ids_l, image_data_pair[0], self.board) res2_r = cv2.aruco.interpolateCornersCharuco( From 33296e40056fa8efce63c412fadb0650ec83ab3e Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 23 Jun 2023 12:42:26 +0200 Subject: [PATCH 54/68] Changes --- calibrate.py | 26 +- depthai_helpers/calibration_utils.py | 32 +- depthai_helpers/calibration_utils_old.py | 1020 ------------------ resources/boards/AR0234DEV.json | 56 - resources/boards/BW1097.json | 54 - resources/boards/BW1098OBC.json | 55 - resources/boards/DM1092.json | 55 - resources/boards/DM1097.json | 54 - resources/boards/DM1098OAKD_WIFI.json | 55 - resources/boards/OAK-1-LITE-W.json | 14 - resources/boards/OAK-1-LITE.json | 14 - resources/boards/OAK-1-MAX.json | 14 - resources/boards/OAK-1-POE.json | 16 - resources/boards/OAK-1-W.json | 14 - resources/boards/OAK-1.json | 14 - resources/boards/OAK-D-LITE.json | 56 - resources/boards/OAK-D-POE-W.json | 55 - resources/boards/OAK-D-POE.json | 55 - resources/boards/OAK-D-PRO-POE-CUSTOM.json | 55 - resources/boards/OAK-D-PRO-POE.json | 55 - resources/boards/OAK-D-PRO-W-CUSTOM.json | 55 - resources/boards/OAK-D-PRO-W-POE-CUSTOM.json | 55 - resources/boards/OAK-D-PRO-W-POE.json | 55 - resources/boards/OAK-D-PRO-W.json | 55 - resources/boards/OAK-D-PRO.json | 55 - resources/boards/OAK-D-S2.json | 55 - resources/boards/OAK-D-W.json | 55 - resources/boards/TESTVER_NO_RGB.json | 55 - resources/boards/TESTVER_NO_VERTICAL.json | 37 - 29 files changed, 23 insertions(+), 2223 deletions(-) delete mode 100644 depthai_helpers/calibration_utils_old.py delete mode 100644 resources/boards/AR0234DEV.json delete mode 100644 resources/boards/BW1097.json delete mode 100644 resources/boards/BW1098OBC.json delete mode 100644 resources/boards/DM1092.json delete mode 100644 resources/boards/DM1097.json delete mode 100644 resources/boards/DM1098OAKD_WIFI.json delete mode 100644 resources/boards/OAK-1-LITE-W.json delete mode 100644 resources/boards/OAK-1-LITE.json delete mode 100644 resources/boards/OAK-1-MAX.json delete mode 100644 resources/boards/OAK-1-POE.json delete mode 100644 resources/boards/OAK-1-W.json delete mode 100644 resources/boards/OAK-1.json delete mode 100644 resources/boards/OAK-D-LITE.json delete mode 100644 resources/boards/OAK-D-POE-W.json delete mode 100644 resources/boards/OAK-D-POE.json delete mode 100644 resources/boards/OAK-D-PRO-POE-CUSTOM.json delete mode 100644 resources/boards/OAK-D-PRO-POE.json delete mode 100644 resources/boards/OAK-D-PRO-W-CUSTOM.json delete mode 100644 resources/boards/OAK-D-PRO-W-POE-CUSTOM.json delete mode 100644 resources/boards/OAK-D-PRO-W-POE.json delete mode 100644 resources/boards/OAK-D-PRO-W.json delete mode 100644 resources/boards/OAK-D-PRO.json delete mode 100644 resources/boards/OAK-D-S2.json delete mode 100644 resources/boards/OAK-D-W.json delete mode 100644 resources/boards/TESTVER_NO_RGB.json delete mode 100644 resources/boards/TESTVER_NO_VERTICAL.json diff --git a/calibrate.py b/calibrate.py index fc4e8408c..36eed81e0 100755 --- a/calibrate.py +++ b/calibrate.py @@ -147,14 +147,14 @@ def parse_args(): parser.add_argument("-m", "--mode", default=['capture', 'process'], nargs='*', type=str, required=False, help="Space-separated list of calibration options to run. By default, executes the full 'capture process' pipeline. To execute a single step, enter just that step (ex: 'process').") parser.add_argument("-brd", "--board", default=None, type=str, required=True, - help="BW1097, BW1098OBC - Board type from resources/boards/ (not case-sensitive). " + help="BW1097, BW1098OBC - Board type from resources/depthai_boards/boards (not case-sensitive). " "Or path to a custom .json board config. Mutually exclusive with [-fv -b -w]") parser.add_argument("-iv", "--invertVertical", dest="invert_v", default=False, action="store_true", help="Invert vertical axis of the camera for the display") parser.add_argument("-ih", "--invertHorizontal", dest="invert_h", default=False, action="store_true", 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("-ep", "--maxEpiploarError", default="0.7", 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("-rlp", "--rgbLensPosition", default=135, type=int, @@ -181,9 +181,7 @@ def parse_args(): help="Enable mouse trigger for image capture") parser.add_argument('-nic', '--noInitCalibration', default=False, action="store_true", help="Don't take the board calibration for initialization but start with an empty one") - parser.add_argument('-dismin', '--displayMinimum', default=False, action="store_true", - help="Display the new minimum every second while taking pictures") - parser.add_argument('-disall', '--printer', default=False, action="store_true", + parser.add_argument('-disall', '--enableDebugMessageSync', default=False, action="store_true", help="Display all the information in calibration.") options = parser.parse_args() @@ -319,7 +317,7 @@ def add_msg(self, name, msg): # print() # print() - def get_synced(self, displayMinimum): + def get_synced(self, enableDebugMessageSync): # Atleast 3 messages should be buffered min_len = min([len(queue) for queue in self.queues.values()]) @@ -357,7 +355,7 @@ def get_synced(self, displayMinimum): # Mark minimum if min_ts_diff is None or (acc_diff < min_ts_diff['ts'] and abs(acc_diff - min_ts_diff['ts']) > 0.0001): min_ts_diff = {'ts': acc_diff, 'indicies': indicies.copy()} - if displayMinimum: + if enableDebugMessageSync: print('new minimum:', min_ts_diff, 'min required:', self.min_diff_timestamp) if min_ts_diff['ts'] < self.min_diff_timestamp: @@ -374,8 +372,8 @@ def get_synced(self, displayMinimum): # pop out the older messages for i in range(0, min_ts_diff['indicies'][name]+1): self.queues[name].popleft() - - print('Returning synced messages with error:', min_ts_diff['ts'], min_ts_diff['indicies']) + if enableDebugMessageSync: + print('Returning synced messages with error:', min_ts_diff['ts'], min_ts_diff['indicies']) return synced # print(' @@ -393,7 +391,7 @@ def __init__(self): global debug self.args = parse_args() debug = self.args.debug - displayMinimum=self.args.displayMinimum + enableDebugMessageSync=self.args.enableDebugMessageSync self.output_scale_factor = self.args.outputScaleFactor self.aruco_dictionary = cv2.aruco.Dictionary_get( cv2.aruco.DICT_4X4_1000) @@ -401,7 +399,7 @@ def __init__(self): if self.args.board: board_path = Path(self.args.board) if not board_path.exists(): - board_path = (Path(__file__).parent / 'resources/boards' / self.args.board.upper()).with_suffix('.json').resolve() + board_path = (Path(__file__).parent / 'resources/depthai_boards/boards' / self.args.board.upper()).with_suffix('.json').resolve() if not board_path.exists(): raise ValueError( 'Board config not found: {}'.format(board_path)) @@ -686,7 +684,7 @@ def capture_images_sync(self): timer = self.args.captureDelay prev_time = None curr_time = None - displayMinimum= self.args.displayMinimum + enableDebugMessageSync= self.args.enableDebugMessageSync self.display_name = "Image Window" syncCollector = MessageSync(len(self.camera_queue), 0.003) # 3ms tolerance self.mouseTrigger = False @@ -840,7 +838,7 @@ def capture_images_sync(self): tried = {} allPassed = True if capturing: - syncedMsgs = syncCollector.get_synced(displayMinimum) + syncedMsgs = syncCollector.get_synced(enableDebugMessageSync) if syncedMsgs == False or syncedMsgs == None: for key in self.camera_queue.keys(): self.camera_queue[key].getAll() diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 823e9ee25..d8aa1f773 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -314,11 +314,7 @@ def calibrate_intrinsics(self, image_files, hfov): image_files) != 0, "ERROR: Images not read correctly, check directory" allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files) - # print(imsize) - # coverageShape = (imsize[0], imsize[1], 3) - #print(imsize) - #print(imsize[-1]) - #print(imsize[::-1]) + coverageImage = np.ones(imsize[::-1], np.uint8) * 255 coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR) coverageImage = self.draw_corners(allCorners, coverageImage) @@ -360,9 +356,6 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu images_left) != 0, "ERROR: Images not found, check directory" assert len( images_right) != 0, "ERROR: Images not found, check directory" - # print('Images from left and right') - # print(images_left[0]) - # print(images_right[0]) scale = None scale_req = False @@ -545,13 +538,16 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz right_corners_sampled = [] obj_pts = [] one_pts = self.board.chessboardCorners - # print('allIds_l') - # print(len(allIds_l)) - # print('allIds_r') - # print(len(allIds_r)) - # print('allIds_l') - # print(allIds_l) - # print(allIds_r) + + if display_all: + print('Length of allIds_l') + print(len(allIds_l)) + print('Length of allIds_r') + print(len(allIds_r)) + print('allIds_l') + print(allIds_l) + print('allIds_r') + print(allIds_r) for i in range(len(allIds_l)): left_sub_corners = [] @@ -605,12 +601,6 @@ def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsiz print(f'Euler angles in XYZ {r_euler} degs') - # ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate( - # obj_pts, left_corners_sampled, right_corners_sampled, - # cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, - # criteria=stereocalib_criteria, flags=flags) - # if np.absolute(T[1]) > 0.2: - R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( cameraMatrix_l, distCoeff_l, diff --git a/depthai_helpers/calibration_utils_old.py b/depthai_helpers/calibration_utils_old.py deleted file mode 100644 index ff9a77617..000000000 --- a/depthai_helpers/calibration_utils_old.py +++ /dev/null @@ -1,1020 +0,0 @@ -#!/usr/bin/env python3 - -import cv2 -import glob -import os -import shutil -import numpy as np -import re -import time -import json -import cv2.aruco as aruco -from pathlib import Path -# Creates a set of 13 polygon coordinates - - -def setPolygonCoordinates(height, width): - horizontal_shift = width//4 - vertical_shift = height//4 - - margin = 60 - slope = 150 - - p_coordinates = [ - [[margin,margin], [margin, height-margin], [width-margin, height-margin], [width-margin, margin]], - - [[margin,0], [margin,height], [width//2, height-slope], [width//2, slope]], - [[horizontal_shift, 0], [horizontal_shift, height], [width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]], - [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 + horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]], - - [[width-margin, 0], [width-margin, height], [width//2, height-slope], [width//2, slope]], - [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width//2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]], - [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width//2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]], - - [[0,margin], [width, margin], [width-slope, height//2], [slope, height//2]], - [[0,vertical_shift], [width, vertical_shift], [width-slope, height//2+vertical_shift], [slope, height//2+vertical_shift]], - [[0,vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope, height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]], - - [[0,height-margin], [width, height-margin], [width-slope, height//2], [slope, height//2]], - [[0,height-vertical_shift], [width, height-vertical_shift], [width-slope, height//2-vertical_shift], [slope, height//2-vertical_shift]], - [[0,height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width-slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]] - ] - return p_coordinates - - -def getPolygonCoordinates(idx, p_coordinates): - return p_coordinates[idx] - - -def getNumOfPolygons(p_coordinates): - return len(p_coordinates) - -# Filters polygons to just those at the given indexes. - - -def select_polygon_coords(p_coordinates, indexes): - if indexes == None: - # The default - return p_coordinates - else: - print("Filtering polygons to those at indexes=", indexes) - return [p_coordinates[i] for i in indexes] - - -def image_filename(stream_name, polygon_index, total_num_of_captured_images): - return "{stream_name}_p{polygon_index}_{total_num_of_captured_images}.png".format(stream_name=stream_name, polygon_index=polygon_index, total_num_of_captured_images=total_num_of_captured_images) - - -def polygon_from_image_name(image_name): - """Returns the polygon index from an image name (ex: "left_p10_0.png" => 10)""" - return int(re.findall("p(\d+)", image_name)[0]) - - -class StereoCalibration(object): - """Class to Calculate Calibration and Rectify a Stereo Camera.""" - - def __init__(self): - """Class to Calculate Calibration and Rectify a Stereo Camera.""" - - def calibrate(self, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, calibrate_rgb, enable_disp_rectify): - """Function to calculate calibration for stereo camera.""" - start_time = time.time() - # init object data - self.calibrate_rgb = calibrate_rgb - self.enable_rectification_disp = enable_disp_rectify - self.cameraModel = camera_model - self.data_path = filepath - self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) - self.board = aruco.CharucoBoard_create( - # 22, 16, - squaresX, squaresY, - square_size, - mrk_size, - self.aruco_dictionary) - - - # parameters = aruco.DetectorParameters_create() - assert mrk_size != None, "ERROR: marker size not set" - self.calibrate_charuco3D(filepath) - - # self.stereo_calibrate_two_homography_calib() - print('~~~~~ Starting Stereo Calibration ~~~~~') - # self.stereo_calib_two_homo() - - # rgb-right extrinsic calibration - if self.calibrate_rgb: - # if True: - # things to do. - # First: change the center and other thigns of the calibration matrix of rgb camera - self.rgb_calibrate(filepath) - else: - self.M3 = np.zeros((3, 3), dtype=np.float32) - self.R_rgb = np.zeros((3, 3), dtype=np.float32) - self.T_rgb = np.zeros(3, dtype=np.float32) - self.d3 = np.zeros(14, dtype=np.float32) - - # self.M3_scaled_write = np.copy(self.M3_scaled) - # self.M3_scaled_write[1, 2] += 40 - - R1_fp32 = self.R1.astype(np.float32) - R2_fp32 = self.R2.astype(np.float32) - M1_fp32 = self.M1.astype(np.float32) - M2_fp32 = self.M2.astype(np.float32) - M3_fp32 = self.M3.astype(np.float32) - - R_fp32 = self.R.astype(np.float32) # L-R rotation - T_fp32 = self.T.astype(np.float32) # L-R translation - R_rgb_fp32 = self.R_rgb.astype(np.float32) - T_rgb_fp32 = self.T_rgb.astype(np.float32) - - d1_coeff_fp32 = self.d1.astype(np.float32) - d2_coeff_fp32 = self.d2.astype(np.float32) - d3_coeff_fp32 = self.d3.astype(np.float32) - - if self.calibrate_rgb: - R_rgb_fp32 = np.linalg.inv(R_rgb_fp32) - T_rgb_fp32[0] = -T_rgb_fp32[0] - T_rgb_fp32[1] = -T_rgb_fp32[1] - T_rgb_fp32[2] = -T_rgb_fp32[2] - - self.calib_data = [R1_fp32, R2_fp32, M1_fp32, M2_fp32, M3_fp32, R_fp32, T_fp32, R_rgb_fp32, T_rgb_fp32, d1_coeff_fp32, d2_coeff_fp32, d3_coeff_fp32] - - if 1: # Print matrices, to compare with device data - np.set_printoptions(suppress=True, precision=6) - print("\nR1 (left)"); print(R1_fp32) - print("\nR2 (right)"); print(R2_fp32) - print("\nM1 (left)"); print(M1_fp32) - print("\nM2 (right)"); print(M2_fp32) - print("\nR"); print(R_fp32) - print("\nT"); print(T_fp32) - print("\nM3 (rgb)"); print(M3_fp32) - print("\R (rgb)") - print(R_rgb_fp32) - print("\nT (rgb)") - print(T_rgb_fp32) - - if 0: # Print computed homography, to compare with device data - np.set_printoptions(suppress=True, precision=6) - for res_height in [800, 720, 400]: - m1 = np.copy(M1_fp32) - m2 = np.copy(M2_fp32) - if res_height == 720: - m1[1, 2] -= 40 - m2[1, 2] -= 40 - if res_height == 400: - m_scale = [[0.5, 0, 0], - [0, 0.5, 0], - [0, 0, 1]] - m1 = np.matmul(m_scale, m1) - m2 = np.matmul(m_scale, m2) - h1 = np.matmul(np.matmul(m2, R1_fp32), np.linalg.inv(m1)) - h2 = np.matmul(np.matmul(m2, R2_fp32), np.linalg.inv(m2)) - h1 = np.linalg.inv(h1) - h2 = np.linalg.inv(h2) - print('\nHomography H1, H2 for height =', res_height) - print(h1) - print() - print(h2) - - print("\tTook %i seconds to run image processing." % - (round(time.time() - start_time, 2))) - - self.create_save_mesh() - - if self.calibrate_rgb: - return self.test_epipolar_charuco_lr(filepath), self.test_epipolar_charuco_rgbr(filepath), self.calib_data - else: - return self.test_epipolar_charuco_lr(filepath), None, self.calib_data - - def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): - """ - Charuco base pose estimation. - """ - # print("POSE ESTIMATION STARTS:") - allCorners = [] - allIds = [] - all_marker_corners = [] - all_marker_ids = [] - all_recovered = [] - # decimator = 0 - # SUB PIXEL CORNER DETECTION CRITERION - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) - count = 0 - for im in images: - print("=> Processing image {0}".format(im)) - frame = cv2.imread(im) - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - # gray = cv2.flip(gray, 0) # TODO(Sachin) : remove this later - # width = scale[1] - expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1]) - # print('expected height -------------------> ' + str(expected_height)) - # print('required height -------------------> ' + - # str(req_resolution)) - - if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]): - # print("scaling----------------------->") - 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) - # print('~~~~~~~~~~ Only resizing.... ~~~~~~~~~~~~~~~~') - 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)) - # print("destination resolution------>") - # print(dest_res) - gray = cv2.resize( - gray, dest_res, interpolation=cv2.INTER_CUBIC) - # print("scaled gray shape") - # print(gray.shape) - 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], :] - - # print("del height ??") - # print(del_height) - # print(gray.shape) - count += 1 - # self.parse_frame(gray, 'rgb_resized', - # 'rgb_resized_'+str(count)) - 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) - print('{0} number of Markers corners detected in the above image'.format( - len(marker_corners))) - if len(marker_corners) > 0: - # print(len(marker_corners)) - # SUB PIXEL DETECTION - # for corner in marker_corners: - # cv2.cornerSubPix(gray, corner, - # winSize = (5,5), - # zeroZone = (-1,-1), - # criteria = criteria) - res2 = cv2.aruco.interpolateCornersCharuco( - marker_corners, ids, gray, self.board) - - # if res2[1] is not None and res2[2] is not None and len(res2[1])>3 and decimator%1==0: - if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3: - - cv2.cornerSubPix(gray, res2[1], - winSize=(5, 5), - zeroZone=(-1, -1), - criteria=criteria) - allCorners.append(res2[1]) # Charco chess corners - allIds.append(res2[2]) # charuco chess corner id's - all_marker_corners.append(marker_corners) - all_marker_ids.append(ids) - all_recovered.append(recoverd) - else: - print("in else") - else: - print(im + " Not found") - # decimator+=1 - - imsize = gray.shape - return allCorners, allIds, all_marker_corners, all_marker_ids, imsize, all_recovered - - def calibrate_charuco3D(self, filepath): - self.objpoints = [] # 3d point in real world space - self.imgpoints_l = [] # 2d points in image plane. - self.imgpoints_r = [] # 2d points in image plane. - - # calcorners_l = [] # 2d points in image - # calcorners_r = [] # 2d points in image - # calids_l = [] # ids found in imag - # calids_r = [] # ids found in imag - - images_left = glob.glob(filepath + "/left/*") - images_right = glob.glob(filepath + "/right/*") - # images_rgb = glob.glob(filepath + "/rgb/*") - # print("Images left path------------------->") - # print(images_left) - images_left.sort() - images_right.sort() - # images_rgb.sort() - - assert len( - images_left) != 0, "ERROR: Images not read correctly, check directory" - assert len( - images_right) != 0, "ERROR: Images not read correctly, check directory" - # assert len( - # images_rgb) != 0, "ERROR: Images not read correctly, check directory" - - print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") - allCorners_l, allIds_l, _, _, imsize, _ = self.analyze_charuco( - images_left) - allCorners_r, allIds_r, _, _, imsize, _ = self.analyze_charuco( - images_right) - self.img_shape = imsize[::-1] - - # self.img_shape_rgb = imsize_rgb[::-1] - if self.cameraModel == 'perspective': - ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_l, allIds_l, self.img_shape) - ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_r, allIds_r, self.img_shape) - else: - ret_l, self.M1, self.d1, rvecs, tvecs = self.calibrate_fisheye(allCorners_l, allIds_l, self.img_shape) - ret_r, self.M2, self.d2, rvecs, tvecs = self.calibrate_fisheye(allCorners_r, allIds_r, self.img_shape) - # self.fisheye_undistort_visualizaation(images_left, self.M1, self.d1, self.img_shape) - # self.fisheye_undistort_visualizaation(images_right, self.M2, self.d2, self.img_shape) - - - print("~~~~~~~~~~~~~RMS error of left~~~~~~~~~~~~~~") - print(ret_l) - print(ret_r) - print(self.M1) - print(self.M2) - print(self.d1) - print(self.d2) - # if self.cameraModel == 'perspective': - ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) - # else: - # ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T = self.calibrate_stereo(allCorners_l, allIds_l, allCorners_r, allIds_r, self.img_shape, self.M1, self.d1, self.M2, self.d2) - print("~~~~~~~~~~~~~RMS error of L-R~~~~~~~~~~~~~~") - print(ret) - """ - left_corners_sampled = [] - right_corners_sampled = [] - obj_pts = [] - one_pts = self.board.chessboardCorners - for i in range(len(allIds_l)): - left_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_l[i])): - idx = np.where(allIds_r[i] == allIds_l[i][j]) - if idx[0].size == 0: - continue - left_sub_corners.append(allCorners_l[i][j]) - right_sub_corners.append(allCorners_r[i][idx]) - obj_pts_sub.append(one_pts[allIds_l[i][j]]) - - obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) - left_corners_sampled.append( - np.array(left_sub_corners, dtype=np.float32)) - right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - - self.objpoints = obj_pts - self.imgpoints_l = left_corners_sampled - self.imgpoints_r = right_corners_sampled - - flags = 0 - flags |= cv2.CALIB_USE_INTRINSIC_GUESS - flags |= cv2.CALIB_RATIONAL_MODEL - - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - ret, self.M1, self.d1, self.M2, self.d2, self.R, self.T, E, F = cv2.stereoCalibrate( - self.objpoints, self.imgpoints_l, self.imgpoints_r, - self.M1, self.d1, self.M2, self.d2, self.img_shape, - criteria=stereocalib_criteria, flags=flags) - print("<~~ ~~~~~~~~~~~ RMS of stereo ~~~~~~~~~~~ ~~>") - print('RMS error of stereo calibration of left-right is {0}'.format(ret)) """ - - # TODO(sachin): Fix rectify for Fisheye - if self.cameraModel == 'perspective': - - self.R1, self.R2, self.P1, self.P2, self.Q, validPixROI1, validPixROI2 = cv2.stereoRectify( - self.M1, - self.d1, - self.M2, - self.d2, - self.img_shape, self.R, self.T) - else: - self.R1, self.R2, self.P1, self.P2, self.Q = cv2.fisheye.stereoRectify(self.M1, - self.d1, - self.M2, - self.d2, - self.img_shape, self.R, self.T) - - self.H1 = np.matmul(np.matmul(self.M2, self.R1), - np.linalg.inv(self.M1)) - self.H2 = np.matmul(np.matmul(self.M2, self.R2), - np.linalg.inv(self.M2)) - - def fisheye_undistort_visualizaation(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': - 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) - - undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) - cv2.imshow("undistorted", undistorted_img) - cv2.waitKey(0) - # cv2.destroyAllWindows() - - - def calibrate_camera_charuco(self, allCorners, allIds, imsize): - """ - Calibrates the camera using the dected corners. - """ - print("CAMERA CALIBRATION") - print(imsize) - if 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.]]) - - print("Camera Matrix initialization.............") - print(cameraMatrixInit) - - distCoeffsInit = np.zeros((5, 1)) - flags = (cv2.CALIB_USE_INTRINSIC_GUESS + - cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO) - # flags = (cv2.CALIB_RATIONAL_MODEL) - (ret, camera_matrix, distortion_coefficients, - rotation_vectors, translation_vectors, - stdDeviationsIntrinsics, stdDeviationsExtrinsics, - perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended( - charucoCorners=allCorners, - charucoIds=allIds, - board=self.board, - imageSize=imsize, - cameraMatrix=cameraMatrixInit, - distCoeffs=distCoeffsInit, - flags=flags, - criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9)) - - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors - - def calibrate_fisheye(self, allCorners, allIds, imsize): - one_pts = self.board.chessboardCorners - obj_points = [] - for i in range(len(allIds)): - obj_pts_sub = [] - for j in range(len(allIds[i])): - 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]]) - - print("Camera Matrix initialization.............") - print(cameraMatrixInit) - flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - distCoeffsInit = np.zeros((4, 1)) - term_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags = flags, criteria = term_criteria) - - def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r): - left_corners_sampled = [] - right_corners_sampled = [] - obj_pts = [] - one_pts = self.board.chessboardCorners - print('allIds_l') - print(len(allIds_l)) - print(len(allIds_r)) - print('allIds_l') - # print(allIds_l) - print('allIds_r') - # print(allIds_r) - - for i in range(len(allIds_l)): - left_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_l[i])): - idx = np.where(allIds_r[i] == allIds_l[i][j]) - if idx[0].size == 0: - continue - left_sub_corners.append(allCorners_l[i][j]) - right_sub_corners.append(allCorners_r[i][idx]) - obj_pts_sub.append(one_pts[allIds_l[i][j]]) - - obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) - left_corners_sampled.append( - np.array(left_sub_corners, dtype=np.float32)) - right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - if self.cameraModel == 'perspective': - flags = 0 - flags |= cv2.CALIB_USE_INTRINSIC_GUESS # TODO(sACHIN): Try without intrinsic guess - flags |= cv2.CALIB_RATIONAL_MODEL - - return cv2.stereoCalibrate( - obj_pts, left_corners_sampled, right_corners_sampled, - cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, - criteria=stereocalib_criteria, flags=flags) - elif self.cameraModel == 'fisheye': - print(len(obj_pts)) - print('obj_pts') - # print(obj_pts) - print(len(left_corners_sampled)) - print('left_corners_sampled') - # print(left_corners_sampled) - print(len(right_corners_sampled)) - print('right_corners_sampled') - # print(right_corners_sampled) - for i in range(len(obj_pts)): - print('---------------------') - print(i) - print(len(obj_pts[i])) - print(len(left_corners_sampled[i])) - print(len(right_corners_sampled[i])) - flags = 0 - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC # TODO(sACHIN): Try without intrinsic guess - return cv2.fisheye.stereoCalibrate( - obj_pts, left_corners_sampled, right_corners_sampled, - cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, imsize, - flags=flags, criteria=stereocalib_criteria), None, None - - def rgb_calibrate(self, filepath): - images_right = glob.glob(filepath + "/right/*") - images_rgb = glob.glob(filepath + "/rgb/*") - - images_rgb_pth = Path(filepath + "/rgb") - if not images_rgb_pth.exists(): - raise RuntimeError("RGB dataset folder not found!! To skip rgb calibration use -drgb argument") - - images_right.sort() - images_rgb.sort() - - allCorners_rgb_scaled, allIds_rgb_scaled, _, _, imsize_rgb_scaled, _ = self.analyze_charuco( - images_rgb, scale_req=True, req_resolution=(720, 1280)) - self.img_shape_rgb_scaled = imsize_rgb_scaled[::-1] - - ret_rgb_scaled, self.M3_scaled, self.d3_scaled, rvecs, tvecs = self.calibrate_camera_charuco( - allCorners_rgb_scaled, allIds_rgb_scaled, imsize_rgb_scaled[::-1]) - - allCorners_r_rgb, allIds_r_rgb, _, _, _, _ = self.analyze_charuco( - images_right, scale_req=True, req_resolution=(720, 1280)) - - print("RGB callleded RMS at 720") - print(ret_rgb_scaled) - print(imsize_rgb_scaled) - print('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~') - # print(self.M3_scaled) - - # sampling common detected corners - rgb_scaled_rgb_corners_sampled = [] - rgb_scaled_right_corners_sampled = [] - rgb_scaled_obj_pts = [] - rgb_pts = None - right_pts = None - one_pts = self.board.chessboardCorners - for i in range(len(allIds_rgb_scaled)): - rgb_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - # if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_rgb_scaled[i])): - idx = np.where(allIds_r_rgb[i] == allIds_rgb_scaled[i][j]) - if idx[0].size == 0: - continue - rgb_sub_corners.append(allCorners_rgb_scaled[i][j]) - right_sub_corners.append(allCorners_r_rgb[i][idx]) - obj_pts_sub.append(one_pts[allIds_rgb_scaled[i][j]]) - - rgb_scaled_obj_pts.append( - np.array(obj_pts_sub, dtype=np.float32)) - rgb_scaled_rgb_corners_sampled.append( - np.array(rgb_sub_corners, dtype=np.float32)) - rgb_scaled_right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - if rgb_pts is None: - rgb_pts = np.array(rgb_sub_corners, dtype=np.float32) - right_pts = np.array(right_sub_corners, dtype=np.float32) - else: - np.vstack( - (rgb_pts, np.array(rgb_sub_corners, dtype=np.float32))) - np.vstack((right_pts, np.array( - right_sub_corners, dtype=np.float32))) - - self.objpoints_rgb_r = rgb_scaled_obj_pts - self.imgpoints_rgb = rgb_scaled_rgb_corners_sampled - self.imgpoints_rgb_right = rgb_scaled_right_corners_sampled - - flags = 0 - flags |= cv2.CALIB_FIX_INTRINSIC - flags |= cv2.CALIB_RATIONAL_MODEL - - - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 100, 1e-5) - - # print(M_RGB) - print('vs. intrinisics computed after scaling the image --->') - # self.M3, self.d3 - scale = 1920/1280 - print(scale) - scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]]) - self.M3 = np.matmul(scale_mat, self.M3_scaled) - self.d3 = self.d3_scaled - print(self.M3_scaled) - print(self.M3) - - self.M2_rgb = np.copy(self.M2) - self.M2_rgb[1, 2] -= 40 - self.d2_rgb = np.copy(self.d1) - - ret, _, _, _, _, self.R_rgb, self.T_rgb, E, F = cv2.stereoCalibrate( - self.objpoints_rgb_r, self.imgpoints_rgb, self.imgpoints_rgb_right, - self.M3_scaled, self.d3_scaled, self.M2_rgb, self.d2_rgb, self.img_shape_rgb_scaled, - criteria=stereocalib_criteria, flags=flags) - print("~~~~~~ Stereo calibration rgb-left RMS error ~~~~~~~~") - print(ret) - - # Rectification is only to test the epipolar - self.R1_rgb, self.R2_rgb, self.P1_rgb, self.P2_rgb, self.Q_rgb, validPixROI1, validPixROI2 = cv2.stereoRectify( - self.M3_scaled, - self.d3_scaled, - self.M2_rgb, - self.d2_rgb, - self.img_shape_rgb_scaled, self.R_rgb, self.T_rgb) - - def test_epipolar_charuco_lr(self, dataset_dir): - print("<-----------------Epipolar error of LEFT-right camera---------------->") - images_left = glob.glob(dataset_dir + '/left/*.png') - images_right = glob.glob(dataset_dir + '/right/*.png') - images_left.sort() - images_right.sort() - print("HU IHER") - assert len(images_left) != 0, "ERROR: Images not read correctly" - assert len(images_right) != 0, "ERROR: Images not read correctly" - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) - - # if not use_homo: - mapx_l, mapy_l = cv2.initUndistortRectifyMap( - self.M1, self.d1, self.R1, self.P1, self.img_shape, cv2.CV_32FC1) - mapx_r, mapy_r = cv2.initUndistortRectifyMap( - self.M2, self.d2, self.R2, self.P2, self.img_shape, cv2.CV_32FC1) - print("Printing p1 and p2") - print(self.P1) - print(self.P2) - image_data_pairs = [] - for image_left, image_right in zip(images_left, images_right): - # read images - img_l = cv2.imread(image_left, 0) - img_r = cv2.imread(image_right, 0) - # 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)) - - # compute metrics - imgpoints_r = [] - imgpoints_l = [] - for i, image_data_pair in enumerate(image_data_pairs): - # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - 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) - if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[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 = Path(images_right[i]) - name = img_pth.name - print("Image name {}".format(name)) - 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]) -# obj_pts_sub.append(one_pts[allIds_l[i][j]]) - -# obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) -# left_sub_corners_sampled.append(np.array(left_sub_corners, dtype=np.float32)) -# right_sub_corners_sampled.append(np.array(right_sub_corners, dtype=np.float32)) - - 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): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - - print("Average Epipolar Error per image on host in " + img_pth.name + " : " + - str(epi_error_sum / len(corners_l))) - - epi_error_sum = 0 - for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - - avg_epipolar = epi_error_sum / len(imgpoints_r) - print("Average Epipolar Error: " + str(avg_epipolar)) - - if self.enable_rectification_disp: - self.display_rectification(image_data_pairs) - - return avg_epipolar - - def test_epipolar_charuco_rgbr(self, dataset_dir): - images_rgb = glob.glob(dataset_dir + '/rgb/*.png') - images_right = glob.glob(dataset_dir + '/right/*.png') - images_rgb.sort() - images_right.sort() - print("<-----------------Epipolar error of rgb-right camera---------------->") - assert len(images_rgb) != 0, "ERROR: Images not read correctly" - assert len(images_right) != 0, "ERROR: Images not read correctly" - # criteria for marker detection/corner detections - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) - scale_width = 1280/self.img_shape_rgb_scaled[0] - print('scaled using {0}'.format(self.img_shape_rgb_scaled[0])) - - # if not use_homo: - mapx_rgb, mapy_rgb = cv2.initUndistortRectifyMap( - self.M3_scaled, self.d3_scaled, self.R1_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) - mapx_r, mapy_r = cv2.initUndistortRectifyMap( - self.M2_rgb, self.d2_rgb, self.R2_rgb, self.M3_scaled, self.img_shape_rgb_scaled, cv2.CV_32FC1) - - # self.H1_rgb = np.matmul(np.matmul(self.M2, self.R1_rgb), - # np.linalg.inv(M_rgb)) - # self.H2_r = np.matmul(np.matmul(self.M2, self.R2_rgb), - # np.linalg.inv(self.M2)) - - image_data_pairs = [] - count = 0 - for image_rgb, image_right in zip(images_rgb, images_right): - # read images - img_rgb = cv2.imread(image_rgb, 0) - img_r = cv2.imread(image_right, 0) - img_r = img_r[40: 760, :] - - dest_res = (int(img_rgb.shape[1] * scale_width), - int(img_rgb.shape[0] * scale_width)) - # print("RGB size ....") - # print(img_rgb.shape) - # print(dest_res) - - if img_rgb.shape[0] < 720: - raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format( - img_rgb.shape[0], req_resolution[0])) - del_height = (img_rgb.shape[0] - 720) // 2 - # print("del height ??") - # print(del_height) - img_rgb = img_rgb[del_height: del_height + 720, :] - # print("resized_shape") - # print(img_rgb.shape) - # self.parse_frame(img_rgb, "rectified_rgb_before", - # "rectified_"+str(count)) - - # warp right image - - # img_rgb = cv2.warpPerspective(img_rgb, self.H1_rgb, img_rgb.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - # img_r = cv2.warpPerspective(img_r, self.H2_r, img_r.shape[::-1], - # cv2.INTER_CUBIC + - # cv2.WARP_FILL_OUTLIERS + - # cv2.WARP_INVERSE_MAP) - - img_rgb = cv2.remap(img_rgb, mapx_rgb, mapy_rgb, cv2.INTER_LINEAR) - img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR) - # self.parse_frame(img_rgb, "rectified_rgb", "rectified_"+str(count)) - image_data_pairs.append((img_rgb, img_r)) - count += 1 - - # compute metrics - imgpoints_r = [] - imgpoints_l = [] - for i, image_data_pair in enumerate(image_data_pairs): - # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - 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) - if res2_l[1] is not None and res2_l[2] is not None and len(res2_l[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 - 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): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - img_pth = Path(images_right[i]) - # name = img_pth.name - print("Average Epipolar Error per image on host in " + img_pth.name + " : " + - str(epi_error_sum / len(corners_l))) - - epi_error_sum = 0 - for l_pt, r_pt in zip(imgpoints_l, imgpoints_r): - epi_error_sum += abs(l_pt[0][1] - r_pt[0][1]) - - avg_epipolar = epi_error_sum / len(imgpoints_r) - print("Average Epipolar Error of rgb_right: " + str(avg_epipolar)) - - if self.enable_rectification_disp: - self.display_rectification(image_data_pairs) - - return avg_epipolar - - def display_rectification(self, image_data_pairs): - print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") - for image_data_pair in image_data_pairs: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) - img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - - # draw epipolar lines for debug purposes - 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 - - # show image - cv2.imshow('Stereo Pair', img_concat) - k = cv2.waitKey(0) - if k == 27: # Esc key to stop - break - - # os._exit(0) - # raise SystemExit() - - cv2.destroyWindow('Stereo Pair') - - def display_rectification(self, image_data_pairs): - print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.") - for image_data_pair in image_data_pairs: - img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]]) - img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - - # draw epipolar lines for debug purposes - 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 - - # show image - cv2.imshow('Stereo Pair', img_concat) - k = cv2.waitKey(0) - if k == 27: # Esc key to stop - break - - # os._exit(0) - # raise SystemExit() - - cv2.destroyWindow('Stereo Pair') - - def create_save_mesh(self): #, output_path): - - curr_path = Path(__file__).parent.resolve() - 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) - - map_x_l_fp32 = map_x_l.astype(np.float32) - map_y_l_fp32 = map_y_l.astype(np.float32) - map_x_r_fp32 = map_x_r.astype(np.float32) - map_y_r_fp32 = map_y_r.astype(np.float32) - - print("shape of maps") - print(map_x_l.shape) - print(map_y_l.shape) - print(map_x_r.shape) - print(map_y_r.shape) - - meshCellSize = 16 - mesh_left = [] - mesh_right = [] - - for y in range(map_x_l.shape[0] + 1): - if y % meshCellSize == 0: - row_left = [] - row_right = [] - for x in range(map_x_l.shape[1] + 1): - if x % meshCellSize == 0: - if y == map_x_l.shape[0] and x == map_x_l.shape[1]: - row_left.append(map_y_l[y - 1, x - 1]) - row_left.append(map_x_l[y - 1, x - 1]) - row_right.append(map_y_r[y - 1, x - 1]) - row_right.append(map_x_r[y - 1, x - 1]) - elif y == map_x_l.shape[0]: - row_left.append(map_y_l[y - 1, x]) - row_left.append(map_x_l[y - 1, x]) - row_right.append(map_y_r[y - 1, x]) - row_right.append(map_x_r[y - 1, x]) - elif x == map_x_l.shape[1]: - row_left.append(map_y_l[y, x - 1]) - row_left.append(map_x_l[y, x - 1]) - row_right.append(map_y_r[y, x - 1]) - row_right.append(map_x_r[y, x - 1]) - else: - row_left.append(map_y_l[y, x]) - row_left.append(map_x_l[y, x]) - row_right.append(map_y_r[y, x]) - row_right.append(map_x_r[y, x]) - if (map_x_l.shape[1] % meshCellSize) % 2 != 0: - row_left.append(0) - row_left.append(0) - row_right.append(0) - row_right.append(0) - - mesh_left.append(row_left) - mesh_right.append(row_right) - - mesh_left = np.array(mesh_left) - mesh_right = np.array(mesh_right) - left_mesh_fpath = str(curr_path) + '/../resources/left_mesh.calib' - right_mesh_fpath = str(curr_path) + '/../resources/right_mesh.calib' - mesh_left.tofile(left_mesh_fpath) - mesh_right.tofile(right_mesh_fpath) diff --git a/resources/boards/AR0234DEV.json b/resources/boards/AR0234DEV.json deleted file mode 100644 index 9e1390f9c..000000000 --- a/resources/boards/AR0234DEV.json +++ /dev/null @@ -1,56 +0,0 @@ -{ - "board_config": - { - "name": "FFC-3P", - "revision": "R1M0E1", - "cameras":{ - "CAM_C": { - "name": "left", - "hfov": 140, - "type": "color", - "extrinsics": { - "to_cam": "CAM_B", - "specTranslation": { - "x": -7.5, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - }, - "CAM_B": { - "name": "right", - "hfov": 140, - "type": "color", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 0, - "y": -3.5, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - }, - "CAM_A": { - "name": "below", - "hfov": 140, - "type": "color" - } - - }, - "stereo_config":{ - "left_cam": "CAM_C", - "right_cam": "CAM_B" - } - } -} - diff --git a/resources/boards/BW1097.json b/resources/boards/BW1097.json deleted file mode 100644 index b7d6723b6..000000000 --- a/resources/boards/BW1097.json +++ /dev/null @@ -1,54 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-CM3", - "revision": "R2M2E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 71.86, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_C", - "specTranslation": { - "x": -9.0, - "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": 7, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} diff --git a/resources/boards/BW1098OBC.json b/resources/boards/BW1098OBC.json deleted file mode 100644 index 672df86c2..000000000 --- a/resources/boards/BW1098OBC.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D", - "revision": "R1M0E1", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/DM1092.json b/resources/boards/DM1092.json deleted file mode 100644 index ac029a371..000000000 --- a/resources/boards/DM1092.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-IoT-40", - "revision": "R2M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 71.86, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_C", - "specTranslation": { - "x": -4, - "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": 3, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/DM1097.json b/resources/boards/DM1097.json deleted file mode 100644 index 1b302f5ea..000000000 --- a/resources/boards/DM1097.json +++ /dev/null @@ -1,54 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-CM4", - "revision": "R2M0E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 71.86, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_C", - "specTranslation": { - "x": -9.0, - "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": 7, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} diff --git a/resources/boards/DM1098OAKD_WIFI.json b/resources/boards/DM1098OAKD_WIFI.json deleted file mode 100644 index 7f7d7a1de..000000000 --- a/resources/boards/DM1098OAKD_WIFI.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-IoT-75", - "revision": "R0M0E0", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-1-LITE-W.json b/resources/boards/OAK-1-LITE-W.json deleted file mode 100644 index 24cac5786..000000000 --- a/resources/boards/OAK-1-LITE-W.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1-Lite-W", - "revision": "R3M1E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - } - } - } -} \ No newline at end of file diff --git a/resources/boards/OAK-1-LITE.json b/resources/boards/OAK-1-LITE.json deleted file mode 100644 index ab97c8889..000000000 --- a/resources/boards/OAK-1-LITE.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1-LITE", - "revision": "R1M0E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - } - } - } -} \ No newline at end of file diff --git a/resources/boards/OAK-1-MAX.json b/resources/boards/OAK-1-MAX.json deleted file mode 100644 index 14be62a8a..000000000 --- a/resources/boards/OAK-1-MAX.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1-MAX", - "revision": "R3M1E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - } - } - } -} \ No newline at end of file diff --git a/resources/boards/OAK-1-POE.json b/resources/boards/OAK-1-POE.json deleted file mode 100644 index c682b0c97..000000000 --- a/resources/boards/OAK-1-POE.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1-POE", - "revision": "R2M1E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - } - } - - } -} - diff --git a/resources/boards/OAK-1-W.json b/resources/boards/OAK-1-W.json deleted file mode 100644 index 6dab92b13..000000000 --- a/resources/boards/OAK-1-W.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1-W", - "revision": "R3M1E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - } - } - } -} \ No newline at end of file diff --git a/resources/boards/OAK-1.json b/resources/boards/OAK-1.json deleted file mode 100644 index 2bbf8de9f..000000000 --- a/resources/boards/OAK-1.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "board_config": - { - "name": "OAK-1", - "revision": "R3M1E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - } - } - } -} \ No newline at end of file diff --git a/resources/boards/OAK-D-LITE.json b/resources/boards/OAK-D-LITE.json deleted file mode 100644 index 483ca6c8d..000000000 --- a/resources/boards/OAK-D-LITE.json +++ /dev/null @@ -1,56 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-LITE", - "revision": "R1M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 72.9, - "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": 72.9, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - - diff --git a/resources/boards/OAK-D-POE-W.json b/resources/boards/OAK-D-POE-W.json deleted file mode 100644 index a81caf948..000000000 --- a/resources/boards/OAK-D-POE-W.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D POE-W", - "revision": "R3M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-POE.json b/resources/boards/OAK-D-POE.json deleted file mode 100644 index b9d48c662..000000000 --- a/resources/boards/OAK-D-POE.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-POE", - "revision": "R3M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-POE-CUSTOM.json b/resources/boards/OAK-D-PRO-POE-CUSTOM.json deleted file mode 100644 index 502cab780..000000000 --- a/resources/boards/OAK-D-PRO-POE-CUSTOM.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO-POE-CUSTOM", - "revision": "R3M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 71.86, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-POE.json b/resources/boards/OAK-D-PRO-POE.json deleted file mode 100644 index f68487d30..000000000 --- a/resources/boards/OAK-D-PRO-POE.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO-POE", - "revision": "R3M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-W-CUSTOM.json b/resources/boards/OAK-D-PRO-W-CUSTOM.json deleted file mode 100644 index 742417bcb..000000000 --- a/resources/boards/OAK-D-PRO-W-CUSTOM.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D Pro-W", - "revision": "R3M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 127.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json b/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json deleted file mode 100644 index 7cb811ffa..000000000 --- a/resources/boards/OAK-D-PRO-W-POE-CUSTOM.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO-W-POE", - "revision": "R3M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 127.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-W-POE.json b/resources/boards/OAK-D-PRO-W-POE.json deleted file mode 100644 index 3f576b2c8..000000000 --- a/resources/boards/OAK-D-PRO-W-POE.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D Pro-W PoE", - "revision": "R3M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO-W.json b/resources/boards/OAK-D-PRO-W.json deleted file mode 100644 index b50c56a3f..000000000 --- a/resources/boards/OAK-D-PRO-W.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D Pro-W", - "revision": "R3M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-PRO.json b/resources/boards/OAK-D-PRO.json deleted file mode 100644 index 8a4cfe6f8..000000000 --- a/resources/boards/OAK-D-PRO.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-PRO", - "revision": "R3M1E3", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-S2.json b/resources/boards/OAK-D-S2.json deleted file mode 100644 index 9edbf341e..000000000 --- a/resources/boards/OAK-D-S2.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-S2", - "revision": "R2M0E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 68.7938, - "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": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/OAK-D-W.json b/resources/boards/OAK-D-W.json deleted file mode 100644 index 2d0e63a57..000000000 --- a/resources/boards/OAK-D-W.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "OAK-D-W", - "revision": "R3M2E2", - "cameras":{ - "CAM_A": { - "name": "rgb", - "hfov": 108.0, - "type": "color" - }, - "CAM_B": { - "name": "left", - "hfov": 127.0, - "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": 127.0, - "type": "mono", - "extrinsics": { - "to_cam": "CAM_A", - "specTranslation": { - "x": 3.75, - "y": 0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/TESTVER_NO_RGB.json b/resources/boards/TESTVER_NO_RGB.json deleted file mode 100644 index a55e1908f..000000000 --- a/resources/boards/TESTVER_NO_RGB.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "board_config": - { - "name": "TESTVER", - "revision": "R0M0E0", - "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", - "extrinsics": { - "to_cam": "CAM_D", - "specTranslation": { - "x": 0, - "y": -4.0, - "z": 0 - }, - "rotation":{ - "r": 0, - "p": 0, - "y": 0 - } - } - }, - "CAM_D": { - "name": "vertical", - "hfov": 71.86, - "type": "color" - } - }, - "stereo_config":{ - "left_cam": "CAM_B", - "right_cam": "CAM_C" - } - } -} - diff --git a/resources/boards/TESTVER_NO_VERTICAL.json b/resources/boards/TESTVER_NO_VERTICAL.json deleted file mode 100644 index 01fcc28f8..000000000 --- a/resources/boards/TESTVER_NO_VERTICAL.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "board_config": - { - "name": "TESTVER", - "revision": "R0M0E0", - "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" - } - } -} - From 3fa343793db835279b5c83bcb2026e9dd5d20c5f Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 20 Jul 2023 23:55:53 +0200 Subject: [PATCH 55/68] Adding changes in parameteres --- calibrate.py | 9 ++-- depthai_helpers/calibration_utils.py | 70 +++++----------------------- 2 files changed, 15 insertions(+), 64 deletions(-) diff --git a/calibrate.py b/calibrate.py index 241d53b1a..3494f17af 100755 --- a/calibrate.py +++ b/calibrate.py @@ -116,8 +116,6 @@ def parse_args(): help="number of chessboard squares in Y direction in charuco boards.") parser.add_argument("-rd", "--rectifiedDisp", default=True, action="store_false", help="Display rectified images with lines drawn for epipolar check") - parser.add_argument("-drgb", "--disableRgb", default=False, action="store_true", - help="Disable rgb camera Calibration") parser.add_argument("-slr", "--swapLR", default=False, action="store_true", help="Interchange Left and right camera port.") parser.add_argument("-m", "--mode", default=['capture', 'process'], nargs='*', type=str, required=False, @@ -161,7 +159,8 @@ def parse_args(): help="Set to trace the steps in calibration. Number from 1 to 5. If you want to display all, set trace number to 10.") parser.add_argument('-edms', '--enableDebugMessageSync', default=False, action="store_true", help="Display all the information in calibration.") - + parser.add_argument('-mst', '--minSyncTimestamp', type=float, default=0.03, + help="Minimum time difference between pictures taken from different cameras. Default: %(default)s ") options = parser.parse_args() # Set some extra defaults, `-brd` would override them @@ -605,7 +604,7 @@ def capture_images_sync(self): prev_time = None curr_time = None self.display_name = "Image Window" - syncCollector = MessageSync(len(self.camera_queue), 0.3 ) # 3ms tolerance + syncCollector = MessageSync(len(self.camera_queue), self.args.minSyncTimestamp) # 3ms tolerance syncCollector.enableDebugMessageSync = self.args.enableDebugMessageSync self.mouseTrigger = False while not finished: @@ -892,7 +891,7 @@ def calibrate(self): right_cam = result_config['cameras'][cam_info['extrinsics']['to_cam']]['name'] left_cam = cam_info['name'] - epipolar_threshold = 0.6 + epipolar_threshold = self.args.maxEpiploarError if cam_info['extrinsics']['epipolar_error'] > epipolar_threshold: color = red diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 0d31d77f3..74984685d 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -86,13 +86,12 @@ def polygon_from_image_name(image_name): class StereoCalibration(object): - global traceLevel """Class to Calculate Calibration and Rectify a Stereo Camera.""" def __init__(self): """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, display_all): + def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify): """Function to calculate calibration for stereo camera.""" start_time = time.time() # init object data @@ -169,7 +168,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ 'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32) extrinsics = self.calibrate_extrinsics(left_path, right_path, left_cam_info['intrinsics'], left_cam_info[ - 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, display_all) + 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation) if extrinsics[0] == -1: return -1, extrinsics[1] @@ -203,8 +202,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ extrinsics[3], # Left Rectification rotation extrinsics[4], # Right Rectification rotation extrinsics[5], # Left Rectification Intrinsics - extrinsics[6], # Right Rectification Intrinsics - display_all) + extrinsics[6]) # Right Rectification Intrinsics + left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] left_cam_info['extrinsics']['translation'] = extrinsics[2] left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] @@ -340,7 +339,7 @@ def calibrate_intrinsics(self, image_files, hfov): # (Height, width) return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage - def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation, display_all): + def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): self.objpoints = [] # 3d point in real world space self.imgpoints_l = [] # 2d points in image plane. self.imgpoints_r = [] # 2d points in image plane. @@ -403,7 +402,7 @@ def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, gu assert imsize_r == imsize_l, "Left and right resolution scaling is wrong" 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, display_all) + 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 @@ -531,7 +530,7 @@ def calibrate_fisheye(self, allCorners, allIds, imsize): return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) - def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, display_all): + def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in): left_corners_sampled = [] right_corners_sampled = [] obj_pts = [] @@ -750,23 +749,6 @@ def display_rectification(self, image_data_pairs, images_corners_l, images_corne 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.cvtColor(img_concat, cv2.COLOR_GRAY2RGB) - - # draw epipolar lines for debug purposes - - # 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 img_concat = cv2.resize( img_concat, (0, 0), fx=0.8, fy=0.8) @@ -994,7 +976,9 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) kScaledR = kScaledL = M_rp - print('Lets find the best epipolar Error') + if self.cameraModel != 'perspective': + kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1) + kScaledL = kScaledR if rectProjectionMode: kScaledL = p_lp @@ -1056,33 +1040,6 @@ def test_epipolar_charuco(self, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, - 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, display_all): if self.cameraModel == 'perspective': mapx_l, mapy_l = cv2.initUndistortRectifyMap( M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1) @@ -1094,15 +1051,9 @@ def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, mapx_r, mapy_r = cv2.fisheye.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) @@ -1144,6 +1095,7 @@ def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, image_epipolar_color = [] # new_imagePairs = []) for i, image_data_pair in enumerate(image_data_pairs): + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 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, From 86d35e3deb37af8a20efd4b37a61e59c29bef7be Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 21 Jul 2023 00:27:39 +0200 Subject: [PATCH 56/68] Testing on OAK-D-PRO and small changes --- calibrate.py | 23 +++++++++++++++++------ requirements.txt | 10 +++++----- 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/calibrate.py b/calibrate.py index 3494f17af..508969e62 100755 --- a/calibrate.py +++ b/calibrate.py @@ -60,7 +60,6 @@ '60': dai.CameraControl.AntiBandingMode.MAINS_60_HZ, } - def create_blank(width, height, rgb_color=(0, 0, 0)): """Create new image(numpy array) filled with certain color in RGB""" # Create black blank image @@ -128,7 +127,7 @@ def parse_args(): parser.add_argument("-ih", "--invertHorizontal", dest="invert_h", default=False, action="store_true", help="Invert horizontal axis of the camera for the display") parser.add_argument("-ep", "--maxEpiploarError", default="0.7", type=float, required=False, - help="Sets the maximum epiploar allowed with rectification") + help="Sets the maximum epiploar allowed with rectification. Default: %(default)s") parser.add_argument("-cm", "--cameraMode", default="perspective", type=str, required=False, help="Choose between perspective and Fisheye") parser.add_argument("-rlp", "--rgbLensPosition", default=135, type=int, @@ -239,7 +238,7 @@ def get_synced(self): class MessageSync: - def __init__(self, num_queues, min_diff_timestamp, max_num_messages=10, min_queue_depth=3): + def __init__(self, num_queues, min_diff_timestamp, max_num_messages=4, min_queue_depth=3): self.num_queues = num_queues self.min_diff_timestamp = min_diff_timestamp self.max_num_messages = max_num_messages @@ -262,7 +261,7 @@ def add_msg(self, name, msg): # print() # print() - def get_synced(self, enableDebugMessageSync): + def get_synced(self): # Atleast 3 messages should be buffered min_len = min([len(queue) for queue in self.queues.values()]) @@ -386,8 +385,20 @@ def __init__(self): # self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check() break - pipeline = self.create_pipeline() - self.device.startPipeline(pipeline) + self.charuco_board = cv2.aruco.CharucoBoard_create( + self.args.squaresX, self.args.squaresY, + self.args.squareSizeCm, + self.args.markerSizeCm, + self.aruco_dictionary) + + + def mouse_event_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + self.mouseTrigger = True + + def startPipeline(self): + pipeline = self.create_pipeline() + self.device.startPipeline(pipeline) self.camera_queue = {} for config_cam in self.board_config['cameras']: diff --git a/requirements.txt b/requirements.txt index 8def9dd06..59a73c082 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,10 +1,10 @@ requests==2.26.0 --extra-index-url https://www.piwheels.org/simple -opencv-contrib-python==4.5.4.58 -scipy -# -e ./depthai_sdk -# --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ -# pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" +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 +depthai-sdk==1.9.4 +--extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-depthai-data-local/wheels/ +pyqt5>5,<5.15.6 ; platform_machine != "armv6l" and platform_machine != "armv7l" and platform_machine != "aarch64" and platform_machine != "arm64" --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ depthai==2.21.2.0 Qt.py From 18c828eac533f02786907c7dcdca724e52f77681 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Sat, 22 Jul 2023 17:06:21 +0200 Subject: [PATCH 57/68] Change in rescale of text in combined images --- depthai_helpers/calibration_utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 74984685d..0055c9128 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -131,8 +131,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ cam_info['name'], intrinsics)) coverage_name = cam_info['name'] - print_text = f'Coverage Image of {coverage_name} with reprojection error of {ret}' - cv2.putText(coverageImage, print_text, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 0), 2) + print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}' + cv2.putText(coverageImage, print_text, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/coverageImage.shape[1], (0, 0, 0), 2) # coverageImages[coverage_name] = coverageImage coverage_file_path = filepath + '/' + coverage_name + '_coverage.png' cv2.imwrite(coverage_file_path, coverageImage) From 25038f16a9c7e32f26ae87d9d144faf57af7c734 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 27 Jul 2023 22:31:57 +0200 Subject: [PATCH 58/68] Adding the combinedCoverageimages --- depthai_helpers/calibration_utils.py | 46 ++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 9 deletions(-) diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py index 0055c9128..436809beb 100644 --- a/depthai_helpers/calibration_utils.py +++ b/depthai_helpers/calibration_utils.py @@ -110,16 +110,30 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ self.aruco_dictionary) # parameters = aruco.DetectorParameters_create() + combinedCoverageImage = None + resizeWidth, resizeHeight = 0, 0 assert mrk_size != None, "ERROR: marker size not set" for camera in board_config['cameras'].keys(): - combinedCoverageImage = None + cam_info = board_config['cameras'][camera] + images_path = filepath + '/' + cam_info['name'] + image_files = glob.glob(images_path + "/*") + image_files.sort() + for im in image_files: + frame = cv2.imread(im) + height, width, _ = frame.shape + widthRatio = resizeWidth / width + heightRatio = resizeHeight / height + if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0): + resizeWidth = width + resizeHeight = height + break + for camera in board_config['cameras'].keys(): cam_info = board_config['cameras'][camera] print( '<------------Calibrating {} ------------>'.format(cam_info['name'])) images_path = filepath + '/' + cam_info['name'] ret, intrinsics, dist_coeff, _, _, size, coverageImage = self.calibrate_intrinsics( images_path, cam_info['hfov']) - # coverageImages[cam_info['name']] = coverageImage cam_info['intrinsics'] = intrinsics cam_info['dist_coeff'] = dist_coeff cam_info['size'] = size # (Width, height) @@ -132,16 +146,30 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ coverage_name = cam_info['name'] print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}' - cv2.putText(coverageImage, print_text, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/coverageImage.shape[1], (0, 0, 0), 2) - # coverageImages[coverage_name] = coverageImage - coverage_file_path = filepath + '/' + coverage_name + '_coverage.png' - cv2.imwrite(coverage_file_path, coverageImage) + height, width, _ = coverageImage.shape + + if width > resizeWidth and height > resizeHeight: + coverageImage = cv2.resize( + coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width) + + height, width, _ = coverageImage.shape + if height > resizeHeight: + height_offset = (height - resizeHeight)//2 + coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :] + + height, width, _ = coverageImage.shape + height_offset = (resizeHeight - height)//2 + width_offset = (resizeWidth - width)//2 + subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0) + cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2) if combinedCoverageImage is None: - combinedCoverageImage = coverageImage + combinedCoverageImage = subImage else: - combinedCoverageImage = np.hstack((combinedCoverageImage, coverageImage)) + combinedCoverageImage = np.hstack((combinedCoverageImage, subImage)) + coverage_file_path = filepath + '/' + coverage_name + '_coverage.png' + cv2.imwrite(coverage_file_path, coverageImage) - combinedCoverageImage = cv2.resize(combinedCoverageImage, (0, 0), fx=0.7, fy=0.7) + combinedCoverageImage = cv2.resize(combinedCoverageImage, (0, 0), fx=0.45, fy=0.45) cv2.imshow('coverage image', combinedCoverageImage) cv2.waitKey(0) cv2.destroyAllWindows() From 4234a59b8cda1779c9a045f85eb674c0697fe5cf Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 27 Jul 2023 23:02:24 +0200 Subject: [PATCH 59/68] Update on calibrate.py --- calibrate.py | 21 ++++++++++++++++----- depthai_calibration | 2 +- resources/depthai_boards | 2 +- 3 files changed, 18 insertions(+), 7 deletions(-) diff --git a/calibrate.py b/calibrate.py index a360518cb..346ec86a2 100755 --- a/calibrate.py +++ b/calibrate.py @@ -60,7 +60,6 @@ '60': dai.CameraControl.AntiBandingMode.MAINS_60_HZ, } - def create_blank(width, height, rgb_color=(0, 0, 0)): """Create new image(numpy array) filled with certain color in RGB""" # Create black blank image @@ -240,7 +239,7 @@ def get_synced(self): class MessageSync: - def __init__(self, num_queues, min_diff_timestamp, max_num_messages=10, min_queue_depth=3): + def __init__(self, num_queues, min_diff_timestamp, max_num_messages=4, min_queue_depth=3): self.num_queues = num_queues self.min_diff_timestamp = min_diff_timestamp self.max_num_messages = max_num_messages @@ -263,7 +262,7 @@ def add_msg(self, name, msg): # print() # print() - def get_synced(self, enableDebugMessageSync): + def get_synced(self): # Atleast 3 messages should be buffered min_len = min([len(queue) for queue in self.queues.values()]) @@ -387,8 +386,20 @@ def __init__(self): # self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check() break - pipeline = self.create_pipeline() - self.device.startPipeline(pipeline) + self.charuco_board = cv2.aruco.CharucoBoard_create( + self.args.squaresX, self.args.squaresY, + self.args.squareSizeCm, + self.args.markerSizeCm, + self.aruco_dictionary) + + + def mouse_event_callback(self, event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + self.mouseTrigger = True + + def startPipeline(self): + pipeline = self.create_pipeline() + self.device.startPipeline(pipeline) self.camera_queue = {} for config_cam in self.board_config['cameras']: diff --git a/depthai_calibration b/depthai_calibration index cd43cec86..9a376c1d0 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit cd43cec864395ff23b88206a01f4cbca9bef6278 +Subproject commit 9a376c1d01682f41218e27e1c7f03b5b1bb4e2f2 diff --git a/resources/depthai_boards b/resources/depthai_boards index e9dfca057..400b12d17 160000 --- a/resources/depthai_boards +++ b/resources/depthai_boards @@ -1 +1 @@ -Subproject commit e9dfca0572a264cc93cb556c926e73a2609cdcae +Subproject commit 400b12d179aaee9ec387b85e847c095d906496f6 From 9f55bcdacc7dea328c4de6bcecd232dd3bc02528 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 27 Jul 2023 23:39:15 +0200 Subject: [PATCH 60/68] Adding the submodules and calibration file --- .gitmodules | 6 +- calibrate.py | 2 +- depthai_calibration | 1 + depthai_helpers/__init__.py | 0 depthai_helpers/app_manager.py | 84 -- depthai_helpers/calibration_utils.py | 1305 -------------------------- depthai_helpers/cli_utils.py | 41 - depthai_helpers/config_manager.py | 289 ------ depthai_helpers/image_scaler.py | 109 --- depthai_helpers/projector_3d.py | 148 --- depthai_helpers/supervisor.py | 75 -- depthai_helpers/version_check.py | 68 -- resources/depthai_boards | 2 +- 13 files changed, 6 insertions(+), 2124 deletions(-) create mode 160000 depthai_calibration delete mode 100644 depthai_helpers/__init__.py delete mode 100644 depthai_helpers/app_manager.py delete mode 100644 depthai_helpers/calibration_utils.py delete mode 100644 depthai_helpers/cli_utils.py delete mode 100644 depthai_helpers/config_manager.py delete mode 100644 depthai_helpers/image_scaler.py delete mode 100644 depthai_helpers/projector_3d.py delete mode 100644 depthai_helpers/supervisor.py delete mode 100644 depthai_helpers/version_check.py diff --git a/.gitmodules b/.gitmodules index de666a84a..fe44c0e8d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,6 +2,6 @@ path = resources/depthai_boards url = https://github.com/luxonis/depthai-boards -[submodule "depthai-boards"] - path = depthai-boards - url = https://github.com/luxonis/depthai-boards +[submodule "depthai_calibration"] + path = depthai_calibration + url = git@github.com:luxonis/depthai-calibration.git \ No newline at end of file diff --git a/calibrate.py b/calibrate.py index 508969e62..5c88e87a8 100755 --- a/calibrate.py +++ b/calibrate.py @@ -20,7 +20,7 @@ import numpy as np import copy -import depthai_helpers.calibration_utils as calibUtils +import depthai_calibration.calibration_utils as calibUtils font = cv2.FONT_HERSHEY_SIMPLEX debug = False diff --git a/depthai_calibration b/depthai_calibration new file mode 160000 index 000000000..9a376c1d0 --- /dev/null +++ b/depthai_calibration @@ -0,0 +1 @@ +Subproject commit 9a376c1d01682f41218e27e1c7f03b5b1bb4e2f2 diff --git a/depthai_helpers/__init__.py b/depthai_helpers/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/depthai_helpers/app_manager.py b/depthai_helpers/app_manager.py deleted file mode 100644 index cb3f22f43..000000000 --- a/depthai_helpers/app_manager.py +++ /dev/null @@ -1,84 +0,0 @@ -import os -import shutil -import signal -import subprocess -import sys -import time -from pathlib import Path - -initEnv = os.environ.copy() -if "PYTHONPATH" in initEnv: - initEnv["PYTHONPATH"] += ":" + str(Path(__file__).parent.parent.absolute()) -else: - initEnv["PYTHONPATH"] = str(Path(__file__).parent.parent.absolute()) - - - -def quoted(val): - return '"' + str(val) + '"' - -class App: - def __init__(self, appName, appPath=None, appRequirements=None, appEntrypoint=None): - self.appName = appName - self.appPath = appPath or Path(__file__).parent.parent / "apps" / self.appName - self.venvPath = self.appPath / "venv" - self.appPip = str(self.venvPath / "bin" / "pip") if os.name != 'nt' else (self.venvPath / "Scripts" / "pip.exe") - self.appInterpreter = str(self.venvPath / "bin" / "python") if os.name != 'nt' else (self.venvPath / "Scripts" / "python.exe") - self.appRequirements = appRequirements or self.appPath / "requirements.txt" - self.appEntrypoint = appEntrypoint or self.appPath / "main.py" - - def createVenv(self, force=False): - try: - subprocess.check_call(' '.join([quoted(sys.executable), '-m', 'venv', '-h']), env=initEnv, shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) - except: - print(f"Error accessing \"venv\" module! Please try to install \"python3.{sys.version_info[1]}-venv\" or see oficial docs here - https://docs.python.org/3/library/venv.html", file=sys.stderr) - sys.exit(1) - try: - subprocess.check_call(' '.join([quoted(sys.executable), '-m', 'pip', '-h']), env=initEnv, shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) - except: - print("Error accessing \"pip\" module! Please try to install \"python3-pip\" or see oficial docs here - https://pip.pypa.io/en/stable/installation/", file=sys.stderr) - sys.exit(1) - - if not force and Path(self.appInterpreter).exists() and Path(self.appPip).exists(): - print("Existing venv found.") - else: - if self.venvPath.exists(): - print("Recreating venv...") - shutil.rmtree(self.venvPath) - else: - print("Creating venv...") - try: - subprocess.check_call(' '.join([quoted(sys.executable), '-m', 'venv', quoted(str(self.venvPath.absolute()))]), shell=True, env=initEnv, cwd=self.appPath) - except: - print(f"Error creating a new virtual environment using \"venv\" module! Please try to install \"python3.{sys.version_info[1]}-venv\" again", file=sys.stderr) - sys.exit(1) - print("Installing requirements...") - subprocess.check_call(' '.join([quoted(self.appInterpreter), '-m', 'pip', 'install', '-U', 'pip']), env=initEnv, shell=True, cwd=self.appPath) - subprocess.check_call(' '.join([quoted(self.appInterpreter), '-m', 'pip', 'install', '--prefer-binary', '-r', quoted(str(self.appRequirements))]), env=initEnv, shell=True, cwd=self.appPath) - - def runApp(self, shouldRun = lambda: True): - # Passthrough args to the app - args = [quoted(arg) for arg in sys.argv[1:]] - args.insert(0, quoted(str(self.appEntrypoint))) - args.insert(0, quoted(self.appInterpreter)) - if os.name == 'nt': - pro = subprocess.Popen(' '.join(args), env=initEnv, shell=True, cwd=self.appPath) - else: - pro = subprocess.Popen(' '.join(args), env=initEnv, shell=True, cwd=self.appPath, preexec_fn=os.setsid) - while shouldRun() and pro.poll() is None: - try: - time.sleep(1) - except KeyboardInterrupt: - break - - # if pro.poll() is not None: - try: - if os.name == 'nt': - subprocess.call(['taskkill', '/F', '/T', '/PID', str(pro.pid)]) - else: - os.killpg(os.getpgid(pro.pid), signal.SIGTERM) - - except ProcessLookupError: - pass - - diff --git a/depthai_helpers/calibration_utils.py b/depthai_helpers/calibration_utils.py deleted file mode 100644 index 436809beb..000000000 --- a/depthai_helpers/calibration_utils.py +++ /dev/null @@ -1,1305 +0,0 @@ -#!/usr/bin/env python3 - -import cv2 -import glob -import os -import shutil -import numpy as np -from scipy.spatial.transform import Rotation -import time -import json -import cv2.aruco as aruco -from pathlib import Path -from functools import reduce -from collections import deque -# Creates a set of 13 polygon coordinates -rectProjectionMode = 0 - -colors = [(0, 255 , 0), (0, 0, 255)] -def setPolygonCoordinates(height, width): - horizontal_shift = width//4 - vertical_shift = height//4 - - margin = 60 - slope = 150 - - p_coordinates = [ - [[margin, margin], [margin, height-margin], - [width-margin, height-margin], [width-margin, margin]], - - [[margin, 0], [margin, height], [width//2, height-slope], [width//2, slope]], - [[horizontal_shift, 0], [horizontal_shift, height], [ - width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]], - [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 + - horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]], - - [[width-margin, 0], [width-margin, height], - [width//2, height-slope], [width//2, slope]], - [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width // - 2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]], - [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width // - 2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]], - - [[0, margin], [width, margin], [ - width-slope, height//2], [slope, height//2]], - [[0, vertical_shift], [width, vertical_shift], [width-slope, - height//2+vertical_shift], [slope, height//2+vertical_shift]], - [[0, vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope, - height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]], - - [[0, height-margin], [width, height-margin], - [width-slope, height//2], [slope, height//2]], - [[0, height-vertical_shift], [width, height-vertical_shift], [width - - slope, height//2-vertical_shift], [slope, height//2-vertical_shift]], - [[0, height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width - - slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]] - ] - return p_coordinates - - -def getPolygonCoordinates(idx, p_coordinates): - return p_coordinates[idx] - - -def getNumOfPolygons(p_coordinates): - return len(p_coordinates) - -# Filters polygons to just those at the given indexes. - - -def select_polygon_coords(p_coordinates, indexes): - if indexes == None: - # The default - return p_coordinates - else: - print("Filtering polygons to those at indexes=", indexes) - return [p_coordinates[i] for i in indexes] - - -def image_filename(stream_name, polygon_index, total_num_of_captured_images): - return "{stream_name}_p{polygon_index}_{total_num_of_captured_images}.png".format(stream_name=stream_name, polygon_index=polygon_index, total_num_of_captured_images=total_num_of_captured_images) - - -def polygon_from_image_name(image_name): - """Returns the polygon index from an image name (ex: "left_p10_0.png" => 10)""" - return int(re.findall("p(\d+)", image_name)[0]) - - -class StereoCalibration(object): - """Class to Calculate Calibration and Rectify a Stereo Camera.""" - - def __init__(self): - """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): - """Function to calculate calibration for stereo camera.""" - start_time = time.time() - # init object data - if self.traceLevel == 1 or self.traceLevel == 10: - print(f'squareX is {squaresX}') - self.enable_rectification_disp = enable_disp_rectify - self.cameraModel = camera_model - self.data_path = filepath - self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000) - - self.board = aruco.CharucoBoard_create( - # 22, 16, - squaresX, squaresY, - square_size, - mrk_size, - self.aruco_dictionary) - - # parameters = aruco.DetectorParameters_create() - combinedCoverageImage = None - resizeWidth, resizeHeight = 0, 0 - assert mrk_size != None, "ERROR: marker size not set" - for camera in board_config['cameras'].keys(): - cam_info = board_config['cameras'][camera] - images_path = filepath + '/' + cam_info['name'] - image_files = glob.glob(images_path + "/*") - image_files.sort() - for im in image_files: - frame = cv2.imread(im) - height, width, _ = frame.shape - widthRatio = resizeWidth / width - heightRatio = resizeHeight / height - if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0): - resizeWidth = width - resizeHeight = height - break - for camera in board_config['cameras'].keys(): - cam_info = board_config['cameras'][camera] - print( - '<------------Calibrating {} ------------>'.format(cam_info['name'])) - images_path = filepath + '/' + cam_info['name'] - ret, intrinsics, dist_coeff, _, _, size, coverageImage = self.calibrate_intrinsics( - images_path, cam_info['hfov']) - cam_info['intrinsics'] = intrinsics - cam_info['dist_coeff'] = dist_coeff - cam_info['size'] = size # (Width, height) - cam_info['reprojection_error'] = ret - print("Reprojection error of {0}: {1}".format( - cam_info['name'], ret)) - if self.traceLevel == 1 or self.traceLevel == 10: - print("Estimated intrinsics of {0}: \n {1}".format( - cam_info['name'], intrinsics)) - - coverage_name = cam_info['name'] - print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}' - height, width, _ = coverageImage.shape - - if width > resizeWidth and height > resizeHeight: - coverageImage = cv2.resize( - coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width) - - height, width, _ = coverageImage.shape - if height > resizeHeight: - height_offset = (height - resizeHeight)//2 - coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :] - - height, width, _ = coverageImage.shape - height_offset = (resizeHeight - height)//2 - width_offset = (resizeWidth - width)//2 - subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0) - cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2) - if combinedCoverageImage is None: - combinedCoverageImage = subImage - else: - combinedCoverageImage = np.hstack((combinedCoverageImage, subImage)) - coverage_file_path = filepath + '/' + coverage_name + '_coverage.png' - cv2.imwrite(coverage_file_path, coverageImage) - - combinedCoverageImage = cv2.resize(combinedCoverageImage, (0, 0), fx=0.45, fy=0.45) - cv2.imshow('coverage image', combinedCoverageImage) - cv2.waitKey(0) - cv2.destroyAllWindows() - - for camera in board_config['cameras'].keys(): - left_cam_info = board_config['cameras'][camera] - if 'extrinsics' in left_cam_info: - if 'to_cam' in left_cam_info['extrinsics']: - left_cam = camera - right_cam = left_cam_info['extrinsics']['to_cam'] - left_path = filepath + '/' + left_cam_info['name'] - - right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']] - right_path = filepath + '/' + right_cam_info['name'] - print('<-------------Extrinsics calibration of {} and {} ------------>'.format( - left_cam_info['name'], right_cam_info['name'])) - - specTranslation = left_cam_info['extrinsics']['specTranslation'] - rot = left_cam_info['extrinsics']['rotation'] - - translation = np.array( - [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32) - rotation = Rotation.from_euler( - 'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32) - - extrinsics = self.calibrate_extrinsics(left_path, right_path, left_cam_info['intrinsics'], left_cam_info[ - 'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation) - if extrinsics[0] == -1: - return -1, extrinsics[1] - - if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: - board_config['stereo_config']['rectification_left'] = extrinsics[3] - board_config['stereo_config']['rectification_right'] = extrinsics[4] - board_config['stereo_config']['p_left'] = extrinsics[5] - board_config['stereo_config']['p_right'] = extrinsics[6] - elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam: - board_config['stereo_config']['rectification_left'] = extrinsics[4] - board_config['stereo_config']['rectification_right'] = extrinsics[3] - board_config['stereo_config']['p_left'] = extrinsics[6] - board_config['stereo_config']['p_right'] = extrinsics[5] - - """ for stereoObj in board_config['stereo_config']: - - if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1: - stereoObj['rectification_left'] = extrinsics[3] - stereoObj['rectification_right'] = extrinsics[4] """ - - print('<-------------Epipolar error of {} and {} ------------>'.format( - left_cam_info['name'], right_cam_info['name'])) - left_cam_info['extrinsics']['epipolar_error'] = self.test_epipolar_charuco( - left_path, - right_path, - left_cam_info['intrinsics'], - left_cam_info['dist_coeff'], - right_cam_info['intrinsics'], - right_cam_info['dist_coeff'], - extrinsics[2], # Translation between left and right Cameras - extrinsics[3], # Left Rectification rotation - extrinsics[4], # Right Rectification rotation - extrinsics[5], # Left Rectification Intrinsics - extrinsics[6]) # Right Rectification Intrinsics - - left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1] - left_cam_info['extrinsics']['translation'] = extrinsics[2] - left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] - - return 1, board_config - - def draw_corners(self, charuco_corners, displayframe): - for corners in charuco_corners: - color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255))) - for corner in corners: - corner_int = (int(corner[0][0]), int(corner[0][1])) - cv2.circle(displayframe, corner_int, 4, color, -1) - height, width = displayframe.shape[:2] - start_point = (0, 0) # top of the image - end_point = (0, height) - - color = (0, 0, 0) # blue in BGR - thickness = 4 - - # Draw the line on the image - cv2.line(displayframe, start_point, end_point, color, thickness) - return displayframe - - def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)): - """ - Charuco base pose estimation. - """ - # print("POSE ESTIMATION STARTS:") - allCorners = [] - allIds = [] - all_marker_corners = [] - all_marker_ids = [] - all_recovered = [] - # decimator = 0 - # 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 self.traceLevel == 2 or self.traceLevel == 10: - 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 - 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 self.traceLevel == 1 or self.traceLevel == 10: - print('{0} number of Markers corners detected in the image {1}'.format( - len(marker_corners), img_pth.name)) - if len(marker_corners) > 0: - res2 = cv2.aruco.interpolateCornersCharuco( - marker_corners, ids, gray, self.board) - - # if res2[1] is not None and res2[2] is not None and len(res2[1])>3 and decimator%1==0: - if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3: - - cv2.cornerSubPix(gray, res2[1], - winSize=(5, 5), - zeroZone=(-1, -1), - criteria=criteria) - allCorners.append(res2[1]) # Charco chess corners - allIds.append(res2[2]) # charuco chess corner id's - all_marker_corners.append(marker_corners) - all_marker_ids.append(ids) - all_recovered.append(recoverd) - else: - raise RuntimeError("Failed to detect markers in the image") - else: - print(im + " Not found") - raise RuntimeError("Failed to detect markers in the image") - - # imsize = gray.shape[::-1] - return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered - - def calibrate_intrinsics(self, image_files, hfov): - image_files = glob.glob(image_files + "/*") - image_files.sort() - assert len( - image_files) != 0, "ERROR: Images not read correctly, check directory" - - allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files) - - coverageImage = np.ones(imsize[::-1], np.uint8) * 255 - coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR) - coverageImage = self.draw_corners(allCorners, coverageImage) - - if self.cameraModel == 'perspective': - ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_camera_charuco( - allCorners, allIds, imsize, hfov) - # (Height, width) - if self.traceLevel == 3 or self.traceLevel == 10: - self.fisheye_undistort_visualizaation( - image_files, camera_matrix, distortion_coefficients, imsize) - - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage - else: - print('Fisheye--------------------------------------------------') - ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors = self.calibrate_fisheye( - allCorners, allIds, imsize) - if self.traceLevel == 3 or self.traceLevel == 10: - self.fisheye_undistort_visualizaation( - image_files, camera_matrix, distortion_coefficients, imsize) - print('Fisheye rotation vector', rotation_vectors[0]) - print('Fisheye translation vector', translation_vectors[0]) - - # (Height, width) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, imsize, coverageImage - - def calibrate_extrinsics(self, images_left, images_right, M_l, d_l, M_r, d_r, guess_translation, guess_rotation): - self.objpoints = [] # 3d point in real world space - self.imgpoints_l = [] # 2d points in image plane. - self.imgpoints_r = [] # 2d points in image plane. - - images_left = glob.glob(images_left + "/*") - images_right = glob.glob(images_right + "/*") - - images_left.sort() - images_right.sort() - - assert len( - images_left) != 0, "ERROR: Images not found, check directory" - assert len( - images_right) != 0, "ERROR: Images not found, check directory" - - 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 - - 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]) - if self.traceLevel == 4 or self.traceLevel == 10: - 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)) - - # 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) - - # print("~~~~~~~~~~~ POSE ESTIMATION LEFT CAMERA ~~~~~~~~~~~~~") - allCorners_l, allIds_l, _, _, imsize_l, _ = self.analyze_charuco( - images_left, scale_req, scaled_res) - - # print("~~~~~~~~~~~ POSE ESTIMATION RIGHT CAMERA ~~~~~~~~~~~~~") - allCorners_r, allIds_r, _, _, imsize_r, _ = self.analyze_charuco( - images_right, scale_req, scaled_res) - if self.traceLevel == 4 or self.traceLevel == 10: - print(f'Image size of right side (w, h): {imsize_r}') - print(f'Image size of left side (w, h): {imsize_l}') - - assert imsize_r == imsize_l, "Left and right resolution scaling is wrong" - - 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 self.traceLevel == 2 or self.traceLevel == 10: - 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): - 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) - # 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), kScaled, img_size, cv2.CV_32FC1) - else: - map1, map2 = cv2.fisheye.initUndistortRectifyMap( - K, D, np.eye(3), K, 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 self.traceLevel == 4 or self.traceLevel == 10: - 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): - """ - 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("INTRINSIC CALIBRATION") - 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 self.traceLevel == 1 or self.traceLevel == 10: - print( - f'Camera Matrix initialization with HFOV of {hfov} is.............') - print(cameraMatrixInit) - - distCoeffsInit = np.zeros((5, 1)) - flags = (cv2.CALIB_USE_INTRINSIC_GUESS + - cv2.CALIB_RATIONAL_MODEL) - - # flags = (cv2.CALIB_RATIONAL_MODEL) - (ret, camera_matrix, distortion_coefficients, - rotation_vectors, translation_vectors, - stdDeviationsIntrinsics, stdDeviationsExtrinsics, - perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended( - charucoCorners=allCorners, - charucoIds=allIds, - board=self.board, - imageSize=imsize, - cameraMatrix=cameraMatrixInit, - distCoeffs=distCoeffsInit, - flags=flags, - criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9)) - if self.traceLevel == 2 or self.traceLevel == 10: - print('Per View Errors...') - print(perViewErrors) - return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors - - def calibrate_fisheye(self, allCorners, allIds, imsize): - one_pts = self.board.chessboardCorners - obj_points = [] - for i in range(len(allIds)): - obj_pts_sub = [] - for j in range(len(allIds[i])): - obj_pts_sub.append(one_pts[allIds[i][j]]) - obj_points.append(np.array(obj_pts_sub, dtype=np.float32)) - - cameraMatrixInit = np.array([[907.84859625, 0.0 , 995.15888273], - [ 0.0 , 889.29269629, 627.49748034], - [ 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 - # flags |= cv2.fisheye.CALIB_FIX_SKEW - distCoeffsInit = np.zeros((4, 1)) - term_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 50000, 1e-9) - - return cv2.fisheye.calibrate(obj_points, allCorners, imsize, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria) - - def calibrate_stereo(self, allCorners_l, allIds_l, allCorners_r, allIds_r, imsize, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in): - left_corners_sampled = [] - right_corners_sampled = [] - obj_pts = [] - one_pts = self.board.chessboardCorners - - if self.traceLevel == 5 or self.traceLevel == 10: - print('Length of allIds_l') - print(len(allIds_l)) - print('Length of allIds_r') - print(len(allIds_r)) - print('allIds_l') - print(allIds_l) - print('allIds_r') - print(allIds_r) - - for i in range(len(allIds_l)): - left_sub_corners = [] - right_sub_corners = [] - obj_pts_sub = [] - #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70: - # continue - for j in range(len(allIds_l[i])): - idx = np.where(allIds_r[i] == allIds_l[i][j]) - if idx[0].size == 0: - continue - left_sub_corners.append(allCorners_l[i][j]) - right_sub_corners.append(allCorners_r[i][idx]) - obj_pts_sub.append(one_pts[allIds_l[i][j]]) - if len(left_sub_corners) > 3 and len(right_sub_corners) > 3: - obj_pts.append(np.array(obj_pts_sub, dtype=np.float32)) - left_corners_sampled.append( - np.array(left_sub_corners, dtype=np.float32)) - right_corners_sampled.append( - np.array(right_sub_corners, dtype=np.float32)) - else: - return -1, "Stereo Calib failed due to less common features" - - stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT + - cv2.TERM_CRITERIA_EPS, 1000, 1e-9) - - if self.cameraModel == 'perspective': - flags = 0 - # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS - # print(flags) - - flags |= cv2.CALIB_FIX_INTRINSIC - # flags |= cv2.CALIB_USE_INTRINSIC_GUESS - flags |= cv2.CALIB_RATIONAL_MODEL - # print(flags) - if self.traceLevel == 1 or self.traceLevel == 10: - print('Printing Extrinsics guesses...') - print(r_in) - print(t_in) - 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) - - r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True) - print(f'Reprojection error is {ret}') - if self.traceLevel == 5 or self.traceLevel == 10: - print('Printing Extrinsics res...') - print(R) - print(T) - print(f'Euler angles in XYZ {r_euler} degs') - - - R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( - cameraMatrix_l, - distCoeff_l, - cameraMatrix_r, - distCoeff_r, - imsize, R, T) # , alpha=0.1 - # self.P_l = P_l - # self.P_r = P_r - r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) - if self.traceLevel == 5 or self.traceLevel == 10: - print(f'R_L Euler angles in XYZ {r_euler}') - r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) - if self.traceLevel == 5 or self.traceLevel == 10: - print(f'R_R Euler angles in XYZ {r_euler}') - - # print(f'P_l is \n {P_l}') - # print(f'P_r is \n {P_r}') - - return [ret, R, T, R_l, R_r, P_l, P_r] - elif self.cameraModel == 'fisheye': - # 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 = 0 - # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC - # flags |= cv2.fisheye.CALIB_CHECK_COND - # flags |= cv2.fisheye.CALIB_FIX_SKEW - flags |= cv2.fisheye.CALIB_FIX_INTRINSIC - flags |= cv2.fisheye.CALIB_FIX_K1 - flags |= cv2.fisheye.CALIB_FIX_K2 - flags |= cv2.fisheye.CALIB_FIX_K3 - flags |= cv2.fisheye.CALIB_FIX_K4 - # 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 self.traceLevel == 3 or self.traceLevel == 10: - print('Fisyeye stereo model..................') - (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_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True) - print(f'Reprojection error is {ret}') - if self.traceLevel == 3 or self.traceLevel == 10: - print('Printing Extrinsics res...') - print(R) - print(T) - print(f'Euler angles in XYZ {r_euler} degs') - isHorizontal = np.absolute(T[0]) > np.absolute(T[1]) - - if 0: - if not isHorizontal: - rotated_k_l = cameraMatrix_l.copy() - rotated_k_r = cameraMatrix_r.copy() - rotated_k_l[0][0] = cameraMatrix_l[1][1] # swap fx and fy - rotated_k_r[0][0] = cameraMatrix_r[1][1] # swap fx and fy - rotated_k_l[1][1] = cameraMatrix_l[0][0] # swap fx and fy - rotated_k_r[1][1] = cameraMatrix_r[0][0] # swap fx and fy - - rotated_k_l[0][2] = cameraMatrix_l[1][2] # swap optical center x and y - rotated_k_r[0][2] = cameraMatrix_r[1][2] # swap optical center x and y - rotated_k_l[1][2] = cameraMatrix_l[0][2] # swap optical center x and y - rotated_k_r[1][2] = cameraMatrix_r[0][2] # swap optical center x and y - - T_mod = T.copy() - T_mod[0] = T[1] - T_mod[1] = T[0] - - r = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True) - R_mod = r.as_matrix() - print(f' Image size is {imsize} and modified iamge size is {imsize[::-1]}') - R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( - rotated_k_l, - distCoeff_l, - rotated_k_r, - distCoeff_r, - imsize[::-1], R_mod, T_mod, flags=0) - # TODO revier things back to original style for Rotation and translation - r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) - R_l = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True).as_matrix() - - r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) - R_r = Rotation.from_euler('xyz', [r_euler[1], r_euler[0], r_euler[2]], degrees=True).as_matrix() - - temp = P_l[0][0] - P_l[0][0] = P_l[1][1] - P_l[1][1] = temp - temp = P_r[0][0] - P_r[0][0] = P_r[1][1] - P_r[1][1] = temp - - temp = P_l[0][2] - P_l[0][2] = P_l[1][2] - P_l[1][2] = temp - temp = P_r[0][2] - P_r[0][2] = P_r[1][2] - P_r[1][2] = temp - - temp = P_l[0][3] - P_l[0][3] = P_l[1][3] - P_l[1][3] = temp - temp = P_r[0][3] - P_r[0][3] = P_r[1][3] - P_r[1][3] = temp - else: - R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify( - cameraMatrix_l, - distCoeff_l, - cameraMatrix_r, - distCoeff_r, - imsize, R, T, flags=0) - R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify( - cameraMatrix_l, - distCoeff_l, - cameraMatrix_r, - distCoeff_r, - imsize, R, T) # , alpha=0.1 - - r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True) - if self.traceLevel == 3 or self.traceLevel == 10: - print(f'R_L Euler angles in XYZ {r_euler}') - r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True) - if self.traceLevel == 3 or self.traceLevel == 10: - print(f'R_R Euler angles in XYZ {r_euler}') - - return [ret, R, T, R_l, R_r, P_l, P_r] - - 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 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]]) - 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.8, fy=0.8) - - # show image - cv2.imshow('Stereo Pair', img_concat) - k = cv2.waitKey(0) - 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]) - if self.traceLevel == 2 or self.traceLevel == 10: - 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): - 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 = [] - imagesCount = 0 - - 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) - - # 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)) - - 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) - - 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') - images_left.sort() - images_right.sort() - assert len(images_left) != 0, "ERROR: Images not read correctly" - assert len(images_right) != 0, "ERROR: Images not read correctly" - isHorizontal = np.absolute(t[0]) > np.absolute(t[1]) - - 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 diff < 0: - scaled_res = (int(scaled_height), scaled_res[1]) - if self.traceLevel == 4 or self.traceLevel == 10: - 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)) - - M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res) - M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res) - if rectProjectionMode: - p_lp = self.scale_intrinsics(p_l, frame_left_shape, scaled_res) - p_rp = self.scale_intrinsics(p_r, frame_right_shape, scaled_res) - print('Projection intrinsics ....') - print(p_lp) - print(p_rp) - - criteria = (cv2.TERM_CRITERIA_EPS + - cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001) - - # TODO(Sachin): Observe Images by adding visualization - # TODO(Sachin): Check if the stetch is only in calibration Images - print('Original intrinsics ....') - print(f"L {M_lp}") - print(f"R: {M_rp}") - if self.traceLevel == 4 or self.traceLevel == 10: - print(f'Width and height is {scaled_res[::-1]}') - # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) - # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0) - # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0) - kScaledR = kScaledL = M_rp - - if self.cameraModel != 'perspective': - kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1) - kScaledL = kScaledR - - if rectProjectionMode: - kScaledL = p_lp - kScaledR = p_rp - - print('Intrinsics from the getOptimalNewCameraMatrix/Original ....') - print(f"L: {kScaledL}") - print(f"R: {kScaledR}") - oldEpipolarError = None - epQueue = deque() - movePos = True - if 0: - while True: - - epError = self.sgdEpipolar(images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal) - - if oldEpipolarError is None: - epQueue.append((epError, kScaledR)) - oldEpipolarError = epError - kScaledR[0][0] += 1 - kScaledR[1][1] += 1 - continue - if movePos: - if epError < oldEpipolarError: - epQueue.append((epError, kScaledR)) - oldEpipolarError = epError - kScaledR[0][0] += 1 - kScaledR[1][1] += 1 - else: - movePos = False - startPos = epQueue.popleft() - oldEpipolarError = startPos[0] - kScaledR = startPos[1] - epQueue.appendleft((oldEpipolarError, kScaledR)) - kScaledR[0][0] -= 1 - kScaledR[1][1] -= 1 - else: - if epError < oldEpipolarError: - epQueue.appendleft((epError, kScaledR)) - oldEpipolarError = epError - kScaledR[0][0] -= 1 - kScaledR[1][1] -= 1 - else: - break - oldEpipolarError = None - while epQueue: - currEp, currK = epQueue.popleft() - if oldEpipolarError is None: - oldEpipolarError = currEp - kScaledR = currK - else: - currEp, currK = epQueue.popleft() - if currEp < oldEpipolarError: - oldEpipolarError = currEp - kScaledR = currK - - - #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): - # read images - 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)) - - if self.traceLevel == 4 or self.traceLevel == 10: - cv2.imshow("undistorted-Left", img_l) - cv2.imshow("undistorted-right", img_r) - # 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 - if self.traceLevel == 4 or self.traceLevel == 10: - cv2.destroyWindow("undistorted-Left") - cv2.destroyWindow("undistorted-right") - # compute metrics - imgpoints_r = [] - imgpoints_l = [] - image_epipolar_color = [] - # new_imagePairs = []) - for i, image_data_pair in enumerate(image_data_pairs): - # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - 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) - print(f'Marekrs length for pair {i} is: L {len(marker_corners_l)} | R {len(marker_corners_r)} ') - #print(f'Marekrs length l is {len(marker_corners_l)}') - 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.append(corners_l) - 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: - 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: - 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) - if self.traceLevel == 4 or self.traceLevel == 10: - print("Epipolar 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))) - return -1 - lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA) - rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA) - image_data_pairs[i] = (lText, rText) - - - epi_error_sum = 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 / total_corners - print("Average Epipolar Error is : " + str(avg_epipolar)) - - if self.enable_rectification_disp: - self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal) - - return avg_epipolar - - def create_save_mesh(self): # , output_path): - - curr_path = Path(__file__).parent.resolve() - print("Mesh path") - print(curr_path) - - 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) - map_y_l_fp32 = map_y_l.astype(np.float32) - map_x_r_fp32 = map_x_r.astype(np.float32) - map_y_r_fp32 = map_y_r.astype(np.float32) - - - print("shape of maps") - print(map_x_l.shape) - print(map_y_l.shape) - print(map_x_r.shape) - print(map_y_r.shape) """ - - meshCellSize = 16 - mesh_left = [] - mesh_right = [] - - for y in range(map_x_l.shape[0] + 1): - if y % meshCellSize == 0: - row_left = [] - row_right = [] - for x in range(map_x_l.shape[1] + 1): - if x % meshCellSize == 0: - if y == map_x_l.shape[0] and x == map_x_l.shape[1]: - row_left.append(map_y_l[y - 1, x - 1]) - row_left.append(map_x_l[y - 1, x - 1]) - row_right.append(map_y_r[y - 1, x - 1]) - row_right.append(map_x_r[y - 1, x - 1]) - elif y == map_x_l.shape[0]: - row_left.append(map_y_l[y - 1, x]) - row_left.append(map_x_l[y - 1, x]) - row_right.append(map_y_r[y - 1, x]) - row_right.append(map_x_r[y - 1, x]) - elif x == map_x_l.shape[1]: - row_left.append(map_y_l[y, x - 1]) - row_left.append(map_x_l[y, x - 1]) - row_right.append(map_y_r[y, x - 1]) - row_right.append(map_x_r[y, x - 1]) - else: - row_left.append(map_y_l[y, x]) - row_left.append(map_x_l[y, x]) - row_right.append(map_y_r[y, x]) - row_right.append(map_x_r[y, x]) - if (map_x_l.shape[1] % meshCellSize) % 2 != 0: - row_left.append(0) - row_left.append(0) - row_right.append(0) - row_right.append(0) - - mesh_left.append(row_left) - mesh_right.append(row_right) - - mesh_left = np.array(mesh_left) - mesh_right = np.array(mesh_right) - left_mesh_fpath = str(curr_path) + '/../resources/left_mesh.calib' - right_mesh_fpath = str(curr_path) + '/../resources/right_mesh.calib' - mesh_left.tofile(left_mesh_fpath) - mesh_right.tofile(right_mesh_fpath) \ No newline at end of file diff --git a/depthai_helpers/cli_utils.py b/depthai_helpers/cli_utils.py deleted file mode 100644 index 1122ff31c..000000000 --- a/depthai_helpers/cli_utils.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python3 - -from types import SimpleNamespace - - -class RangeFloat(object): - def __init__(self, start, end): - self.start = start - self.end = end - - def __eq__(self, other): - return self.start <= other <= self.end - - def __contains__(self, item): - return self.__eq__(item) - - def __iter__(self): - yield self - - def __str__(self): - return '[{0},{1}]'.format(self.start, self.end) - - -PrintColors = SimpleNamespace( - HEADER="\033[95m", - BLUE="\033[94m", - GREEN="\033[92m", - RED="\033[91m", - WARNING="\033[1;5;31m", - FAIL="\033[91m", - ENDC="\033[0m", - BOLD="\033[1m", - UNDERLINE="\033[4m", - BLACK_BG_RED="\033[1;31;40m", - BLACK_BG_GREEN="\033[1;32;40m", - BLACK_BG_BLUE="\033[1;34;40m", -) - - -def cliPrint(msg, print_color): - print("{0}{1}{2}".format(print_color, msg, PrintColors.ENDC)) diff --git a/depthai_helpers/config_manager.py b/depthai_helpers/config_manager.py deleted file mode 100644 index 26d5d3987..000000000 --- a/depthai_helpers/config_manager.py +++ /dev/null @@ -1,289 +0,0 @@ -import os -import platform -import subprocess -from pathlib import Path -import cv2 -import depthai as dai -import numpy as np - -from depthai_helpers.cli_utils import cliPrint, PrintColors -from depthai_sdk.previews import Previews - - -DEPTHAI_ZOO = Path(__file__).parent.parent / Path(f"resources/nn/") -DEPTHAI_VIDEOS = Path(__file__).parent.parent / Path(f"videos/") -DEPTHAI_VIDEOS.mkdir(exist_ok=True) - - -class ConfigManager: - labels = "" - customFwCommit = '' - - def __init__(self, args): - self.args = args - - # Get resolution width as it's required by some functions - self.rgbResWidth = self.rgbResolutionWidth(self.args.rgbResolution) - - self.args.encode = dict(self.args.encode) - self.args.cameraOrientation = dict(self.args.cameraOrientation) - if (Previews.left.name in self.args.cameraOrientation or Previews.right.name in self.args.cameraOrientation) and self.useDepth: - print("[WARNING] Changing mono cameras orientation may result in incorrect depth/disparity maps") - - def rgbResolutionWidth(self, res: dai.ColorCameraProperties.SensorResolution) -> int: - if res == dai.ColorCameraProperties.SensorResolution.THE_720_P: return 720 - elif res == dai.ColorCameraProperties.SensorResolution.THE_800_P: return 800 - elif res == dai.ColorCameraProperties.SensorResolution.THE_1080_P: return 1080 - elif res == dai.ColorCameraProperties.SensorResolution.THE_4_K: return 2160 - elif res == dai.ColorCameraProperties.SensorResolution.THE_12_MP: return 3040 - elif res == dai.ColorCameraProperties.SensorResolution.THE_13_MP: return 3120 - else: raise Exception('Resolution not supported!') - - # Not needed, but might be useful for SDK in the future - # def _monoResWidth(self, res: dai.MonoCameraProperties.SensorResolution) -> int: - # if res == dai.MonoCameraProperties.SensorResolution.THE_400_P: return 400 - # elif res == dai.MonoCameraProperties.SensorResolution.THE_480_P: return 480 - # elif res == dai.MonoCameraProperties.SensorResolution.THE_720_P: return 720 - # elif res == dai.MonoCameraProperties.SensorResolution.THE_800_P: return 800 - # else: raise Exception('Resolution not supported!') - - @property - def debug(self): - return not self.args.noDebug - - @property - def useCamera(self): - return not self.args.video - - @property - def useNN(self): - return not self.args.disableNeuralNetwork - - @property - def useDepth(self): - return not self.args.disableDepth and self.useCamera - - @property - def maxDisparity(self): - maxDisparity = 95 - if (self.args.extendedDisparity): - maxDisparity *= 2 - if (self.args.subpixel): - maxDisparity *= 32 - - return maxDisparity - - def getModelSource(self): - if not self.useCamera: - return "host" - if self.args.camera == "left": - if self.useDepth: - return "rectifiedLeft" - return "left" - if self.args.camera == "right": - if self.useDepth: - return "rectifiedRight" - return "right" - if self.args.camera == "color": - return "color" - - def irEnabled(self, device): - try: - drivers = device.getIrDrivers() - return len(drivers) > 0 - except RuntimeError: - return False - - def getModelName(self): - if self.args.cnnModel: - return self.args.cnnModel - modelDir = self.getModelDir() - if modelDir is not None: - return Path(modelDir).stem - - def getModelDir(self): - if self.args.cnnPath: - return self.args.cnnPath - if self.args.cnnModel is not None and (DEPTHAI_ZOO / self.args.cnnModel).exists(): - return DEPTHAI_ZOO / self.args.cnnModel - - def getAvailableZooModels(self): - def verify(path: Path): - return path.parent.name == path.stem - - def convert(path: Path): - return path.stem - - return list(map(convert, filter(verify, DEPTHAI_ZOO.rglob("**/*.json")))) - - def getColorMap(self): - cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), getattr(cv2, "COLORMAP_{}".format(self.args.colorMap))) - cvColorMap[0] = [0, 0, 0] - return cvColorMap - - def getUsb2Mode(self): - if self.args['forceUsb2']: - cliPrint("FORCE USB2 MODE", PrintColors.WARNING) - usb2Mode = True - else: - usb2Mode = False - return usb2Mode - - def adjustPreviewToOptions(self): - if len(self.args.show) != 0: - depthPreviews = [Previews.rectifiedRight.name, Previews.rectifiedLeft.name, Previews.depth.name, - Previews.depthRaw.name, Previews.disparity.name, Previews.disparityColor.name] - - if len([preview for preview in self.args.show if preview in depthPreviews]) == 0 and not self.useNN: - print("No depth-related previews chosen, disabling depth...") - self.args.disableDepth = True - return - - self.args.show.append(Previews.color.name) - if self.useDepth: - self.args.show.append(Previews.disparityColor.name) - - if self.args.guiType == "qt": - if self.useNN: - self.args.show.append(Previews.nnInput.name) - - if self.useDepth: - if self.lowBandwidth: - self.args.show.append(Previews.disparityColor.name) - else: - self.args.show.append(Previews.depthRaw.name) - self.args.show.append(Previews.rectifiedLeft.name) - self.args.show.append(Previews.rectifiedRight.name) - else: - self.args.show.append(Previews.left.name) - self.args.show.append(Previews.right.name) - - def adjustParamsToDevice(self, device): - deviceInfo = device.getDeviceInfo() - cams = device.getConnectedCameras() - depthEnabled = dai.CameraBoardSocket.LEFT in cams and dai.CameraBoardSocket.RIGHT in cams - - sensorNames = device.getCameraSensorNames() - if dai.CameraBoardSocket.RGB in cams: - name = sensorNames[dai.CameraBoardSocket.RGB] - if name == 'OV9782': - if self.rgbResWidth not in [720, 800]: - self.args.rgbResolution = dai.ColorCameraProperties.SensorResolution.THE_800_P - cliPrint(f'{name} requires 720 or 800 resolution, defaulting to {self.args.rgbResolution}', - PrintColors.RED) - else: - if self.rgbResWidth in [720, 800]: - self.args.rgbResolution = dai.ColorCameraProperties.SensorResolution.THE_1080_P - cliPrint(f'{name} doesn\'t support 720 / 800 resolutions, defaulting to {self.args.rgbResolution}', - PrintColors.RED) - - if not depthEnabled: - if not self.args.disableDepth: - print("Disabling depth...") - self.args.disableDepth = True - if self.args.spatialBoundingBox: - print("Disabling spatial bounding boxes...") - self.args.spatialBoundingBox = False - if self.args.camera != 'color': - print("Switching source to RGB camera...") - self.args.camera = 'color' - updatedShowArg = [] - for name in self.args.show: - if name in ("nnInput", "color"): - updatedShowArg.append(name) - else: - print("Disabling {} preview...".format(name)) - if len(updatedShowArg) == 0: - print("No previews available, adding defaults...") - updatedShowArg.append("color") - if self.useNN: - updatedShowArg.append("nnInput") - self.args.show = updatedShowArg - - if self.args.bandwidth == "auto": - if deviceInfo.protocol != dai.XLinkProtocol.X_LINK_USB_VSC: - print("Enabling low-bandwidth mode due to connection mode... (protocol: {})".format(deviceInfo.protocol)) - self.args.bandwidth = "low" - print("Setting PoE video quality to 50 to reduce latency...") - self.args.poeQuality = 50 - elif device.getUsbSpeed() not in [dai.UsbSpeed.SUPER, dai.UsbSpeed.SUPER_PLUS]: - print("Enabling low-bandwidth mode due to low USB speed... (speed: {})".format(device.getUsbSpeed())) - self.args.bandwidth = "low" - else: - self.args.bandwidth = "high" - - def linuxCheckApplyUsbRules(self): - if platform.system() == 'Linux': - ret = subprocess.call(['grep', '-irn', 'ATTRS{idVendor}=="03e7"', '/etc/udev/rules.d'], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) - if(ret != 0): - cliPrint("WARNING: Usb rules not found", PrintColors.WARNING) - cliPrint(""" -Run the following commands to set USB rules: - -$ echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' | sudo tee /etc/udev/rules.d/80-movidius.rules -$ sudo udevadm control --reload-rules && sudo udevadm trigger - -After executing these commands, disconnect and reconnect USB cable to your OAK device""", PrintColors.RED) - os._exit(1) - - def getCountLabel(self, nnetManager): - if self.args.countLabel is None: - return None - - if self.args.countLabel.isdigit(): - obj = nnetManager.getLabelText(int(self.args.countLabel)).lower() - print(f"Counting number of {obj} in the frame") - return obj - else: return self.args.countLabel.lower() - - @property - def leftCameraEnabled(self): - return (self.args.camera == Previews.left.name and self.useNN) or \ - Previews.left.name in self.args.show or \ - Previews.rectifiedLeft.name in self.args.show or \ - self.useDepth - - @property - def rightCameraEnabled(self): - return (self.args.camera == Previews.right.name and self.useNN) or \ - Previews.right.name in self.args.show or \ - Previews.rectifiedRight.name in self.args.show or \ - self.useDepth - - @property - def rgbCameraEnabled(self): - return (self.args.camera == Previews.color.name and self.useNN) or \ - Previews.color.name in self.args.show - - @property - def inputSize(self): - return tuple(map(int, self.args.cnnInputSize.split('x'))) if self.args.cnnInputSize else None - - @property - def previewSize(self): - return (576, 320) - - @property - def lowBandwidth(self): - return self.args.bandwidth == "low" - - @property - def lowCapabilities(self): - return platform.machine().startswith("arm") or platform.machine().startswith("aarch") - - @property - def shaves(self): - if self.args.shaves is not None: - return self.args.shaves - if not self.useCamera: - return 8 - if self.rgbResWidth > 1080: - return 5 - return 6 - - @property - def dispMultiplier(self): - val = 255 / self.maxDisparity - return val - - diff --git a/depthai_helpers/image_scaler.py b/depthai_helpers/image_scaler.py deleted file mode 100644 index b45e05afc..000000000 --- a/depthai_helpers/image_scaler.py +++ /dev/null @@ -1,109 +0,0 @@ -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/depthai_helpers/projector_3d.py b/depthai_helpers/projector_3d.py deleted file mode 100644 index 8466e8111..000000000 --- a/depthai_helpers/projector_3d.py +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/env python3 -import traceback -import sys - -try: - import open3d as o3d -except ImportError: - traceback.print_exc() - print("Importing open3d failed, please run the following command or visit https://pypi.org/project/open3d/") - print() - print(sys.executable + " -m pip install open3d") - - -class PointCloudVisualizer(): - def __init__(self, intrinsic_matrix, width, height): - self.depth_map = None - self.rgb = None - self.pcl = None - # self.depth_scale = depth_scale - # self.depth_trunc = depth_trunc - # self.rgbd_mode = rgbd_mode - # self.pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(intrinsic_file) - - self.pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(width, - height, - intrinsic_matrix[0][0], - intrinsic_matrix[1][1], - intrinsic_matrix[0][2], - intrinsic_matrix[1][2]) - self.vis = o3d.visualization.Visualizer() - self.vis.create_window() - self.isstarted = False - - - # intrinsic = o3d.camera.PinholeCameraIntrinsic() - # print(str(type(intrinsic))) - # open3d::camera::PinholeCameraIntrinsic instrinsic; # TODO: Try this - # instrinsic.SetIntrinsics(480, 272, 282.15, 321.651, 270, 153.9); - # self.vis.add_geometry(self.pcl) - - - # def dummy_pcl(self): - # x = np.linspace(-3, 3, 401) - # mesh_x, mesh_y = np.meshgrid(x, x) - # z = np.sinc((np.power(mesh_x, 2) + np.power(mesh_y, 2))) - # z_norm = (z - z.min()) / (z.max() - z.min()) - # xyz = np.zeros((np.size(mesh_x), 3)) - # xyz[:, 0] = np.reshape(mesh_x, -1) - # xyz[:, 1] = np.reshape(mesh_y, -1) - # xyz[:, 2] = np.reshape(z_norm, -1) - # pcd = o3d.geometry.PointCloud() - # pcd.points = o3d.utility.Vector3dVector(xyz) - # return pcd - - def rgbd_to_projection(self, depth_map, rgb): - self.depth_map = depth_map - self.rgb = rgb - rgb_o3d = o3d.geometry.Image(self.rgb) - depth_o3d = o3d.geometry.Image(self.depth_map) - rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(rgb_o3d, depth_o3d) - if self.pcl is None: - self.pcl = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, self.pinhole_camera_intrinsic) - else: - pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, self.pinhole_camera_intrinsic) - self.pcl.points = pcd.points - self.pcl.colors = pcd.colors - return self.pcl - - - def visualize_pcd(self): - if not self.isstarted: - self.vis.add_geometry(self.pcl) - origin = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.3, origin=[0, 0, 0]) - self.vis.add_geometry(origin) - self.isstarted = True - else: - self.vis.update_geometry(self.pcl) - self.vis.poll_events() - self.vis.update_renderer() - - # def depthmap_to_projection(self, depth_map): - # # depth_map[depth_map == 65535] = 0 - # # self.depth_map = 65535 // depth_map - # self.depth_map = depth_map - # img = o3d.geometry.Image(self.depth_map) - # - # if not self.isstarted: - # print("inhere") - # self.pcl = o3d.geometry.PointCloud.create_from_depth_image(img, self.pinhole_camera_intrinsic) - # self.vis.add_geometry(self.pcl) - # self.isstarted = True - # else: - # print("not inhere") - # pcd = o3d.geometry.PointCloud.create_from_depth_image(img, self.pinhole_camera_intrinsic, depth_scale = 1000, depth_trunc = 50) - # self.pcl.points = pcd.points - # self.vis.update_geometry(self.pcl) - # self.vis.poll_events() - # self.vis.update_renderer() - - def close_window(self): - self.vis.destroy_window() - - - -def depthmap_to_projection(depth_map, intrinsic, stride = 1, depth_scale = 1, depth_trunc = 96): - # o3d_intrinsic_matrix = o3d.camera.PinholeCameraIntrinsicParameters() - # pinhole_camera_intrinsic - # o3d_intrinsic_matrix.set_intrinsics(depth_map.shape[1], depth_map.shape[0], - # intrinsic[0, 0], intrinsic[1, 1], - # intrinsic[0, 2], intrinsic[1, 2]) - - img = o3d.geometry.Image((depth_map)) - pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic("intrinsic.json") - print(pinhole_camera_intrinsic.intrinsic_matrix) - - # pcd = o3d.geometry.PointCloud.create_from_depth_image(depth_map, pinhole_camera_intrinsic, depth_scale=depth_scale, - # depth_trunc=depth_trunc, stride=stride) - pcd = o3d.geometry.PointCloud.create_from_depth_image(img, pinhole_camera_intrinsic) - - return pcd - -def visualize(pcd): - - # o3d.visualization.draw_geometries([pcd], zoom=0.3412, - # front=[0.4257, -0.2125, -0.8795], - # lookat=[2.6172, 2.0475, 1.532], - # up=[-0.0694, -0.9768, 0.2024]) - # downpcd = pcd.voxel_down_sample(voxel_size=0.05) - o3d.visualization.draw_geometries([pcd]) - -# def depthmap_to_projection(depth_map, M): # can add step size which does subsampling -# c_x = M[2,2] -# c_y = M[2,2] -# f_x = M[0,0] -# f_y = M[1,1] -# point_cloud = [] -# rows, cols = depth_map.shape -# for u in range(rows): -# for v in range(cols): -# if(depth_map[u,v] == 65535 or depth_map[u,v] == 0) # Not sure about zero -# continue -# z = depth_map[u,v] -# x = (u - c_x) * z / f_x -# y = (v - c_y) * z / f_y -# point_cloud.append((x,y,z)) -# return point_cloud - - diff --git a/depthai_helpers/supervisor.py b/depthai_helpers/supervisor.py deleted file mode 100644 index 97388e882..000000000 --- a/depthai_helpers/supervisor.py +++ /dev/null @@ -1,75 +0,0 @@ -import atexit -import importlib.util -import os -import signal -import subprocess -import sys -import time -from pathlib import Path - - -def createNewArgs(args): - def removeArg(name, withValue=True): - if name in sys.argv: - idx = sys.argv.index(name) - if withValue: - del sys.argv[idx + 1] - del sys.argv[idx] - - removeArg("-gt") - removeArg("--guiType") - removeArg("--noSupervisor") - return sys.argv[1:] + ["--noSupervisor", "--guiType", args.guiType] - - -class Supervisor: - child = None - - def __init__(self): - signal.signal(signal.SIGINT, self.cleanup) - signal.signal(signal.SIGTERM, self.cleanup) - atexit.register(self.cleanup) - - def runDemo(self, args): - repo_root = Path(__file__).parent.parent - args.noSupervisor = True - new_args = createNewArgs(args) - env = os.environ.copy() - - if args.guiType == "qt": - new_env = env.copy() - new_env["QT_QUICK_BACKEND"] = "software" - new_env["LD_LIBRARY_PATH"] = str(Path(importlib.util.find_spec("PyQt5").origin).parent / "Qt5/lib") - new_env["DEPTHAI_INSTALL_SIGNAL_HANDLER"] = "0" - try: - cmd = ' '.join([f'"{sys.executable}"', "depthai_demo.py"] + new_args) - self.child = subprocess.Popen(cmd, shell=True, env=new_env, cwd=str(repo_root.resolve())) - self.child.communicate() - if self.child.returncode != 0: - raise subprocess.CalledProcessError(self.child.returncode, cmd) - except subprocess.CalledProcessError as ex: - print("Error while running demo script... {}".format(ex)) - print("Waiting 5s for the device to be discoverable again...") - time.sleep(5) - args.guiType = "cv" - if args.guiType == "cv": - new_env = env.copy() - new_env["DEPTHAI_INSTALL_SIGNAL_HANDLER"] = "0" - new_args = createNewArgs(args) - cmd = ' '.join([f'"{sys.executable}"', "depthai_demo.py"] + new_args) - self.child = subprocess.Popen(cmd, shell=True, env=new_env, cwd=str(repo_root.resolve())) - self.child.communicate() - - def checkQtAvailability(self): - return importlib.util.find_spec("PyQt5") is not None - - def cleanup(self, *args, **kwargs): - if self.child is not None and self.child.poll() is None: - self.child.terminate() - try: - self.child.wait(1) - except subprocess.TimeoutExpired: - pass - - - diff --git a/depthai_helpers/version_check.py b/depthai_helpers/version_check.py deleted file mode 100644 index 932fbfb42..000000000 --- a/depthai_helpers/version_check.py +++ /dev/null @@ -1,68 +0,0 @@ -import sys - -import depthai -from pathlib import Path - - -def getVersionFromRequirements(package_name, req_path): - with req_path.resolve().open() as f: - for line in f.readlines(): - if package_name in line: - #not commented out and has version indicator (==) - if not line.startswith('#') and '==' in line: - try: - split = line.split('==') - name = split[0] - if name != package_name: - continue - version = split[1] - version = version.split(';')[0] - #remove any whitespace - version = version.strip() - except: - version = None - return version - - -def getVersion(module_name): - try: - import importlib - module = importlib.import_module(module_name) - if hasattr(module, '__version__'): - return module.__version__ - if hasattr(module, 'version'): - return module.version - except: - pass - try: - import pkg_resources - return pkg_resources.get_distribution(module_name).version - except: - pass - - try: - from importlib.metadata import version - return version(module_name) - except: - pass - - return None - -def checkRequirementsVersion(): - daiVersionRequired = getVersionFromRequirements('depthai', Path(__file__).parent / Path('../requirements.txt')) - if daiVersionRequired is not None: - if daiVersionRequired != getVersion('depthai'): - print(f"\033[1;5;31mVersion mismatch\033[0m\033[91m between installed depthai lib and the required one by the script.\033[0m \n\ - Required: {daiVersionRequired}\n\ - Installed: {getVersion('depthai')}\n\ - \033[91mRun: python3 install_requirements.py \033[0m") - sys.exit(42) - - daiSdkVersionRequired = getVersionFromRequirements('depthai-sdk', Path(__file__).parent / Path('../requirements.txt')) - if daiSdkVersionRequired is not None: - if daiSdkVersionRequired != getVersion('depthai-sdk'): - print(f"\033[1;5;31mVersion mismatch\033[0m\033[91m between installed depthai-sdk lib and the required one by the script.\033[0m \n\ - Required: {daiSdkVersionRequired}\n\ - Installed: {getVersion('depthai_sdk')}\n\ - \033[91mRun: python3 install_requirements.py \033[0m") - sys.exit(42) diff --git a/resources/depthai_boards b/resources/depthai_boards index e9dfca057..400b12d17 160000 --- a/resources/depthai_boards +++ b/resources/depthai_boards @@ -1 +1 @@ -Subproject commit e9dfca0572a264cc93cb556c926e73a2609cdcae +Subproject commit 400b12d179aaee9ec387b85e847c095d906496f6 From 65df223388601b81eb5c4ac9f9bc049a9c3f9130 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 28 Jul 2023 01:00:31 +0200 Subject: [PATCH 61/68] Path to depthai_calibtraion --- depthai_calibration | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_calibration b/depthai_calibration index 9a376c1d0..1dc436218 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit 9a376c1d01682f41218e27e1c7f03b5b1bb4e2f2 +Subproject commit 1dc43621822e76a4259dc70b78afad2dea5f79d8 From 1a17f19d6e8ce4b32fbb09d952573fa017a92b39 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 28 Jul 2023 01:06:48 +0200 Subject: [PATCH 62/68] Path to submodule --- depthai_calibration | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_calibration b/depthai_calibration index 1dc436218..160bb709a 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit 1dc43621822e76a4259dc70b78afad2dea5f79d8 +Subproject commit 160bb709a1cd291e2734de9a7ec76532c2d1e878 From 8b16e61bf6872c956cafd757463872f2a481bedc Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 28 Jul 2023 01:11:51 +0200 Subject: [PATCH 63/68] Adding output scale factor as arg --- calibrate.py | 1 + 1 file changed, 1 insertion(+) diff --git a/calibrate.py b/calibrate.py index 5c88e87a8..9476cb0d1 100755 --- a/calibrate.py +++ b/calibrate.py @@ -821,6 +821,7 @@ def calibrate(self): print("Starting image processing") stereo_calib = calibUtils.StereoCalibration() stereo_calib.traceLevel = self.args.traceLevel + stereo_calib.output_scale_factor = self.args.outputScaleFactor dest_path = str(Path('resources').absolute()) # self.args.cameraMode = 'perspective' # hardcoded for now try: From 029870f37e19f981d7b23c370b80a5a38a333298 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Fri, 4 Aug 2023 21:41:50 +0200 Subject: [PATCH 64/68] Fixing traceLvl and timesync --- calibrate.py | 49 ++++++++++++++++++++++++++++------------ depthai_calibration | 2 +- resources/depthai_boards | 2 +- 3 files changed, 36 insertions(+), 17 deletions(-) diff --git a/calibrate.py b/calibrate.py index 9476cb0d1..3e559d7be 100755 --- a/calibrate.py +++ b/calibrate.py @@ -23,7 +23,6 @@ import depthai_calibration.calibration_utils as calibUtils font = cv2.FONT_HERSHEY_SIMPLEX -debug = False red = (255, 0, 0) green = (0, 255, 0) @@ -115,8 +114,6 @@ def parse_args(): help="number of chessboard squares in Y direction in charuco boards.") parser.add_argument("-rd", "--rectifiedDisp", default=True, action="store_false", help="Display rectified images with lines drawn for epipolar check") - parser.add_argument("-slr", "--swapLR", default=False, action="store_true", - help="Interchange Left and right camera port.") parser.add_argument("-m", "--mode", default=['capture', 'process'], nargs='*', type=str, required=False, help="Space-separated list of calibration options to run. By default, executes the full 'capture process' pipeline. To execute a single step, enter just that step (ex: 'process').") parser.add_argument("-brd", "--board", default=None, type=str, required=True, @@ -134,7 +131,6 @@ def parse_args(): required=False, help="Set the manual lens position of the camera for calibration") 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.") parser.add_argument("-fac", "--factoryCalibration", default=False, action="store_true", help="Enable writing to Factory Calibration.") parser.add_argument("-osf", "--outputScaleFactor", type=float, default=0.5, @@ -156,9 +152,7 @@ def parse_args(): help="Don't take the board calibration for initialization but start with an empty one") parser.add_argument('-trc', '--traceLevel', type=int, default=0, help="Set to trace the steps in calibration. Number from 1 to 5. If you want to display all, set trace number to 10.") - parser.add_argument('-edms', '--enableDebugMessageSync', default=False, action="store_true", - help="Display all the information in calibration.") - parser.add_argument('-mst', '--minSyncTimestamp', type=float, default=0.03, + parser.add_argument('-mst', '--minSyncTimestamp', type=float, default=0.05, help="Minimum time difference between pictures taken from different cameras. Default: %(default)s ") options = parser.parse_args() @@ -299,7 +293,7 @@ def get_synced(self): # Mark minimum if min_ts_diff is None or (acc_diff < min_ts_diff['ts'] and abs(acc_diff - min_ts_diff['ts']) > 0.03): min_ts_diff = {'ts': acc_diff, 'indicies': indicies.copy()} - if self.enableDebugMessageSync: + if self.traceLevel == 0 or self.traceLevel == 1: print('new minimum:', min_ts_diff, 'min required:', self.min_diff_timestamp) if min_ts_diff['ts'] < self.min_diff_timestamp: @@ -316,7 +310,7 @@ def get_synced(self): # pop out the older messages for i in range(0, min_ts_diff['indicies'][name]+1): self.queues[name].popleft() - if self.enableDebugMessageSync: + if self.traceLevel == 1: print('Returning synced messages with error:', min_ts_diff['ts'], min_ts_diff['indicies']) return synced @@ -331,10 +325,7 @@ class Main: images_captured = 0 def __init__(self): - global debug self.args = parse_args() - debug = self.args.debug - self.enableDebugMessageSync=self.args.enableDebugMessageSync self.traceLevel= self.args.traceLevel self.output_scale_factor = self.args.outputScaleFactor self.aruco_dictionary = cv2.aruco.Dictionary_get( @@ -356,7 +347,7 @@ def __init__(self): # random polygons for count self.total_images = self.args.count * \ len(calibUtils.setPolygonCoordinates(1000, 600)) - if debug: + if self.traceLevel == 1: print("Using Arguments=", self.args) if self.args.datasetPath: Path(self.args.datasetPath).mkdir(parents=True, exist_ok=True) @@ -577,6 +568,24 @@ def show(position, text): cv2.imshow(self.display_name, info_frame) cv2.waitKey(1000) + def show_failed_sync_images(self): + width, height = int( + self.width * self.output_scale_factor), int(self.height * self.output_scale_factor) + info_frame = np.zeros((self.height, self.width, 3), np.uint8) + print(f"py: Capture failed, unable to sync images! Fix the argument minSyncTimestamp or (-mts). Set to: {self.minSyncTimestamp}") + def show(position, text): + cv2.putText(info_frame, text, position, + cv2.FONT_HERSHEY_TRIPLEX, 0.7, (0, 255, 0)) + + show((50, int(height / 2 - 40)), + "Capture failed, unable to sync images!") + show((60, int(height / 2 + 40)), "Fix the argument -mts.") + + # cv2.imshow("left", info_frame) + # cv2.imshow("right", info_frame) + cv2.imshow(self.display_name, info_frame) + cv2.waitKey(0) + def show_failed_orientation(self): width, height = int( self.width * self.output_scale_factor), int(self.height * self.output_scale_factor) @@ -615,9 +624,11 @@ def capture_images_sync(self): prev_time = None curr_time = None self.display_name = "Image Window" - syncCollector = MessageSync(len(self.camera_queue), self.args.minSyncTimestamp) # 3ms tolerance - syncCollector.enableDebugMessageSync = self.args.enableDebugMessageSync + self.minSyncTimestamp = self.args.minSyncTimestamp + syncCollector = MessageSync(len(self.camera_queue), self.minSyncTimestamp) # 3ms tolerance + syncCollector.traceLevel = self.args.traceLevel self.mouseTrigger = False + sync_trys = 0 while not finished: currImageList = {} for key in self.camera_queue.keys(): @@ -769,10 +780,18 @@ def capture_images_sync(self): allPassed = True if capturing: syncedMsgs = syncCollector.get_synced() + if sync_trys > 10: + self.show_failed_sync_images() + finished = True + self.device.close() + print("Images were unable to sync, threshold to high. Device closing with exception.") + raise SystemExit(1) if syncedMsgs == False or syncedMsgs == None: for key in self.camera_queue.keys(): self.camera_queue[key].getAll() + sync_trys += 1 continue + for name, frameMsg in syncedMsgs.items(): print(f"Time stamp of {name} is {frameMsg.getTimestamp()}") if self.coverageImages[name] is None: diff --git a/depthai_calibration b/depthai_calibration index 160bb709a..bf0c18661 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit 160bb709a1cd291e2734de9a7ec76532c2d1e878 +Subproject commit bf0c1866182462d427d56a9c1659632493f52055 diff --git a/resources/depthai_boards b/resources/depthai_boards index 400b12d17..e9dfca057 160000 --- a/resources/depthai_boards +++ b/resources/depthai_boards @@ -1 +1 @@ -Subproject commit 400b12d179aaee9ec387b85e847c095d906496f6 +Subproject commit e9dfca0572a264cc93cb556c926e73a2609cdcae From af1e1648d6c822fae93c965e85ee18366198bc88 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Wed, 16 Aug 2023 23:39:41 +0200 Subject: [PATCH 65/68] Changes in readme file (submodules) --- README.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 2331e9044..6849d8a10 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,16 @@ _Click on the GIF below to see a full example run_ __Documentation is available at [https://docs.luxonis.com/en/latest/pages/tutorials/first_steps/](https://docs.luxonis.com/en/latest/pages/tutorials/first_steps/#first-steps-with-depthai).__ ## Installation - +First you need to clone this repository with +``` +git clone https://github.com/luxonis/depthai.git +git pull +git submodule update --init +``` +In case you have repository already cloned, you can update your submodules with: +``` +git pull --recurse-submodules +``` There are two installation steps that need to be performed to make sure the demo works: - **One-time installation** that will add all necessary packages to your OS. ``` From 971a02aca3661bea7e1ec30fc51e65cd1a3511fa Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Wed, 16 Aug 2023 23:42:12 +0200 Subject: [PATCH 66/68] Adding pull --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 6849d8a10..c559b7224 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,7 @@ __Documentation is available at [https://docs.luxonis.com/en/latest/pages/tutori First you need to clone this repository with ``` git clone https://github.com/luxonis/depthai.git +cd depthai git pull git submodule update --init ``` From 5a537987eb403028cb45d984cea1b729f41bf625 Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 17 Aug 2023 02:09:22 +0200 Subject: [PATCH 67/68] Change to recursive --- README.md | 5 +---- depthai_calibration | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index c559b7224..2ef68ae46 100644 --- a/README.md +++ b/README.md @@ -17,10 +17,7 @@ __Documentation is available at [https://docs.luxonis.com/en/latest/pages/tutori ## Installation First you need to clone this repository with ``` -git clone https://github.com/luxonis/depthai.git -cd depthai -git pull -git submodule update --init +git clone --recursive https://github.com/luxonis/depthai.git ``` In case you have repository already cloned, you can update your submodules with: ``` diff --git a/depthai_calibration b/depthai_calibration index 0958433ec..18603cd20 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit 0958433ec4ca8dc300c613933190570101f5e1eb +Subproject commit 18603cd201e35f0a50ca2797f6c620c05bfa81da From f60fc9cc5d72e52b6ce9aeb74f2f96bdc7e8d7cc Mon Sep 17 00:00:00 2001 From: Matic Tonin Date: Thu, 17 Aug 2023 02:11:24 +0200 Subject: [PATCH 68/68] Change submodule --- depthai_calibration | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_calibration b/depthai_calibration index 18603cd20..0958433ec 160000 --- a/depthai_calibration +++ b/depthai_calibration @@ -1 +1 @@ -Subproject commit 18603cd201e35f0a50ca2797f6c620c05bfa81da +Subproject commit 0958433ec4ca8dc300c613933190570101f5e1eb