diff --git a/python/cli/calibrate_frd/__init__.py b/python/cli/calibrate_frd/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/python/cli/calibrate_frd/calibrate_frd.py b/python/cli/calibrate_frd/calibrate_frd.py new file mode 100644 index 0000000..52dddb1 --- /dev/null +++ b/python/cli/calibrate_frd/calibrate_frd.py @@ -0,0 +1,226 @@ +""" +Calibrate device Front-Right-Down orientation. +""" + +import spectacularAI +import cv2 +import numpy as np +import os +import json + + +def define_args(parser): + parser.add_argument("sdk_recording_path", help="Path to the Spectacular AI SDK recording directory.") + parser.add_argument( + "--index", + type=int, + default=0, + help="Camera index that is used for aligning the forward diretion. Default is 0." + ) + parser.add_argument( + "--skip_outputs", + type=int, + default=0, + help="Optional: Number of VIO outputs to skip before displaying an image. Default is 0." + ) + parser.add_argument( + "--zoom", + type=float, + default=1.0, + help="Optional: Zoom factor for the image selection window. E.g., 2.0 for 2x zoom. Default is 1.0." + ) + parser.add_argument( + '--no_confirm', + action='store_true', + help='Select double clicked target without confirmation') + parser.add_argument( + '--no_gravity', + action='store_true', + help='Do not use gravity to compute a Front-Right-Down IMU-to-output matrix and only compute IMU forward vector') + return parser + + +def define_subparser(subparsers): + sub = subparsers.add_parser('calibrate-frd', help=__doc__.strip()) + sub.set_defaults(func=calibrate_frd) + return define_args(sub) + + +def draw_rectigle(img, pixel, color): + x, y = pixel + CROSSHAIR_SIZE = 15 + CENTER_GAP = 2 + cv2.line(img, + (x, y - CROSSHAIR_SIZE), + (x, y - CENTER_GAP), + color, 1) + cv2.line(img, + (x, y + CENTER_GAP), + (x, y + CROSSHAIR_SIZE), + color, 1) + cv2.line(img, + (x - CROSSHAIR_SIZE, y), + (x - CENTER_GAP, y), + color, 1) + cv2.line(img, + (x + CENTER_GAP, y), + (x + CROSSHAIR_SIZE, y), + color, 1) + + +class RayApp: + def __init__(self, args): + self.args = args + self.vio_output_counter = 0 + self.should_quit = False + self.index = args.index + with open(os.path.join(args.sdk_recording_path, 'calibration.json')) as f: + self.calibration_json = json.load(f) + num_cams = len(self.calibration_json['cameras']) + if (self.index >= num_cams): raise Exception(f"Too large camera index {self.index}, must be between 0 and {num_cams - 1} (inclusive).") + self.replay = spectacularAI.Replay( + args.sdk_recording_path, + ignoreFolderConfiguration=True, + configuration={'useStereo': num_cams != 1, 'useMagnetometer': False, 'parameterSets': ['no-threads']}) + + self.imuToCam = np.array(self.calibration_json['cameras'][args.index]['imuToCamera']) + + self.replay.setExtendedOutputCallback(self.on_vio_output) + self.replay.setPlaybackSpeed(-1) + + def on_vio_output(self, _, frames): + """ + Callback function that gets called for each VIO output from the replay. + """ + if self.should_quit: + self.replay.close() + return + + self.vio_output_counter += 1 + + # Skip outputs if requested + if self.vio_output_counter <= self.args.skip_outputs: + if self.vio_output_counter % 100 == 0: + print(f"Skipped {self.vio_output_counter} outputs...") + return + + frame = frames[self.index] + if frame is None: + print("No frame") + return + + image = frame.image.toArray() + + # --- Get Pixel from User Click (with zoom) --- + zoom_factor = self.args.zoom + if zoom_factor < 0.1: + print("Warning: Zoom factor is very small, clamping to 0.1.") + zoom_factor = 0.1 + + if zoom_factor != 1.0: + display_image = cv2.resize(image, None, fx=zoom_factor, fy=zoom_factor, interpolation=cv2.INTER_LINEAR) + else: + display_image = image + + display_image = cv2.cvtColor(display_image, cv2.COLOR_GRAY2BGR) + + window_name = "Double-click target. Press SPACE to confirm selection. Use W, A, S, D keys to fine tune target." + cv2.namedWindow(window_name) + clicked_point = None + hover_point = None + def mouse_callback(event, x, y, *args, **kwargs): + nonlocal clicked_point, hover_point + if event == cv2.EVENT_LBUTTONDBLCLK: + clicked_point = (x, y) + elif event == cv2.EVENT_MOUSEMOVE: + hover_point = (x, y) + cv2.setMouseCallback(window_name, mouse_callback) + + print("Please double-click on a point in the image to select it. Press SPACE to confirm selection. Use W, A, S, D keys to fine tune target.") + self.original_point = None + selected_point = None + while True: + temp_img = display_image.copy() + if hover_point: draw_rectigle(temp_img, hover_point, (0, 20, 225)) + if selected_point: draw_rectigle(temp_img, selected_point, (20, 225, 0)) + + # 4. Update the image to be displayed + cv2.imshow(window_name, temp_img) + + key = cv2.waitKey(1) & 0xFF + if key == 27 or key == ord("q"): + self.should_quit = True + return + elif key == ord("w") and selected_point: clicked_point = (selected_point[0], selected_point[1] - 1) + elif key == ord("a") and selected_point: clicked_point = (selected_point[0] - 1, selected_point[1]) + elif key == ord("s") and selected_point: clicked_point = (selected_point[0], selected_point[1] + 1) + elif key == ord("d") and selected_point: clicked_point = (selected_point[0] + 1, selected_point[1]) + elif key != 0xFF: + if not self.args.no_confirm and self.original_point is not None: + break + if clicked_point is not None: + x, y = clicked_point + selected_point = (x, y) + self.original_point = (x / zoom_factor, y / zoom_factor) + clicked_point = None + + self.should_quit = True # Mark as done to prevent re-triggering + + if self.original_point is None: + print("No point was selected. Exiting.") + return + + main_camera = frame.cameraPose.camera + ray = main_camera.pixelToRay(spectacularAI.PixelCoordinates(*self.original_point)) + if ray is None: + print("pixelToRay failed (outside valid FoV?)") + return + + camToImu = self.imuToCam[:3,:3].transpose() + self.rayImu = camToImu @ [ray.x, ray.y, ray.z] + + if not self.args.no_gravity: + camToWorld = frame.cameraPose.getCameraToWorldMatrix() + imuToWorld = camToWorld[:3, :3] @ self.imuToCam[:3,:3] + worldToImu = imuToWorld[:3, :3].transpose() + downVectorWorld = [0,0,-1] + downVectorImu = worldToImu @ downVectorWorld + rightVectorImu = np.cross(downVectorImu, self.rayImu) + forwardVectorImu = np.cross(rightVectorImu, downVectorImu) + + self.frd = np.eye(4) + self.frd[:3,:3] = np.hstack([v[:, np.newaxis] / np.linalg.norm(v) for v in [forwardVectorImu, rightVectorImu, downVectorImu]]).transpose() + self.rayImu = self.frd[:3, 0] + + self.displayResults() + + def displayResults(self): + print("\n" + "="*45) + print("Camera ray in World coordinates") + print("="*45) + print(f"Original pixel coordinates: {self.original_point}") + print(f"Ray direction (IMU): {np.array2string(self.rayImu, precision=4)}") + print("="*45) + print('Updated calibration.json:\n') + self.calibration_json['imuForward'] = self.rayImu.tolist() + if not self.args.no_gravity: + self.calibration_json['imuToOutput'] = self.frd.tolist() + print(json.dumps(self.calibration_json, indent=2)) + + def run(self): + self.replay.runReplay() + + +def calibrate_frd(args): + app = RayApp(args) + app.run() + + +if __name__ == '__main__': + def parse_args(): + import argparse + parser = argparse.ArgumentParser(description=__doc__.strip()) + parser = define_args(parser) + return parser.parse_args() + + calibrate_frd(parse_args()) diff --git a/python/cli/sai_cli.py b/python/cli/sai_cli.py index 3c709cd..436288f 100644 --- a/python/cli/sai_cli.py +++ b/python/cli/sai_cli.py @@ -6,6 +6,7 @@ from .smooth import define_subparser as smooth_define_subparser from .calibrate.calibrate import define_subparser as calibrate_define_subparser from .diagnose.diagnose import define_subparser as diagnose_define_subparser +from .calibrate_frd.calibrate_frd import define_subparser as frd_define_subparser def parse_args(): def get_sdk_version(): @@ -29,6 +30,7 @@ def get_sdk_version(): calibrate_define_subparser(subparsers) convert_define_subparser(subparsers) diagnose_define_subparser(subparsers) + frd_define_subparser(subparsers) return parser.parse_args() def main():