diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ab21b565..d2655233 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -69,7 +69,7 @@ jobs: uses: coverallsapp/github-action@v2 with: github-token: ${{ secrets.GITHUB_TOKEN }} - fail-on-error: true + fail-on-error: false flag-name: unittests debug: true files: $(find ../../ -name "coverage.xml" -type f) diff --git a/spot_wrapper/calibration/README.md b/spot_wrapper/calibration/README.md index 418a28e1..54e103ba 100644 --- a/spot_wrapper/calibration/README.md +++ b/spot_wrapper/calibration/README.md @@ -1,216 +1,246 @@ # Automatic Robotic Stereo Camera Calibration with Charuco Target -# Table of Contents +## Table of Contents -1. [***Overview***](#overview) -2. [***Quick Start***](#quick-start) -3. [***Check if you have a Legacy Charuco Board***](#check-if-you-have-a-legacy-charuco-board) -4. [***All Options***](#all-options) +1. [Overview](#overview) +2. [Setup](#setup) +3. [Quick Start](#quick-start) + - [Full Calibration (collect data + calibrate + optionally push to robot)](#full-calibration) + - [Calibrate from an Existing Dataset](#calibrate-from-an-existing-dataset) + - [Push an Existing Calibration YAML to the Robot](#push-an-existing-calibration-yaml-to-the-robot) +4. [Output Format](#output-format) +5. [Check if You Have a Legacy Charuco Board](#check-if-you-have-a-legacy-charuco-board) +6. [All Options](#all-options) -# Overview +--- -![side by side comparison](registration_qualitative_example.jpg) +## Overview -This utility streamlines automatic -camera calibration to **solve for the intrinsic and extrinsic parameters for two or more -cameras mounted in a fixed pose relative to each other on a robot** -based off of moving the robot to view a Charuco target from different poses. You can also calculate the calibration from an existing dataset of photos or make a new dataset from a pre-defined list of poses (if available). Additionally, the new calibrations can be directly written to the robot. +![side by side comparison](docs/registration_qualitative_example.jpg) -The CLI tool's saving capability allows to store multiple unique calibration runs in one configuration file -with calibration metadata, to document related runs with different setups or parameters. +This utility automates stereo camera calibration for cameras mounted in a fixed configuration on a robot (e.g. the RGB and depth cameras in Spot's hand). It: -For more info, see ```calibration_util.py```, where the functions -```get_multiple_perspective_camera_calibration_dataset``` and ```multistereo_calibration_charuco``` -do most of the heavy lifting. +- Moves the robot arm through a grid of viewpoints relative to a Charuco calibration target +- Captures synchronized RGB and depth image pairs at each pose +- Runs stereo intrinsic + extrinsic calibration (OpenCV Charuco pipeline) +- Runs hand-eye calibration (OpenCV DANIILIDIS method) to produce `wr1_tform_sensor` transforms for both cameras +- Optionally saves the dataset, the calibration YAML, and/or pushes the result directly to the robot's internal calibration storage via the Spot SDK -> [!NOTE] -> For a more in-depth explanation of this calibration tool, see ![this document](in_depth_calibration_doc.md). +For implementation details see `calibration_util.py` (`get_multiple_perspective_camera_calibration_dataset`, `multistereo_calibration_charuco`) and `spot_in_hand_camera_calibration.py` and `calibration_util.py`. + +--- + +## Setup + +**Physical setup:** -# Quick Start +![spot eye in hand cal](docs/spot_eye_in_hand_setup.jpg) -## Full Calibration +- Place the Charuco board roughly 0.5–0.6 m in front of the robot's hand, facing the hand camera +- The robot must be **sitting (not docked)** and no one else may hold the robot lease before running -To run through the full calibration script to calibrate your Spot's hand camera (depth to rgb, with rgb at default resolution), first set up your robot and charuco board like below +**Board defaults** (matching the standard Spot calibration board): -![spot eye in hand cal](spot_eye_in_hand_setup.jpg) +| Parameter | Default | +|---|---| +| Width (checkers) | 9 | +| Height (checkers) | 4 | +| ArUco dictionary | `DICT_4X4_50` | +| Checker size | 0.115 m | +| Marker size | 0.09 m | + +If your board differs, override these via CLI flags (see [All Options](#all-options)). + +> [!NOTE] +> If you are using a Spot default calibration board with OpenCV ≥ 4.7, add `--legacy_charuco_pattern`. See [Check if You Have a Legacy Charuco Board](#check-if-you-have-a-legacy-charuco-board). -You can then run the following command: +--- + +## Quick Start + +### Full Calibration + +Collect a new dataset, calibrate, and optionally push the result to the robot: > [!WARNING] -> The robot will move. Be sure the robot is sitting (not docked) in front of the charuco board and that no one else is holding the lease before beginning (you will have to release control from the tablet). +> The robot will move. Ensure it is standing in front of the board, nothing is within ~1 m of the arm, and no other client holds the lease. ```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw +# Collect data, calibrate, save result YAML +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --ip -u -pw \ + --data_path --save_data \ + --result_path ``` -To save the collected images, add the ```--save_data``` and ```--data_path``` flags: +To also write the calibration directly to the robot's internal storage, add the `--save_to_robot` flag: ```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw \ ---data_path --save_data +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --ip -u -pw \ + --data_path --save_data \ + --result_path --save_to_robot ``` -To also save the calculated calibrations, add the ```--result_path``` flag: +--- -```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw \ ---data_path --save_data --result_path -``` +### Calibrate from an Existing Dataset -To overwrite the robot's internal calibration with your newly calculated one, also add the ```--save_to_robot``` flag: +If you have already collected a dataset, you can skip robot movement entirely with `--from_data`. Robot credentials (IP, username, password) are only needed here if you also pass `--save_to_robot`: ```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw \ ---data_path --save_data --result_path --save_to_robot +# Calibrate only — no robot needed +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --from_data --data_path \ + --result_path --legacy_charuco_pattern + +# Calibrate and push to robot +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --ip -u -pw \ + --from_data --data_path \ + --result_path --legacy_charuco_pattern --save_to_robot ``` -> [!NOTE] -> If you are using a legacy charuco board, add ```--legacy_charuco_pattern True```. If you're not sure, go to [Check if you have a Legacy Charuco Board](#check-if-you-have-a-legacy-charuco-board). +**Expected dataset folder structure** (produced by `--save_data`): -Other options are listed in [All Options](#all-options) below. +``` +/ + 0/ # RGB images: 1.png, 2.png, ... + 1/ # Depth images: 1.png, 2.png, ... + poses/ # Robot hand poses: 1.npy, 2.npy, ... (4×4 body_T_hand transforms) +``` -## Calibration with Existing Dataset +--- -If you already have a folder of images to calibrate on, then add the ```--from_data``` flag and remove the ```--save_data``` flag. This will *not* move the robot: +### Push an Existing Calibration YAML to the Robot + +If you have a previously computed calibration YAML and only want to upload it to the robot: ```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw \ ---data_path --from_data +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --ip -u -pw \ + --from_data --from_yaml --result_path --save_to_robot ``` -You can still optionally add the ```--result_path ``` and/or ```--save_to_robot``` flags to save the calculated calibration to a yaml file and/or overwrite the robot's internally saved calibrations, respectively. +> [!NOTE] If you are using a legacy charuco board (Only relevant for OpenCV ≥ 4.7) you should also be adding the `--legacy_charuco_pattern`. More information and how to check is below in [Check if You Have a Legacy Charuco Board](#check-if-you-have-a-legacy-charuco-board). + +--- + +## Output Format + +Calibration results are saved as a YAML file with one or more named **tags** (default tag: `default`). This allows multiple calibration runs to coexist in a single file. Password and username are never written to the file. + +```yaml +default: + intrinsic: + 0: # RGB camera (origin / parent) + camera_matrix: [fx, 0, cx, 0, fy, cy, 0, 0, 1] + dist_coeffs: [k1, k2, p1, p2, k3] + image_dim: [width, height] + 1: # Depth camera (reference / child) + camera_matrix: [...] + dist_coeffs: [...] + image_dim: [...] + extrinsic: + 0: # Keyed by origin camera index + 1: # Stereo extrinsic: depth expressed in RGB frame + R: [...] # 3×3 rotation, row-major + T: [...] # 3-element translation (metres) + planning_frame: # Hand-eye result: hand frame expressed in RGB frame + R: [...] + T: [...] + run_params: # All CLI flags used (for reproducibility) + num_images: 135 + timestamp: '2026-03-04 11:19:12' + ... + camera_t_body: [...] # 4×4 homogeneous transform, row-major (16 values) + # = RGB camera frame expressed in body/hand frame +``` -## Overwrite Robot Calibration with Existing Yaml +The `camera_t_body` (or `camera_t_hand`) 4×4 matrix at the top level is the hand-eye calibration result in homogeneous form, provided as a convenience for downstream consumers. -If you have previously run the calibration script and saved the results, you can send those directly to the robot to be saved on its internal hardware without needing to run the entire calibration script by including the ```--from_yaml``` and ```--data_path``` flags and ensuring the `````` goes to the correct calibration yaml file: +--- -```bash -python3 calibrate_spot_hand_camera_cli.py --ip -u -pw \ ---from_yaml --data_path --save_to_robot -``` +## Check if You Have a Legacy Charuco Board -## All Options - -Below are all the various flags available to you to further fine-tune your calibration. You can also see them all in your terminal by running: +Only relevant for OpenCV ≥ 4.7 — You can check your OpenCV version with: ```bash -python3 calibrate_spot_hand_camera_cli.py -h +python3 -c "import cv2; print(cv2.__version__)" ``` +Run the calibration script with `--show_board_pattern` to display the virtual board and compare it visually to your physical board: + ```bash -options: - -h, --help show this help message and exit - --stereo_pairs STEREO_PAIRS, -sp STEREO_PAIRS - Capture images returns a list of images. Stereo pairs correspond towhat - index in the list of images' corresponding camerato calibrate to what - camera. Say capture_images returns [rgb_img, depth_img]and you want to - register depth to rgb, then the desired stereo pairis "[(1,0)]". If you - want to register more than one pair, you can do it like "[(1, 0), - (2,0)]."Make sure to put the stereo pairs in quotes so bash doesn't - complain - --legacy_charuco_pattern LEGACY_CHARUCO_PATTERN, -l LEGACY_CHARUCO_PATTERN - Whether to use the legacy charuco pattern. For Spot Default board, this - should be True.If you aren't sure if your board is legacy, try supplying - the --check_board_patternarg to verify that the cv2 board matches your - board. - --check_board_pattern - Whether to visually verify the board pattern (to check legacy and internal - corner ordering. - --allow_default_internal_corner_ordering - Whether to allow default internal corner ordering. For new versions of - OpenCV, it is recommended to NOT set this parameter to make sure that - corners are ordered in a known pattern. Try supplying the - --check_board_pattern flag to check whether you should enable this flag - When checking, Z-Axis should point out of board. - --photo_utilization_ratio PHOTO_UTILIZATION_RATIO, -pur PHOTO_UTILIZATION_RATIO - Photos that are collected/loaded vs. used for calibration are in a 1 - tophoto utilization ratio. For getting a rough guess on cheaper - hardwarewithout losing collection fidelity. For example, set to 2 to only - use half the photos. - --num_checkers_width NUM_CHECKERS_WIDTH, -ncw NUM_CHECKERS_WIDTH - How many checkers wide is your board - --num_checkers_height NUM_CHECKERS_HEIGHT, -nch NUM_CHECKERS_HEIGHT - How many checkers tall is your board - --dict_size {DICT_4X4_50,DICT_4X4_100,DICT_4X4_250,DICT_4X4_1000,DICT_5X5_50,DICT_5X5_100,DICT_5X5_250,DICT_5X5_1000,DICT_6X6_50,DICT_6X6_100,DICT_6X6_250,DICT_6X6_1000,DICT_7X7_50,DICT_7X7_100,DICT_7X7_250,DICT_7X7_1000,DICT_ARUCO_ORIGINAL} - Choose the ArUco dictionary size. - --checker_dim CHECKER_DIM, -cd CHECKER_DIM - Checker size in meters - --marker_dim MARKER_DIM, -md MARKER_DIM - Aruco Marker size in meters - --data_path DATA_PATH, -dp DATA_PATH - The path in which to save images - --result_path RESULT_PATH, -rp RESULT_PATH - Where to store calibration result as yaml file - --tag TAG, -t TAG What heading to put for the calibration in the config file.If this is your - first time running, the tag should be set to default for the sake of - interoperability with other functionality.If this is a shared config file - with other people, perhaps puta unique identifier, or default, if you'd - like to overridefor everyone. - --unsafe_tag_save If set, skips safety checks for tagging calibration. - --dist_from_board_viewpoint_range DIST_FROM_BOARD_VIEWPOINT_RANGE [DIST_FROM_BOARD_VIEWPOINT_RANGE ...], -dfbvr DIST_FROM_BOARD_VIEWPOINT_RANGE [DIST_FROM_BOARD_VIEWPOINT_RANGE ...] - What distances to conduct calibrations at relative to the board. (along the - normal vector) Three value array arg defines the [Start, Stop), step. for - the viewpoint sweep. These are used to sample viewpoints for automatic - collection. - --degrees, -d Use degrees for rotation ranges (default) - --radians, -r Use radians for rotation ranges - --x_axis_rot_viewpoint_range X_AXIS_ROT_VIEWPOINT_RANGE [X_AXIS_ROT_VIEWPOINT_RANGE ...], -xarvr X_AXIS_ROT_VIEWPOINT_RANGE [X_AXIS_ROT_VIEWPOINT_RANGE ...] - What range of viewpoints around x-axis to sample relative to boards normal - vector. Three value array arg defines the [Start, Stop), step. for the - viewpoint sweep These are used to sample viewpoints for automatic - collection. Assuming that the camera pose is in opencv/ROS format. - --y_axis_rot_viewpoint_range Y_AXIS_ROT_VIEWPOINT_RANGE [Y_AXIS_ROT_VIEWPOINT_RANGE ...], -yarvr Y_AXIS_ROT_VIEWPOINT_RANGE [Y_AXIS_ROT_VIEWPOINT_RANGE ...] - What range of viewpoints around y-axis to sample relative to boards normal - vector. Three value array arg defines the [Start, Stop), step. for the - viewpoint sweep These are used to sample viewpoints for automatic - collection. Assuming that the camera pose is in opencv/ROS format. - --z_axis_rot_viewpoint_range Z_AXIS_ROT_VIEWPOINT_RANGE [Z_AXIS_ROT_VIEWPOINT_RANGE ...], -zarvr Z_AXIS_ROT_VIEWPOINT_RANGE [Z_AXIS_ROT_VIEWPOINT_RANGE ...] - What range of viewpoints around z-axis to sample relative to boards normal - vector. Three value array arg defines the [Start, Stop), step. for the - viewpoint sweep These are used to sample viewpoints for automatic - collection. Assuming that the camera pose is in opencv/ROS format. - --max_num_images MAX_NUM_IMAGES - The maximum number of images - --settle_time SETTLE_TIME, -st SETTLE_TIME - How long to wait after movement to take a picture; don't want motion blur - --save_data, -sd whether to save the images to file - --from_data, -fd Whether to only calibrate from recorded dataset on file. - --save_to_robot, -send - Whether to save the calibration to the robot. - --from_yaml, -yaml Whether the data is from a yaml file. Use this and the '--from_data' and ' - --send' args to send a previously saved calibration yaml to the robot - --ip IP, -i IP, -ip IP - The IP address of the Robot to calibrate - --user USERNAME, -u USERNAME, --username USERNAME - Robot Username - --pass PASSWORD, -pw PASSWORD, --password PASSWORD - Robot Password - --spot_rgb_photo_width SPOT_RGB_PHOTO_WIDTH, -dpw SPOT_RGB_PHOTO_WIDTH - What resolution use for Spot's RGB Hand Camera (width). Currently, only 640 - and 1920 are supported - --spot_rgb_photo_height SPOT_RGB_PHOTO_HEIGHT, -dph SPOT_RGB_PHOTO_HEIGHT - What resolution use for Spot's RGB Hand Camera (width). Currently, only 480 - and 1080 are supported +python3 spot_wrapper/calibration/calibrate_spot_hand.py \ + --show_board_pattern --legacy_charuco_pattern ``` -# Check if you have a Legacy Charuco Board +Press any key to dismiss the window. The rendered board should match your physical board exactly: +- ArUco tags must match between virtual and physical boards +- The displayed axis should have **Y (green) pointing up**, **X (red) pointing right**, and **Z pointing out of the board** +- If the match looks correct with `--legacy_charuco_pattern`, your board is legacy; if it only matches without it, do not pass the flag + +The Spot default calibration board has an ArUco marker in the **top-left corner** — this is a legacy board. -You only need to do this if using an opencv version after ```4.7```( -check with```python3 -c "import cv2; print(cv2.__version__)"```) +--- -Through using the CLI tool (```python3 calibrate_multistereo_cameras_with_charuco_cli.py -h```), you can check if you have a legacy board through visually comparing the generated drawn virtual board to your physical charuco board target. Some legacy boards have an aruco tag in the top -left corner, whether as some non-legacy boards have a checker in the top left corner. -Also, check to see that the aruco tags match between virtual and physical boards. -It is important that the virtual board matches the physical board, otherwise this calibration -will not work. +## All Options + +Print all calibration script options: ``` -python3 calibrate_multistereo_cameras_with_charuco_cli.py --check_board_pattern --legacy_charuco_pattern t +python3 spot_wrapper/calibration/calibrate_spot_hand.py -h ``` -There should be an axis at the center of the board, where the Y axis (green) -points upwards, the X axis (red) points to the right, and the figure should be labelled -as Z-axis out of board. If it isn't then try without legacy (```--legacy_charuco_pattern f```). - -If you are using the default Spot Calibation board, and there is an aruco marker -in the top left corner, then it legacy (so supply true argument to legacy.) +### Calibration target + +| Flag | Short | Default | Description | +|---|---|---|---| +| `--legacy_charuco_pattern` | `-l` | off | Use legacy Charuco pattern (required for Spot default board on OpenCV ≥ 4.7) | +| `--show_board_pattern` | | off | Display the virtual board for visual verification before running | +| `--allow_default_internal_corner_ordering` | | off | Skip bottom-left-to-top-right corner ID enforcement (not recommended) | +| `--num_checkers_width` | `-ncw` | `9` | Number of checkers along the board width | +| `--num_checkers_height` | `-nch` | `4` | Number of checkers along the board height | +| `--dict_size` | | `DICT_4X4_50` | ArUco dictionary (see choices in `calibration_clis.py`) | +| `--checker_dim_meters` | `-cd` | `0.115` | Physical checker square size in metres | +| `--marker_dim_meters` | `-md` | `0.09` | Physical ArUco marker size in metres | + +### Data collection / viewpoint sweep + +| Flag | Short | Default | Description | +|---|---|---|---| +| `--dist_from_board_viewpoint_range` | `-dfbvr` | `0.5 0.6 0.1` | `[start, stop, step]` distances along board normal (metres) | +| `--dist_along_board_width` | `-dabw` | `-0.2 0.3 0.1` | `[start, stop, step]` lateral offsets along board width (metres) | +| `--x_axis_rot_viewpoint_range` | `-xarvr` | `-10 11 10` | `[start, stop, step]` rotations around X axis | +| `--y_axis_rot_viewpoint_range` | `-yarvr` | `-10 11 10` | `[start, stop, step]` rotations around Y axis | +| `--z_axis_rot_viewpoint_range` | `-zarvr` | `-10 11 10` | `[start, stop, step]` rotations around Z axis | +| `--degrees` | `-d` | on | Interpret rotation ranges in degrees (default) | +| `--radians` | `-r` | off | Interpret rotation ranges in radians | +| `--max_num_images` | | `10000` | Cap on total viewpoints visited | +| `--settle_time` | `-st` | `1.0` | Seconds to wait after each move before capturing | +| `--stereo_pairs` | `-sp` | `0,1` | Camera index pairs to calibrate (e.g. `0,1`) | +| `--photo_utilization_ratio` | `-pur` | `1` | Use every Nth collected image for calibration (e.g. `2` = use half) | + +### I/O and workflow + +| Flag | Short | Default | Description | +|---|---|---|---| +| `--data_path` | `-dp` | `None` | Directory for dataset images/poses, or YAML path when `--from_yaml` | +| `--save_data` | `-sd` | off | Save captured images and poses to `--data_path` | +| `--from_data` | `-fd` | off | Skip data collection; calibrate from existing dataset at `--data_path` | +| `--from_yaml` | `-yaml` | off | Used with `--result_path` to push a saved YAML to the robot | +| `--result_path` | `-rp` | `calibration_result.yaml` | Where to write the calibration YAML | +| `--tag` | `-t` | `default` | Tag name for this run within the YAML file | +| `--unsafe_tag_save` | | off | Skip overwrite confirmation prompts | + +### Robot connection (required when connecting to the robot) +and `--save_to_robot` +| Flag | Short | Description | +|---|---|---| +| `--ip` | `-i` / `-ip` | Robot IP address | +| `--user` | `-u` | Robot username | +| `--pass` | `-pw` | Robot password | +| `--spot_rgb_photo_width` | `-dpw` | RGB image width: `640` (default) or `1920` | +| `--spot_rgb_photo_height` | `-dph` | RGB image height: `480` (default) or `1080` | +| `--save_to_robot` | `-save` | Push the computed calibration to the robot's internal storage via Spot SDK | diff --git a/spot_wrapper/calibration/__init__.py b/spot_wrapper/calibration/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py b/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py deleted file mode 100644 index c070fe4d..00000000 --- a/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py +++ /dev/null @@ -1,332 +0,0 @@ -# Copyright (c) 2025 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. - -# Copyreference (c) 2024 Boston Dynamics AI Institute LLC. All references reserved. - -import argparse -import ast -import logging -from typing import List, Tuple, Union - -import cv2 -import numpy as np - -from spot_wrapper.calibration.calibration_util import ( - charuco_pose_sanity_check, - create_charuco_board, - create_ideal_charuco_image, - detect_charuco_corners, - load_dataset_from_path, - multistereo_calibration_charuco, - save_calibration_parameters, -) - -logging.basicConfig( - level=logging.INFO, -) - -logger = logging.getLogger(__name__) -# - - -def setup_calibration_param( - parser: argparse.ArgumentParser, -) -> Tuple[argparse.Namespace, cv2.aruco_Dictionary, cv2.aruco_CharucoBoard]: - args = parser.parse_args() - if hasattr(cv2.aruco, args.dict_size): - aruco_dict = cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, args.dict_size)) - else: - raise ValueError(f"Invalid ArUco dictionary: {args.dict_size}") - charuco = create_charuco_board( - num_checkers_width=args.num_checkers_width, - num_checkers_height=args.num_checkers_height, - checker_dim=args.checker_dim, - marker_dim=args.marker_dim, - aruco_dict=aruco_dict, - legacy=args.legacy_charuco_pattern, - ) - - if not args.allow_default_internal_corner_ordering: - logger.warning("Enforcing bottom up charuco ordering. Pre-computing correlation now...") - detect_charuco_corners( - create_ideal_charuco_image(charuco_board=charuco), - charuco_board=charuco, - aruco_dict=aruco_dict, - enforce_ascending_ids_from_bottom_left_corner=True, - ) - if args.check_board_pattern: - logger.warning("Checking board, you'll need to close a window in a sec (press any key)") - charuco_pose_sanity_check( - create_ideal_charuco_image(charuco_board=charuco, colorful=True), - charuco_board=charuco, - aruco_dict=aruco_dict, - ) - return args, aruco_dict, charuco - - -def calibration_helper( - images: Union[List[np.ndarray], np.ndarray], - args: argparse.Namespace, - charuco: cv2.aruco_CharucoBoard, - aruco_dict: cv2.aruco_Dictionary, - poses: np.ndarray, - result_path: str = None, -) -> dict: - logger.warning( - f"Calibrating from {len(images)} images.. for every " - f"{args.photo_utilization_ratio} recorded photos 1 is used to calibrate" - ) - if not args.allow_default_internal_corner_ordering: - logger.warning("Turning off corner swap (needed for localization) for calibration solution...") - logger.warning("Corner swap needed for initial localization, but breaks calibration.") - logger.warning("See https://github.com/opencv/opencv/issues/26126") - detect_charuco_corners( - create_ideal_charuco_image(charuco_board=charuco), - charuco_board=charuco, - aruco_dict=aruco_dict, - enforce_ascending_ids_from_bottom_left_corner=False, - ) - calibration = multistereo_calibration_charuco( - images[:: args.photo_utilization_ratio], - desired_stereo_pairs=args.stereo_pairs, - charuco_board=charuco, - aruco_dict=aruco_dict, - poses=poses, - ) - logger.info(f"Finished script, obtained {calibration}") - logger.info("Saving calibration param...") - - if result_path is None: - result_path = input("Please provide a path to save the calibration results (or type 'No' to skip): ") - - args.result_path = result_path - - # Save the calibration parameters if a valid result path is provided - calibration_dict = save_calibration_parameters( - data=calibration, - output_path=args.result_path, - num_images=len(images[:: args.photo_utilization_ratio]), - tag=args.tag, - parser_args=args, - unsafe=args.unsafe_tag_save, - ) - return calibration_dict - - -def main(): - parser = calibrator_cli() - args, aruco_dict, charuco = setup_calibration_param(parser) - logger.info(f"Loading images from {args.data_path}") - - if args.data_path is not None: - images, poses = load_dataset_from_path(args.data_path) - calibration_helper( - images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses, result_path=args.result_path - ) - else: - logger.warning("Could not load any images to calibrate from, supply --data_path") - - -def calibrator_cli() -> argparse.ArgumentParser: - parser = argparse.ArgumentParser("Calibrate Eye-In Hand Camera System :)") - # Calculations - parser.add_argument( - "--stereo_pairs", - "-sp", - type=parse_tuple_list, - required=False, - default=[(1, 0)], - help=( - "Capture images returns a list of images. Stereo pairs correspond to" - "what index in the list of images' corresponding camera" - "to calibrate to what camera. Say capture_images returns [rgb_img, depth_img]" - "and you want to register depth to rgb, then the desired stereo pair" - 'is "[(1,0)]". If you want to register more than one pair, you can do it like "[(1, 0), (2,0)]."' - "Make sure to put the stereo pairs in quotes so bash doesn't complain" - ), - ) - - parser.add_argument( - "--legacy_charuco_pattern", - "-l", - type=str2bool, - required=True, - help=( - "Whether to use the legacy charuco pattern. For Spot Default board, this should be True." - "If you aren't sure if your board is legacy, try supplying the --check_board_pattern" - "arg to verify that the cv2 board matches your board." - ), - ) - parser.add_argument( - "--check_board_pattern", - action="store_true", - default=False, - help="Whether to visually verify the board pattern (to check legacy and internal corner ordering.", - ) - - parser.add_argument( - "--allow_default_internal_corner_ordering", - action="store_true", - default=False, - help=( - "Whether to allow default internal corner ordering. " - "For new versions of OpenCV, it is recommended " - "to NOT set this parameter to make sure that corners " - "are ordered in a known pattern. " - "Try supplying the --check_board_pattern flag " - "to check whether you should enable this flag " - "When checking, Z-Axis should point out of board. " - ), - ) - - parser.add_argument( - "--photo_utilization_ratio", - "-pur", - dest="photo_utilization_ratio", - type=int, - default=1, - help=( - "Photos that are collected/loaded vs. used for calibration are in a 1 to" - "photo utilization ratio. For getting a rough guess on cheaper hardware" - "without losing collection fidelity. For example, set to 2 to only use half the photos." - ), - ) - - # calibration param - parser.add_argument( - "--num_checkers_width", - "-ncw", - dest="num_checkers_width", - type=int, - help="How many checkers wide is your board", - default=9, - ) - - parser.add_argument( - "--num_checkers_height", - "-nch", - dest="num_checkers_height", - type=int, - help="How many checkers tall is your board", - default=4, - ) - - parser.add_argument( - "--dict_size", - type=str, - choices=[ - "DICT_4X4_50", - "DICT_4X4_100", - "DICT_4X4_250", - "DICT_4X4_1000", - "DICT_5X5_50", - "DICT_5X5_100", - "DICT_5X5_250", - "DICT_5X5_1000", - "DICT_6X6_50", - "DICT_6X6_100", - "DICT_6X6_250", - "DICT_6X6_1000", - "DICT_7X7_50", - "DICT_7X7_100", - "DICT_7X7_250", - "DICT_7X7_1000", - "DICT_ARUCO_ORIGINAL", - ], - default="DICT_4X4_50", - help="Choose the ArUco dictionary size.", - ) - - parser.add_argument( - "--checker_dim", - "-cd", - dest="checker_dim", - type=float, - default=0.115, - help="Checker size in meters", - ) - - parser.add_argument( - "--marker_dim", - "-md", - dest="marker_dim", - type=float, - default=0.09, - help="Aruco Marker size in meters", - ) - - # path and saving - parser.add_argument( - "--data_path", - "-dp", - dest="data_path", - type=str, - help="The path in which to save images", - default=None, - ) - - parser.add_argument( - "--result_path", - "-rp", - dest="result_path", - default="", - type=str, - required=False, - help="Where to store calibration result as yaml file", - ) - - parser.add_argument( - "--tag", - "-t", - dest="tag", - type=str, - required=False, - default="default", - help=( - "What heading to put for the calibration in the config file." - "If this is your first time running, the tag should be set to default " - "for the sake of interoperability with other functionality." - "If this is a shared config file with other people, perhaps put" - "a unique identifier, or default, if you'd like to override" - "for everyone." - ), - ) - - parser.add_argument( - "--unsafe_tag_save", - action="store_true", - help="If set, skips safety checks for tagging calibration.", - ) - - return parser - - -def str2bool(value: str) -> bool: - """ - Convert a string to a boolean. - Accepts 'true', 'false', 'yes', 'no', '1', '0', etc. - """ - if isinstance(value, bool): - return value - if value.lower() in ("yes", "true", "t", "y", "1"): - return True - elif value.lower() in ("no", "false", "f", "n", "0"): - return False - else: - raise argparse.ArgumentTypeError("Boolean value expected.") - - -# collection -def parse_tuple_list(string: str) -> List[Tuple[int, int]]: - try: - # Use ast.literal_eval to safely evaluate the string as a Python expression - value = ast.literal_eval(string) - if isinstance(value, list) and all(isinstance(t, tuple) and len(t) == 2 for t in value): - return value - else: - raise argparse.ArgumentTypeError(f"Expected a list of tuples, but got: {string}") - except (ValueError, SyntaxError): - raise argparse.ArgumentTypeError(f"Invalid tuple list format: {string}") - - -if __name__ == "__main__": - main() diff --git a/spot_wrapper/calibration/calibrate_spot_hand.py b/spot_wrapper/calibration/calibrate_spot_hand.py new file mode 100644 index 00000000..1e3e3be6 --- /dev/null +++ b/spot_wrapper/calibration/calibrate_spot_hand.py @@ -0,0 +1,141 @@ +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +import argparse +import logging +from typing import Tuple + +import cv2 +import numpy as np +import yaml + +from spot_wrapper.calibration.calibration_clis import ( + calibrate_robot_cli, + setup_calibration_param, + spot_cli, +) +from spot_wrapper.calibration.calibration_helpers import AutomaticCameraCalibrationRobot +from spot_wrapper.calibration.calibration_util import ( + calibration_helper, + get_multiple_perspective_camera_calibration_dataset, + load_dataset_from_path, +) +from spot_wrapper.calibration.spot_in_hand_camera_calibration import ( + SpotInHandCalibration, +) + +logging.basicConfig( + level=logging.INFO, +) + +logger = logging.getLogger(__name__) + + +def _require_robot_credentials(args: argparse.Namespace) -> None: + """Raise a clear error when robot credentials are needed but were not supplied.""" + missing = [ + flag for flag, val in (("--ip", args.ip), ("--user", args.username), ("--pass", args.password)) if not val + ] + if missing: + raise ValueError(f"Connecting to the robot requires {', '.join(missing)} to be specified.") + + +def create_robot( + args: argparse.ArgumentParser, charuco: cv2.aruco_CharucoBoard, aruco_dict: cv2.aruco_Dictionary +) -> Tuple[AutomaticCameraCalibrationRobot, argparse.Namespace]: + _require_robot_credentials(args) + # Replace with your AutomaticCameraCalibrationRobot + in_hand_bot = SpotInHandCalibration(args.ip, args.username, args.password) + in_hand_bot._set_localization_param( + charuco_board=charuco, + aruco_dict=aruco_dict, + resolution=( + args.spot_rgb_photo_width, + args.spot_rgb_photo_height, + ), + ) + try: + args.robot_name = in_hand_bot.robot.get_cached_robot_id().nickname + except Exception: + logger.warning("Could not determine cached robot nickname, saving name as unknown") + args.robot_name = "unknown" + return in_hand_bot, args + + +def create_robot_parser() -> argparse.ArgumentParser: + parser = calibrate_robot_cli() + return spot_cli(parser=parser) # Replace with robot specific parsing + + +def calibrate_spot_hand() -> None: + parser = create_robot_parser() + args, aruco_dict, charuco = setup_calibration_param(parser) + + # Collect new data and calibrate + if not args.from_data and not args.from_yaml: + in_hand_bot, args = create_robot(args, charuco=charuco, aruco_dict=aruco_dict) + + logger.warning("This script moves the robot around. !!! USE AT YOUR OWN RISK !!!") + logger.warning("HOLD Ctrl + C NOW TO CANCEL") + logger.warning("The calibration board should be about a meter away with nothing within a meter of the robot.") + logger.warning("The robot should NOT be docked, and nobody should have robot control") + logger.warning(f"the ip is: {args.ip}") + input("Press Enter to continue...") + + images, poses = get_multiple_perspective_camera_calibration_dataset( + auto_cam_cal_robot=in_hand_bot, + max_num_images=args.max_num_images, + distances_z=np.arange(*args.dist_from_board_viewpoint_range), + distances_x=np.arange(*args.dist_along_board_width), + x_axis_rots=np.arange(*args.x_axis_rot_viewpoint_range), + y_axis_rots=np.arange(*args.y_axis_rot_viewpoint_range), + z_axis_rots=np.arange(*args.z_axis_rot_viewpoint_range), + use_degrees=args.use_degrees, + settle_time=args.settle_time, + data_path=args.data_path, + save_data=args.save_data, + ) + + calibration = calibration_helper( + images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses, result_path=args.result_path + ) + if args.save_to_robot: + logger.info("Saving calibration to robot...") + in_hand_bot.write_calibration_to_robot(calibration) + in_hand_bot.shutdown() + + # Load and send previously computed and saved calibration data to the robot + # Assumes the user wants to send the calibration to the robot so the -send flag is not needed/checked + elif args.from_yaml: + try: + in_hand_bot, args = create_robot(args, charuco=charuco, aruco_dict=aruco_dict) + with open(args.result_path, "r") as file: + calibration = yaml.safe_load(file) + send_to_robot = input( + f"Loaded calibration data:\n{calibration}\nDo you want to send this calibration to the robot?" + " (y/n): " + ) + if send_to_robot.strip().lower() == "y": + logger.info("Saving calibration to robot...") + in_hand_bot.write_calibration_to_robot(calibration) + else: + logger.info("Calibration not sent to robot. Shutting down.") + except Exception as e: + raise ValueError(f"Failed to load calibration from {args.data_path}:\n{e}\n") + # Load previously collected data and compute calibration + else: + logger.info(f"Loading images from {args.data_path}") + images, poses = load_dataset_from_path(args.data_path) + calibration = calibration_helper( + images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses, result_path=args.result_path + ) + if args.save_to_robot: + logger.info("Connecting to robot...") + in_hand_bot, args = create_robot(args, charuco=charuco, aruco_dict=aruco_dict) + logger.info("Saving calibration to robot...") + in_hand_bot.write_calibration_to_robot(calibration) + + logger.info("Calibration complete!") + + +if __name__ == "__main__": + calibrate_spot_hand() diff --git a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py b/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py deleted file mode 100644 index 74727063..00000000 --- a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py +++ /dev/null @@ -1,290 +0,0 @@ -# Copyright (c) 2025 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. - -# Copyreference (c) 2024 Boston Dynamics AI Institute LLC. All references reserved. - -import argparse -import logging -from typing import Tuple - -import cv2 -import numpy as np -import yaml - -from spot_wrapper.calibration.automatic_camera_calibration_robot import AutomaticCameraCalibrationRobot -from spot_wrapper.calibration.calibrate_multistereo_cameras_with_charuco_cli import ( - calibration_helper, - calibrator_cli, - setup_calibration_param, -) -from spot_wrapper.calibration.calibration_util import ( - get_multiple_perspective_camera_calibration_dataset, - load_dataset_from_path, -) -from spot_wrapper.calibration.spot_in_hand_camera_calibration import ( - SpotInHandCalibration, -) - -logging.basicConfig( - level=logging.INFO, -) - -logger = logging.getLogger(__name__) - - -def create_robot( - args: argparse.ArgumentParser, charuco: cv2.aruco_CharucoBoard, aruco_dict: cv2.aruco_Dictionary -) -> Tuple[AutomaticCameraCalibrationRobot, argparse.Namespace]: - # Replace with your AutomaticCameraCalibrationRobot - in_hand_bot = SpotInHandCalibration(args.ip, args.username, args.password) - in_hand_bot._set_localization_param( - charuco_board=charuco, - aruco_dict=aruco_dict, - resolution=( - args.spot_rgb_photo_width, - args.spot_rgb_photo_height, - ), - ) - try: - args.robot_name = in_hand_bot.robot.get_cached_robot_id().nickname - except Exception: - logger.warning("Could not determine cached robot nickname, saving name as unknown") - args.robot_name = "unknown" - return in_hand_bot, args - - -def create_robot_parser() -> argparse.ArgumentParser: - parser = calibrate_robot_cli() - return spot_cli(parser=parser) # Replace with robot specific parsing - - -def spot_main() -> None: - parser = create_robot_parser() - args, aruco_dict, charuco = setup_calibration_param(parser) - in_hand_bot, args = create_robot(args, charuco=charuco, aruco_dict=aruco_dict) - - # Collect new data and calibrate - if not args.from_data: - logger.warning("This script moves the robot around. !!! USE AT YOUR OWN RISK !!!") - logger.warning("HOLD Ctrl + C NOW TO CANCEL") - logger.warning("The calibration board should be about a meter away with nothing within a meter of the robot.") - logger.warning("The robot should NOT be docked, and nobody should have robot control") - # sleep(5) - - images, poses = get_multiple_perspective_camera_calibration_dataset( - auto_cam_cal_robot=in_hand_bot, - max_num_images=args.max_num_images, - distances_z=np.arange(*args.dist_from_board_viewpoint_range), - distances_x=np.arange(*args.dist_along_board_width), - x_axis_rots=np.arange(*args.x_axis_rot_viewpoint_range), - y_axis_rots=np.arange(*args.y_axis_rot_viewpoint_range), - z_axis_rots=np.arange(*args.z_axis_rot_viewpoint_range), - use_degrees=args.use_degrees, - settle_time=args.settle_time, - data_path=args.data_path, - save_data=args.save_data, - ) - calibration = calibration_helper( - images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses, result_path=args.result_path - ) - if args.save_to_robot: - logger.info("Saving calibration to robot...") - in_hand_bot.write_calibration_to_robot(calibration) - in_hand_bot.shutdown() - # Send previously computed and saved calibration data to the robot - elif args.from_yaml: - try: - with open(args.data_path, "r") as file: - calibration = yaml.safe_load(file) - logger.info(f"Loaded calibration data:\n{calibration}") - if args.save_to_robot: - logger.info("Saving calibration to robot...") - in_hand_bot.write_calibration_to_robot(calibration) - except Exception as e: - raise ValueError(f"Failed to load calibration from {args.data_path}: {e}\nIs it a calibration yaml file?") - # Load previously collected data and compute calibration - else: - logger.info(f"Loading images from {args.data_path}") - images, poses = load_dataset_from_path(args.data_path) - calibration = calibration_helper( - images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses, result_path=args.result_path - ) - if args.save_to_robot: - logger.info("Saving calibration to robot...") - in_hand_bot.write_calibration_to_robot(calibration) - - logger.info("Calibration complete!") - - -def calibrate_robot_cli(parser: argparse.ArgumentParser = None) -> argparse.ArgumentParser: - if parser is None: - parser = calibrator_cli() - - parser.add_argument( - "--dist_from_board_viewpoint_range", - "-dfbvr", - nargs="+", - type=float, - dest="dist_from_board_viewpoint_range", - default=[0.5, 0.6, 0.1], - help=( - "What distances to conduct calibrations at relative to the board. (along the normal vector) " - "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep. " - "These are used to sample viewpoints for automatic collection. " - ), - ) - parser.add_argument( - "--dist_along_board_width", - "-dabw", - nargs="+", - type=float, - dest="dist_along_board_width", - default=[-0.3, 0.4, 0.2], - help=( - "What distances to conduct calibrations at relative to the board. (along the board width) " - "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep. " - "These are used to sample viewpoints for automatic collection. " - ), - ) - group = parser.add_mutually_exclusive_group() - group.add_argument( - "--degrees", - "-d", - dest="use_degrees", - action="store_true", - default=True, - help="Use degrees for rotation ranges (default)", - ) - group.add_argument( - "--radians", - "-r", - dest="use_degrees", - action="store_false", - help="Use radians for rotation ranges", - ) - defaults = [[-10, 11, 10], [-10, 11, 10], [-10, 11, 10]] - for idx, axis in enumerate(["x", "y", "z"]): - parser.add_argument( - f"--{axis}_axis_rot_viewpoint_range", - f"-{axis}arvr", - nargs="+", - type=float, - default=defaults[idx], - dest=f"{axis}_axis_rot_viewpoint_range", - help=( - f"What range of viewpoints around {axis}-axis to sample relative to boards normal vector. " - "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep " - "These are used to sample viewpoints for automatic collection. " - "Assuming that the camera pose is in opencv/ROS format. " - ), - ) - - parser.add_argument( - "--max_num_images", - dest="max_num_images", - type=int, - default=10000, - help="The maximum number of images", - required=False, - ) - - parser.add_argument( - "--settle_time", - "-st", - dest="settle_time", - type=float, - default=1.0, - help="How long to wait after movement to take a picture; don't want motion blur", - ) - - # path things - parser.add_argument( - "--save_data", - "-sd", - dest="save_data", - action="store_true", - help="whether to save the images to file", - ) - - parser.add_argument( - "--from_data", - "-fd", - dest="from_data", - action="store_true", - help="Whether to only calibrate from recorded dataset on file.", - ) - - parser.add_argument( - "--save_to_robot", - "-send", - dest="save_to_robot", - action="store_true", - help="Whether to save the calibration to the robot.", - ) - - parser.add_argument( - "--from_yaml", - "-yaml", - dest="from_yaml", - action="store_true", - help=( - "Whether the data is from a yaml file. Use this and the '--from_data' and '--send' args to send a" - " previously saved calibration yaml to the robot" - ), - ) - - return parser - - -def spot_cli(parser: argparse.ArgumentParser = None) -> argparse.ArgumentParser: - if parser is None: - parser = calibrate_robot_cli() - - parser.add_argument( - "--ip", - "-i", - "-ip", - dest="ip", - type=str, - help="The IP address of the Robot to calibrate", - required=True, - ) - parser.add_argument( - "--user", - "-u", - "--username", - dest="username", - type=str, - help="Robot Username", - required=True, - ) - parser.add_argument( - "--pass", - "-pw", - "--password", - dest="password", - type=str, - help="Robot Password", - required=True, - ) - - parser.add_argument( - "--spot_rgb_photo_width", - "-dpw", - type=int, - default=640, - dest="spot_rgb_photo_width", - help="What resolution use for Spot's RGB Hand Camera (width). Currently, only 640 and 1920 are supported", - ) - - parser.add_argument( - "--spot_rgb_photo_height", - "-dph", - type=int, - default=480, - help="What resolution use for Spot's RGB Hand Camera (width). Currently, only 480 and 1080 are supported", - ) - return parser - - -if __name__ == "__main__": - spot_main() diff --git a/spot_wrapper/calibration/calibration_clis.py b/spot_wrapper/calibration/calibration_clis.py new file mode 100644 index 00000000..cb8c4980 --- /dev/null +++ b/spot_wrapper/calibration/calibration_clis.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + + +import argparse +import logging +from typing import Tuple + +import cv2 + +from spot_wrapper.calibration.charuco_board_detection import ( + charuco_pose_sanity_check, + create_charuco_board, + create_ideal_charuco_image, + detect_charuco_corners, +) + +logging.basicConfig( + level=logging.INFO, +) + +logger = logging.getLogger(__name__) + + +def setup_calibration_param( + parser: argparse.ArgumentParser, +) -> Tuple[argparse.Namespace, cv2.aruco_Dictionary, cv2.aruco_CharucoBoard]: + """Set up calibration parameters from command line arguments. + + Args: + parser (argparse.ArgumentParser): The argument parser to set up from command line. + + Raises: + ValueError: If the provided ArUco dictionary is invalid. + + Returns: + Tuple[argparse.Namespace, cv2.aruco_Dictionary, cv2.aruco_CharucoBoard]: + The parsed arguments, ArUco dictionary, and Charuco board. + """ + args = parser.parse_args() + if hasattr(cv2.aruco, args.dict_size): + aruco_dict = cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, args.dict_size)) + else: + raise ValueError(f"Invalid ArUco dictionary: {args.dict_size}") + charuco = create_charuco_board( + num_checkers_width=args.num_checkers_width, + num_checkers_height=args.num_checkers_height, + checker_dim=args.checker_dim, + marker_dim=args.marker_dim, + aruco_dict=aruco_dict, + legacy=args.legacy_charuco_pattern, + ) + + if not args.allow_default_internal_corner_ordering: + logger.warning("Enforcing bottom up charuco ordering. Pre-computing correlation now...") + detect_charuco_corners( + create_ideal_charuco_image(charuco_board=charuco), + charuco_board=charuco, + aruco_dict=aruco_dict, + enforce_ascending_ids_from_bottom_left_corner=True, + ) + if args.show_board_pattern: + logger.warning("Checking board, you'll need to close a window in a sec (press any key)") + charuco_pose_sanity_check( + create_ideal_charuco_image(charuco_board=charuco, colorful=True), + charuco_board=charuco, + aruco_dict=aruco_dict, + ) + return args, aruco_dict, charuco + + +def calibrator_cli() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser("CalibrateEyeInHandCameraSystem") + # Calculations + + parser.add_argument( + "--legacy_charuco_pattern", + action="store_true", + help=( + "Whether to use the legacy charuco pattern. For Spot Default board, this should be set." + "If you aren't sure if your board is legacy, try supplying the --show_board_pattern" + "arg to verify that the cv2 board matches your board." + ), + ) + parser.add_argument( + "--show_board_pattern", + action="store_true", + default=False, + help="Whether to visually verify the board pattern (to check legacy and internal corner ordering.", + ) + + parser.add_argument( + "--allow_default_internal_corner_ordering", + action="store_true", + default=False, + help=( + "Whether to allow default internal corner ordering. " + "For new versions of OpenCV, it is recommended " + "to NOT set this parameter to make sure that corners " + "are ordered in a known pattern. " + "Try supplying the --show_board_pattern flag " + "to check whether you should enable this flag " + "When checking, Z-Axis should point out of board. " + ), + ) + + parser.add_argument( + "--photo_utilization_ratio", + "-pur", + dest="photo_utilization_ratio", + type=int, + default=1, + help=( + "Photos that are collected/loaded vs. used for calibration are in a 1 to" + "photo utilization ratio. For getting a rough guess on cheaper hardware" + "without losing collection fidelity. For example, set to 2 to only use half the photos." + ), + ) + + # calibration param + parser.add_argument( + "--num_checkers_width", + "-ncw", + dest="num_checkers_width", + type=int, + help="How many checkers is the widest dimension of your board", + default=9, + ) + + parser.add_argument( + "--num_checkers_height", + "-nch", + dest="num_checkers_height", + type=int, + help="How many checkers is the other dimension of your board", + default=4, + ) + + parser.add_argument( + "--dict_size", + type=str, + choices=[ + "DICT_4X4_50", + "DICT_4X4_100", + "DICT_4X4_250", + "DICT_4X4_1000", + "DICT_5X5_50", + "DICT_5X5_100", + "DICT_5X5_250", + "DICT_5X5_1000", + "DICT_6X6_50", + "DICT_6X6_100", + "DICT_6X6_250", + "DICT_6X6_1000", + "DICT_7X7_50", + "DICT_7X7_100", + "DICT_7X7_250", + "DICT_7X7_1000", + "DICT_ARUCO_ORIGINAL", + ], + default="DICT_4X4_50", + help="Choose the ArUco dictionary size.", + ) + + parser.add_argument( + "--checker_dim_meters", + "-cd", + dest="checker_dim", + type=float, + default=0.115, + help="Checker size in meters", + ) + + parser.add_argument( + "--marker_dim_meters", + "-md", + dest="marker_dim", + type=float, + default=0.09, + help="Aruco Marker size in meters", + ) + + # path and saving + parser.add_argument( + "--data_path", + "-dp", + dest="data_path", + type=str, + help="The absolute path in which to save images", + default=None, + ) + + parser.add_argument( + "--result_path", + "-rp", + dest="result_path", + type=str, + required=False, + help="The absolute path where to store calibration result file", + ) + + parser.add_argument( + "--tag", + "-t", + dest="tag", + type=str, + required=False, + default="default", + help=( + "What heading to put for the calibration in the config file." + "If this is your first time running, the tag should be set to default " + "for the sake of interoperability with other functionality." + "If this is a shared config file with other people, perhaps put" + "a unique identifier, or default, if you'd like to override" + "for everyone." + ), + ) + + parser.add_argument( + "--unsafe_tag_save", + action="store_true", + help="If set, skips safety checks for tagging calibration.", + ) + + parser.add_argument( + "--stereo_pairs", + "-sp", + dest="stereo_pairs", + nargs="+", + type=lambda s: tuple(int(x) for x in s.split(",")), + default=[(0, 1)], + help=( + "Stereo camera pairs to calibrate, as comma-separated index pairs. " + "E.g. '0,1' for a single stereo pair between camera 0 and camera 1. " + "Defaults to [(0, 1)]." + ), + ) + + return parser + + +def calibrate_robot_cli(parser: argparse.ArgumentParser | None = None) -> argparse.ArgumentParser: + if parser is None: + parser = calibrator_cli() + + parser.add_argument( + "--dist_from_board_viewpoint_range", + "-dfbvr", + nargs="+", + type=float, + dest="dist_from_board_viewpoint_range", + default=[0.5, 0.6, 0.1], + help=( + "What distances to conduct calibrations at relative to the board. (along the normal vector) " + "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep. " + "These are used to sample viewpoints for automatic collection. " + ), + ) + group = parser.add_mutually_exclusive_group() + group.add_argument( + "--degrees", + "-d", + dest="use_degrees", + action="store_true", + default=True, + help="Use degrees for rotation ranges (default)", + ) + group.add_argument( + "--radians", + "-r", + dest="use_degrees", + action="store_false", + help="Use radians for rotation ranges", + ) + defaults = [[-10, 11, 10], [-10, 11, 10], [-10, 11, 10]] + for idx, axis in enumerate(["x", "y", "z"]): + parser.add_argument( + f"--{axis}_axis_rot_viewpoint_range", + f"-{axis}arvr", + nargs="+", + type=float, + default=defaults[idx], + dest=f"{axis}_axis_rot_viewpoint_range", + help=( + f"What range of viewpoints around {axis}-axis to sample relative to boards normal vector. " + "Three value array arg defines the [Start, Stop), step. for the viewpoint sweep " + "These are used to sample viewpoints for automatic collection. " + "Assuming that the camera pose is in opencv/ROS format. " + ), + ) + + parser.add_argument( + "--max_num_images", + dest="max_num_images", + type=int, + default=10000, + help="The maximum number of images", + required=False, + ) + + parser.add_argument( + "--settle_time", + "-st", + dest="settle_time", + type=float, + default=1.0, + help="How long to wait after movement to take a picture; don't want motion blur", + ) + + parser.add_argument( + "--from_data", + "-fd", + dest="from_data", + action="store_true", + help="Whether to only calibrate from recorded dataset on file.", + ) + + parser.add_argument( + "--dist_along_board_width", + "-dabw", + nargs="+", + type=float, + dest="dist_along_board_width", + default=[-0.2, 0.3, 0.1], + help=( + "What distances to conduct calibrations at relative to the board (along the board width / X axis). " + "Three value array arg defines the [Start, Stop), step for the viewpoint sweep." + ), + ) + + parser.add_argument( + "--save_data", + action="store_true", + dest="save_data", + default=False, + help="Whether to save the collected image dataset to data_path.", + ) + + parser.add_argument( + "--from_yaml", + "-yaml", + dest="from_yaml", + action="store_true", + help=( + "Whether the data is from a yaml file. Use this and the '--from_data' and '--send' args to send a" + " previously saved calibration yaml to the robot" + ), + ) + + return parser + + +def spot_cli(parser: argparse.ArgumentParser | None = None) -> argparse.ArgumentParser: + if parser is None: + parser = calibrate_robot_cli(None) + + parser.add_argument( + "--robot_name", + "-rn", + dest="robot_name", + type=str, + help="The name of the Robot to calibrate (auto-detected from robot if not supplied)", + default=None, + required=False, + ) + + parser.add_argument( + "--ip", + "-i", + "-ip", + dest="ip", + type=str, + help="The IP address of the Robot to calibrate (required when connecting to the robot)", + default=None, + required=False, + ) + parser.add_argument( + "--user", + "-u", + "--username", + dest="username", + type=str, + help="Robot Username (required when connecting to the robot)", + default=None, + required=False, + ) + parser.add_argument( + "--pass", + "-pw", + "--password", + dest="password", + type=str, + help="Robot Password (required when connecting to the robot)", + default=None, + required=False, + ) + + parser.add_argument( + "--spot_rgb_photo_width", + "-dpw", + type=int, + default=640, + dest="spot_rgb_photo_width", + help="What resolution use for Spot's RGB Hand Camera (width). Currently, only 640 and 1920 are supported", + ) + + parser.add_argument( + "--spot_rgb_photo_height", + "-dph", + type=int, + default=480, + help="What resolution use for Spot's RGB Hand Camera (width). Currently, only 480 and 1080 are supported", + ) + + parser.add_argument( + "--save_to_robot", + "-save", + dest="save_to_robot", + action="store_true", + help="Whether to save the calibration to the robot.", + ) + + return parser diff --git a/spot_wrapper/calibration/automatic_camera_calibration_robot.py b/spot_wrapper/calibration/calibration_helpers.py similarity index 88% rename from spot_wrapper/calibration/automatic_camera_calibration_robot.py rename to spot_wrapper/calibration/calibration_helpers.py index f329a680..8c27b426 100644 --- a/spot_wrapper/calibration/automatic_camera_calibration_robot.py +++ b/spot_wrapper/calibration/calibration_helpers.py @@ -1,14 +1,34 @@ -# Copyright (c) 2025 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. -# Copyreference (c) 2024 Boston Dynamics AI Institute LLC. All references reserved. +from __future__ import annotations -# What's below can be used in standalone tool from abc import ABC, abstractmethod -from typing import List, Optional, Tuple, Union +from typing import List, Optional, Tuple, TypedDict, Union import numpy as np +class Intrinsics(TypedDict): + dist_coeffs: np.ndarray + camera_matrix: np.ndarray + nrows: int + ncols: int + + +class CalibrationResults(TypedDict): + dist_coeffs_origin: np.ndarray + camera_matrix_origin: np.ndarray + image_dim_origin: np.ndarray + dist_coeffs_reference: np.ndarray + camera_matrix_reference: np.ndarray + image_dim_reference: np.ndarray + R: np.ndarray + T: np.ndarray + R_handeye: Optional[np.ndarray] + T_handeye: Optional[np.ndarray] + average_reprojection_error: float + + class AutomaticCameraCalibrationRobot(ABC): @abstractmethod def capture_images(self) -> Union[List, np.ndarray]: diff --git a/spot_wrapper/calibration/calibration_util.py b/spot_wrapper/calibration/calibration_util.py index 19cc33a7..9a6b269d 100644 --- a/spot_wrapper/calibration/calibration_util.py +++ b/spot_wrapper/calibration/calibration_util.py @@ -1,1016 +1,31 @@ -# Copyright (c) 2025 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. - -# Copyreference (c) 2024 Boston Dynamics AI Institute LLC. All references reserved. +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. import argparse import logging import os import re -from copy import deepcopy from datetime import datetime from glob import glob -from math import radians from time import sleep -from typing import Dict, List, Optional, Tuple, Union +from typing import Any, Dict, List, Optional, Tuple, Union import cv2 import numpy as np import yaml -from spot_wrapper.calibration.automatic_camera_calibration_robot import ( +from spot_wrapper.calibration.calibration_helpers import ( AutomaticCameraCalibrationRobot, ) - -logging.basicConfig( - level=logging.INFO, +from spot_wrapper.calibration.charuco_board_detection import ( + create_ideal_charuco_image, + detect_charuco_corners, + get_relative_viewpoints_from_board_pose_and_param, + multistereo_calibration_charuco, ) logger = logging.getLogger(__name__) -SPOT_DEFAULT_ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) -OPENCV_VERSION = tuple(map(int, cv2.__version__.split("."))) -OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION = (4, 7, 0) - - -def create_charuco_board( - num_checkers_width: int, - num_checkers_height: int, - checker_dim: float, - marker_dim: float, - aruco_dict: cv2.aruco_Dictionary, - legacy: bool = True, -) -> cv2.aruco_CharucoBoard: - """ - Create a Charuco board using the provided parameters and Aruco dictionary. - Issues a deprecation warning if using the older 'CharucoBoard_create' method. - - Args: - num_checkers_width (int): Number of checkers along the width of the board. - num_checkers_height (int): Number of checkers along the height of the board. - checker_dim (float): Size of the checker squares. - marker_dim (float): Size of the Aruco marker squares. - aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary to use for marker generation. - - Returns: - charuco (cv2.aruco_CharucoBoard): The generated Charuco board. - """ - - opencv_version = tuple(map(int, cv2.__version__.split("."))) - - if opencv_version < (4, 7, 0): - logger.warning( - ( - "Creating Charuco Board..." - "You're using an older version of OpenCV requires the additional OpenCV Modules. " - "This will not work without the additional modules (opencv-contrib-python). " - "Consider upgrading to OpenCV >= 4.7.0 to enable " - "the use of this tool with base opencv (opencv-python) " - "without relying on additional modules." - ), - ) - - # Create Charuco board using the older method - charuco = cv2.aruco.CharucoBoard_create( - num_checkers_width, - num_checkers_height, - checker_dim, - marker_dim, - aruco_dict, - ) - - else: - # Create Charuco board using the newer method - charuco = cv2.aruco_CharucoBoard( - (num_checkers_width, num_checkers_height), - checker_dim, - marker_dim, - aruco_dict, - ) - charuco.setLegacyPattern(legacy) - - return charuco - - -SPOT_DEFAULT_CHARUCO = create_charuco_board( - num_checkers_width=9, - num_checkers_height=4, - checker_dim=0.115, - marker_dim=0.09, - aruco_dict=SPOT_DEFAULT_ARUCO_DICT, - legacy=True, -) - - -def get_multiple_perspective_camera_calibration_dataset( - auto_cam_cal_robot: AutomaticCameraCalibrationRobot, - max_num_images: int = 10000, - distances_x: Optional[np.ndarray] = None, - distances_z: Optional[np.ndarray] = None, - x_axis_rots: Optional[np.ndarray] = None, - y_axis_rots: Optional[np.ndarray] = None, - z_axis_rots: Optional[np.ndarray] = None, - use_degrees: bool = True, - settle_time: float = 0.1, - data_path: str = os.path.expanduser("~"), - save_data: Optional[bool] = True, -) -> Tuple[np.ndarray, np.ndarray]: - """ - Move the robot around to look at the calibration target from different viewpoints, - capturing time-synchronized - consistent-ordering images from every camera at each viewpoint. - - Args: - auto_cam_cal_robot (AutomaticCameraCalibrationRobot): The robot to automatically - calibrate. - max_num_images (int, optional): The maximum amount of images to take with EACH camera, - (so max total number of captures) cuts out the calibration early if this number is - reached prior to all viewpoints being reached . Defaults to 100. - distances (np.ndarray, optional): What distances - away from the calibration board's pose (along the Z axis) - to sample calibration viewpoints from. Defaults to None. - x_axis_rots (np.ndarray, optional): What - x-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to None. - y_axis_rots (np.ndarray, optional): What - y-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to None. - z_axis_rots (np.ndarray, optional): What - z-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to None. - use_degrees (bool, optional): Whether to use degrees for the rotations - about the axis to sample viewpoints from. Defaults to True. - settle_time (float, optional): How long to wait in seconds after moving the robot - to a new viewpoint prior to taking a picture. Defaults to 0.1. - data_path (str, optional): where to save - the calibration dataset, if it's being saved. Defaults to os.path.expanduser("~"). - save_data (Optional[bool], optional): whether to save the calibration - dataset. Defaults to True. - - Returns: - np.ndarray: A list of lists, where the internal list is a time-synchronized - camera-ordering-consistent list of images of the calibration target. - """ - primed_pose = auto_cam_cal_robot.move_cameras_to_see_calibration_target() - logger.info("Primed arm...") - sleep(settle_time) - images = auto_cam_cal_robot.capture_images() - capture_data = {} - capture_data["primed_images"] = images - capture_data["primed_pose"] = primed_pose - - R_vision_to_target, tvec_vision_to_target = auto_cam_cal_robot.localize_target_to_principal_camera(images) - viewpoints = get_relative_viewpoints_from_board_pose_and_param( - R_vision_to_target, - tvec_vision_to_target, - distances_x=distances_x, - distances_z=distances_z, - x_axis_rots=x_axis_rots, - y_axis_rots=y_axis_rots, - z_axis_rots=z_axis_rots, - degree_offset_rotations=use_degrees, - ) - calibration_images = [] - logger.info("Beginning Calibration") - idx = 0 - poses = [] - while idx < max_num_images and idx < len(viewpoints): - logger.info(f"Visiting viewpoint {idx+1} of {min(len(viewpoints), max_num_images)}") - viewpoint = viewpoints[idx] - initial_pose, new_pose = auto_cam_cal_robot.offset_cameras_from_current_view( - transform_offset=viewpoint, - origin_t_planning_frame=primed_pose, - duration_sec=0.1, - ) - poses.append(new_pose) - logger.info("At viewpoint, waiting to settle") - sleep(settle_time) - images = auto_cam_cal_robot.capture_images() - logger.info("Snapped pics ;)") - calibration_images.append(images) - idx = len(calibration_images) - if save_data: - if idx == 1: - create_calibration_save_folders(data_path, len(images)) - logger.info(f"Saving image batch {idx+1}") - for jdx, image in enumerate(images): - cv2.imwrite( - os.path.join(data_path, str(jdx), f"{idx+1}.png"), - image, - ) - np.save(os.path.join(data_path, "poses", f"{idx+1}.npy"), new_pose) - calibration_images = np.array(calibration_images, dtype=object) - return (np.array(calibration_images, dtype=object), poses) - - -def multistereo_calibration_charuco( - images: np.ndarray, - desired_stereo_pairs: Optional[List[Tuple[int, int]]] = None, - charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, - aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, - camera_matrices: Optional[Dict] = {}, - dist_coeffs: Optional[Dict] = {}, - poses: Union[np.ndarray, None] = None, -) -> Dict: - """ - Calibrates the intrinsic and extrinsic parameters for multiple stereo camera pairs - using Charuco board markers. This function performs stereo calibration on a series of - time-synchronized images - captured by multiple cameras. It uses a Charuco board for calibration and returns - the calibrated intrinsic and extrinsic parameters for the specified stereo camera pairs. - - Args: - images (np.ndarray): A 2D array of time-synchronized camera captures. The array should - be of shape (n, m), where 'n' represents the number of time-synchronized captures - and 'm' represents the number of cameras. Each internal list corresponds to a set - of images captured at the same time across multiple cameras. - - desired_stereo_pairs (Optional[List[Tuple[int, int]]], optional): A list of tuples - specifying the pairs of camera indices to be calibrated. The indices refer to the - cameras based on their positions in the `images` array. For example, index `0` - corresponds to the first camera, index `1` to the second camera, and so forth, - up to `n-1` (where `n` is the number of cameras). - If None, all cameras are registered to the first camera (index `0`). Defaults to None. - - The relationship between `desired_stereo_pairs` and `images` is crucial. Each tuple in - `desired_stereo_pairs` should be in the form - `(origin_camera_idx, reference_camera_idx)`, - where `origin_camera_idx` and `reference_camera_idx` correspond to the - indices of the cameras in the `images` array. For each stereo pair, - the function attempts to calibrate the `origin_camera_idx` - relative to the `reference_camera_idx`. If no pairs are provided, - the function defaults to registering - all cameras to the first camera (index `0`). - - charuco_board (cv2.aruco_CharucoBoard, optional): The Charuco board configuration used - for calibration. Defaults to SPOT_DEFAULT_CHARUCO. - - aruco_dict (cv2.aruco_Dictionary, optional): The Aruco dictionary used - to detect the markers. Defaults to SPOT_DEFAULT_ARUCO_DICT. - - camera_matrices (Optional[Dict], optional): A dictionary mapping camera indices - to their existing camera matrices. If a matrix is not provided for a camera, - it will be computed during calibration - if that camera is part of a stereo pair. Defaults to an empty dictionary. - - dist_coeffs (Optional[Dict], optional): A dictionary mapping camera indices - to their distortion coefficients. - If coefficients are not provided for a camera, they will be computed during calibration. - Defaults to an empty dictionary. - poses (Union[np.ndarray, None]): Either a list of 4x4 homogenous transforms from which - pictures where taken, or None if unknown. Needs to be supplied for robot to camera cal. - (planning frame to base frame), or None - - Raises: - ValueError: Raised if fewer than two cameras are provided, as stereo calibration - requires at least two cameras. - ValueError: Raised if a stereo pair includes the same camera twice. - ValueError: Raised if a camera in a stereo pair cannot be calibrated. - - Returns: - Dict: A dictionary containing the calibrated intrinsic and extrinsic parameters - for the specified stereo pairs. - The keys are formatted as "origin_camera_idx_reference_camera_idx", - and the values contain the calibration data. - - """ - calibration = {} - num_cams = images.shape[1] - if num_cams == 0 or num_cams == 1: - raise ValueError("Stereo Madness requires more than one camera. Use the monocular method instead.") - else: # More than one camera - Multi-Stereo Madness ;p - for idx in range(num_cams): - if idx not in camera_matrices: - camera_matrices[idx] = None - if idx not in dist_coeffs: - dist_coeffs[idx] = None - - if desired_stereo_pairs is None: - desired_stereo_pairs = [] - for idx in range(1, num_cams): - """ - Desired stereo pairs is a mapping, where the 0th index is the origin (base) - frame, where the reference frame is defined within the origin (base) frame's - coordinate system. The reference frame is the 1st index. - - By Default, if desired_stereo_pairs is None, - then each camera's intrinsic is defined in the 0th camera's frame, - so this is set as the origin for all stereo calibrations - """ - desired_stereo_pairs.append((0, idx)) # just register all in 0th cam frame - - for idx, pair in enumerate(desired_stereo_pairs): - for index in pair: - if index < 0 or index > num_cams: - raise ValueError( - f"Requested to stereo register non-existent camera index {index} out of {num_cams}" - ) - origin_camera_idx = pair[0] - reference_camera_idx = pair[1] - - if pair[0] == pair[1]: - logging.warning( - "You requested to register a camera to itself. Ignoring,and continuing to the next stereo pair." - ) - continue - logging.info( - f"Attempting to register {origin_camera_idx}" - f"to {reference_camera_idx}, pair {idx} of" - f" {len(desired_stereo_pairs)}" - ) - try: - key = str(origin_camera_idx) + "_" + str(reference_camera_idx) - stereo_dict = stereo_calibration_charuco( - origin_images=images[:, origin_camera_idx], - reference_images=images[:, reference_camera_idx], - charuco_board=charuco_board, - aruco_dict=aruco_dict, - camera_matrix_origin=camera_matrices[origin_camera_idx], - dist_coeffs_origin=dist_coeffs[origin_camera_idx], - camera_matrix_reference=camera_matrices[reference_camera_idx], - dist_coeffs_reference=dist_coeffs[reference_camera_idx], - poses=poses, - ) - camera_matrices[origin_camera_idx] = stereo_dict["camera_matrix_origin"] - dist_coeffs[origin_camera_idx] = stereo_dict["dist_coeffs_origin"] - camera_matrices[reference_camera_idx] = stereo_dict["camera_matrix_reference"] - dist_coeffs[reference_camera_idx] = stereo_dict["dist_coeffs_reference"] - - calibration[key] = stereo_dict - except ValueError as ve: # maybe could keep going here instead for partial calibration? - raise ValueError(f"Failed to calibrate pair {pair} : {ve}") - - return calibration - - -def get_correlation_map_to_enforce_ascending_ids_from_bottom_left_corner( - all_charuco_corners: List, - all_charuco_ids: List, - charuco_board: cv2.aruco_CharucoBoard, -) -> List: - """ - This is needed only for OpenCV versions > 4.7.0. Basically, - this determines the needed correlation - to ensure that internal corners are numbered left to right bottom - up as opposed to left to right top down. - - For more info see https://github.com/opencv/opencv/issues/26126 - - Args: - all_charuco_corners (List): All charuco corners of the board. - all_charuco_ids (List): All charuco ids of the board, as returned by the Charuco detector - for an ideal board with a full view. Expected to be a list ascending - from 0 but could be otherwise - charuco_board (cv2.aruco_CharucoBoard): the charuco board to utilize - to construct the correlation map - - Returns: - List: the correlation map where the indicies represent to the original - ordering of corners, and the values at each index represent the new - ordering index of a corner. Can be used to ensure bottom up - left to right ordering of internal corners. - """ - - num_checker_width, num_checker_height = charuco_board.getChessboardSize() - num_interior_corners = (num_checker_width - 1) * (num_checker_height - 1) - correlation_map = np.array([i for i in range(num_interior_corners)]) - # Adjust indexing to account for nested arrays - first_corner_height = all_charuco_corners[all_charuco_ids[0][0]][0][1] - last_corner_height = all_charuco_corners[all_charuco_ids[-1][0]][0][1] - row_num_a = 0 - row_num_b = num_checker_height - 2 - - if first_corner_height < last_corner_height: - logger.warning( - "Detected Charuco Configuration " - "where internal corners (detected checker corners) are numbered top down, " - "(left to right) as opposed to bottom up (left to right). " - "Enforcing bottom up numbering instead. " - "This ensures that the Z axis points out of the board " - "As opposed to -180 degrees about the X axis " - "relative to the Z out of the board" - ) - else: - logger.warning( - "You have selected to enforce ascending ids from the bottom left corner " - "But it seems as if your ids are already in that form " - "Returning identity correlation map" - ) - return [int(mapping) for mapping in correlation_map] - - while row_num_a < row_num_b: - row_num_a_correlation_slice = slice( - row_num_a * (num_checker_width - 1), (row_num_a * (num_checker_width - 1) + (num_checker_width - 1)), 1 - ) - - row_num_b_correlation_slice = slice( - row_num_b * (num_checker_width - 1), ((row_num_b * (num_checker_width - 1)) + (num_checker_width - 1)), 1 - ) - # Record A - precopy_row_a = deepcopy(correlation_map[row_num_a_correlation_slice]) - # Copy B onto A - correlation_map[row_num_a_correlation_slice] = correlation_map[row_num_b_correlation_slice] - # copy old A into B - correlation_map[row_num_b_correlation_slice] = precopy_row_a - row_num_a += 1 - row_num_b -= 1 - return [int(mapping) for mapping in correlation_map] - - -def detect_charuco_corners( - img: np.ndarray, - charuco_board: cv2.aruco_CharucoBoard, - aruco_dict: cv2.aruco_Dictionary, - enforce_ascending_ids_from_bottom_left_corner: Union[bool, None] = None, -) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]: - """ - Detect the Charuco Corners and their IDs in an image, with support for OpenCV versions before and after 4.7.0. - - Args: - img (np.ndarray): The image to find Charuco corners in. - charuco_board (cv2.aruco_CharucoBoard, optional): The Charuco board to look for. - Defaults to SPOT_DEFAULT_CHARUCO. - aruco_dict (cv2.aruco_Dictionary, optional): The Aruco dictionary to use. - Defaults to SPOT_DEFAULT_ARUCO_DICT. - enforce_ascending_ids_from_bottom_left_corner (Union[bool, None]): whether to - ensure that internal charuco corners are numbered left to right bottom up - (only ensures bottom up, assumes already left to right). - - Returns: - Tuple[Optional[np.ndarray], Optional[np.ndarray]]: The detected corners and their IDs, - or (None, None) if not found. - """ - # Convert the image to grayscale if it's not already - if len(img.shape) == 2: - gray = img - else: - gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - - if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: - # Older OpenCV version - corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict) - if ids is not None: - _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco( - corners, ids, gray, charuco_board, minMarkers=0 - ) - return charuco_corners, charuco_ids - else: - return None, None - else: - # Newer OpenCV version with charuco_detector - detector_params = cv2.aruco.CharucoParameters() - detector_params.minMarkers = 0 - detector_params.tryRefineMarkers = True - charuco_detector = cv2.aruco.CharucoDetector(charuco_board, detector_params) - charuco_detector.setBoard(charuco_board) - charuco_corners, charuco_ids, _, _ = charuco_detector.detectBoard(gray) - - if charuco_ids is None: - return None, None - - enforce_ids = enforce_ascending_ids_from_bottom_left_corner - if enforce_ids is not None and hasattr(detect_charuco_corners, "enforce_ids"): - logger.warning( - "Previously, for detecting internal charuco corners, the enforce " - "ascending from bottom left corner id policy was set to: " - f"{detect_charuco_corners.enforce_ids}" - f"it will now be set to {enforce_ids}" - ) - detect_charuco_corners.enforce_ids = enforce_ids - elif enforce_ids is not None and not hasattr(detect_charuco_corners, "enforce_ids"): - logger.warning( - "For detecting charuco corners, the enforce " - "ascending from bottom left corner id policy is set to: " - f"{enforce_ids}. For this call, and future calls until set otherwise." - ) - detect_charuco_corners.enforce_ids = enforce_ids - - # Create the identity correlation map... - num_checker_width, num_checker_height = charuco_board.getChessboardSize() - num_interior_corners = (num_checker_width - 1) * (num_checker_height - 1) - correlation_map = np.array([i for i in range(num_interior_corners)]) - - if ( - hasattr(detect_charuco_corners, "enforce_ids") - and detect_charuco_corners.enforce_ids - and not hasattr(detect_charuco_corners, "corr_map") - ): # correlation map not computed - ideal_charuco = charuco_board.generateImage(outSize=(1000, 1000)) - all_charuco_corners, all_charuco_ids, _, _ = charuco_detector.detectBoard(ideal_charuco) - - detect_charuco_corners.corr_map = get_correlation_map_to_enforce_ascending_ids_from_bottom_left_corner( - all_charuco_corners, all_charuco_ids, charuco_board - ) - - if ( - hasattr(detect_charuco_corners, "enforce_ids") - and detect_charuco_corners.enforce_ids - and hasattr(detect_charuco_corners, "corr_map") - ): # correlation map computed - logger.warning("Using cached correlation map to order IDs") - correlation_map = detect_charuco_corners.corr_map # grab the map - - reworked_charuco_ids = [] - for charuco_id in charuco_ids: - reworked_charuco_ids.append([correlation_map[charuco_id[0]]]) - - return charuco_corners, reworked_charuco_ids - - -def get_charuco_board_object_points( - charuco_board: cv2.aruco_CharucoBoard, - corners_ids: Union[List, np.ndarray], -) -> np.ndarray: - """ - From a charuco board, and corner ids, generate the object points for use - in a calibration sequence to solve the perspective and point problem. - - Args: - charuco_board (cv2.aruco_CharucoBoard): the charuco board - to generate object points for - corners_ids (Union[List, np.ndarray]): which corner ids to generate points for - - Returns: - np.ndarray: the object points - """ - if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: - corners = charuco_board.chessboardCorners - else: - corners = charuco_board.getChessboardCorners() - object_points = [] - for idx in corners_ids: - object_points.append(corners[idx]) - return np.array(object_points, dtype=np.float32) - - -def calibrate_single_camera_charuco( - images: List[np.ndarray], - charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, - aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, - debug_str: str = "", -) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """ - Calibrate a monocular camera from a charuco board. - - Args: - images (List[np.ndarray]): A list of images of the board - charuco_board (cv2.aruco_CharucoBoard, optional): which - board to look for in the images. Defaults to SPOT_DEFAULT_CHARUCO. - aruco_dict (cv2.aruco_Dictionary, optional): which - aruco dict to look for in the images. Defaults to SPOT_DEFAULT_ARUCO_DICT. - debug_str (str, optional): what to add to the logging - to differentiate this monocular calibration from others, for the sake - of stereo calibrations. Defaults to "". - - Raises: - ValueError: Not enough data to calibrate - - Returns: - Tuple[np.ndarray, np.ndarray]: the camera matrix, distortion coefficients, - rotation matrices, tvecs - """ - all_corners = [] - all_ids = [] - img_size = None - for idx, img in enumerate(images): - if img_size is None: - img_size = img.shape[:2][::-1] - - charuco_corners, charuco_ids = detect_charuco_corners(img, charuco_board, aruco_dict) - - if charuco_corners is not None and len(charuco_corners) >= 8: - all_corners.append(charuco_corners) - all_ids.append(charuco_ids) - else: - logger.warning(f"Not enough corners detected in image index {idx} {debug_str}; ignoring") - - if len(all_corners) > 1: - obj_points_all = [] - img_points = [] - ids = [] - for corners, ids in zip(all_corners, all_ids): - obj_points = get_charuco_board_object_points(charuco_board, ids) - obj_points_all.append(obj_points) - img_points.append(corners) - logger.info(f"About to process {len(obj_points_all)} points for single camera cal | {debug_str}") - # Long term improvement: could pull tvec for use to localize camera relative to robot? - _, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( # use LU flag critical for speed - obj_points_all, - img_points, - img_size, - None, - None, - flags=cv2.CALIB_USE_LU, - ) - rmats = np.array([cv2.Rodrigues(rvec)[0] for rvec in rvecs]) - return camera_matrix, dist_coeffs, rmats, tvecs - else: - raise ValueError(f"Not enough valid points to individually calibrate {debug_str}") - - -def stereo_calibration_charuco( - origin_images: List[np.ndarray], - reference_images: List[np.ndarray], - charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, - aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, - camera_matrix_origin: Optional[np.ndarray] = None, - dist_coeffs_origin: Optional[np.ndarray] = None, - camera_matrix_reference: Optional[np.ndarray] = None, - dist_coeffs_reference: Optional[np.ndarray] = None, - poses: Union[np.ndarray, None] = None, -) -> Dict: - """ - Perform a stereo calibration from a set of synchronized stereo images of a charuco calibration - board. - - Args: - origin_images (List[np.ndarray]): A list of images synced to reference images from camera 0 - reference_images (List[np.ndarray]): A list of images synced to origin images from camera 1 - charuco_board (cv2.aruco_CharucoBoard, optional): What charuco board to - use for the cal. Defaults to SPOT_DEFAULT_CHARUCO. - aruco_dict (cv2.aruco_Dictionary, optional): What aruco board to use for the cal. - Defaults to SPOT_DEFAULT_ARUCO_DICT. - camera_matrix_origin (Optional[np.ndarray], optional): What camera - matrix to assign to camera 0. If none, is computed. Defaults to None. - dist_coeffs_origin (Optional[np.ndarray], optional): What distortion coefficients - to assign to camera 0. If None, is computed. Defaults to None. - camera_matrix_reference (Optional[np.ndarray], optional): What camera - matrix to assign to camera 1. If none, is computed. . Defaults to None. - dist_coeffs_reference (Optional[np.ndarray], optional): What distortion coefficients - to assign to camera 1. If None, is computed. Defaults to None. - poses (Union[np.ndarray, None]): Either a list of 4x4 homogenous transforms from which - pictures where taken, or None if unknown. Needs to be supplied for robot to camera cal. - (planning frame to base frame), or None - Raises: - ValueError: Could not calibrate a camera individually - ValueError: Not enough points to stereo calibrate - ValueError: Could not stereo calibrate - - Returns: - Dict: _description_ - """ - if camera_matrix_origin is None or dist_coeffs_origin is None: - logger.info("Calibrating Origin Camera individually") - (camera_matrix_origin, dist_coeffs_origin, rmats_origin, tvecs_origin) = calibrate_single_camera_charuco( - images=origin_images, - charuco_board=charuco_board, - aruco_dict=aruco_dict, - debug_str="for origin camera", - ) - if camera_matrix_reference is None or dist_coeffs_origin is None: - logger.info("Calibrating reference Camera individually ") - (camera_matrix_reference, dist_coeffs_reference, rmats_reference, tvecs_reference) = ( - calibrate_single_camera_charuco( - images=reference_images, - charuco_board=charuco_board, - aruco_dict=aruco_dict, - debug_str="for reference camera", - ) - ) - - if camera_matrix_origin is None or camera_matrix_reference is None: - raise ValueError("Could not calibrate one of the cameras as desired") - - all_corners_origin = [] - all_corners_reference = [] - all_ids_origin = [] - all_ids_reference = [] - img_size = None - - no_poses = poses is None - if no_poses: - poses = [x for x in range(len(origin_images))] - else: - filtered_poses = [] - - no_poses = poses is None - if no_poses: # fill up poses with dummy values so that you can iterate over poses - # with images zip(origin_images, reference_images, poses) together regardless of if poses - # are actually supplied (for the sake of brevity) - poses = [x for x in range(len(origin_images))] - else: - filtered_poses = [] - - for origin_img, reference_img, pose in zip(origin_images, reference_images, poses): - if img_size is None: - img_size = origin_img.shape[:2][::-1] - - origin_charuco_corners, origin_charuco_ids = detect_charuco_corners(origin_img, charuco_board, aruco_dict) - reference_charuco_corners, reference_charuco_ids = detect_charuco_corners( - reference_img, charuco_board, aruco_dict - ) - - if not no_poses: - filtered_poses.append(pose) - if origin_charuco_corners is not None and reference_charuco_corners is not None: - all_corners_origin.append(origin_charuco_corners) - all_corners_reference.append(reference_charuco_corners) - all_ids_origin.append(origin_charuco_ids) - all_ids_reference.append(reference_charuco_ids) - - if len(all_corners_origin) > 0: - obj_points_all = [] - img_points_origin = [] - img_points_reference = [] - for ( - origin_corners, - reference_corners, - origin_ids, - reference_ids, - ) in zip( - all_corners_origin, - all_corners_reference, - all_ids_origin, - all_ids_reference, - ): - common_ids = np.intersect1d(origin_ids, reference_ids) - if len(common_ids) >= 6: # Ensure there are at least 6 points - obj_points = get_charuco_board_object_points(charuco_board, common_ids) - obj_points_all.append(obj_points) - img_points_origin.append(origin_corners[np.isin(origin_ids, common_ids)]) - img_points_reference.append(reference_corners[np.isin(reference_ids, common_ids)]) - - if len(obj_points_all) > 0: - logger.info(f"Collected {len(obj_points_all)} shared point sets for stereo calibration.") - _, _, _, _, _, R, T, _, _ = cv2.stereoCalibrate( - obj_points_all, - img_points_origin, - img_points_reference, - camera_matrix_origin, - dist_coeffs_origin, - camera_matrix_reference, - dist_coeffs_reference, - img_size, - criteria=( - cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, - 100, - 1e-6, - ), - flags=cv2.CALIB_USE_LU, - ) - logger.info("Stereo calibration completed.") - result_dict = { - "dist_coeffs_origin": dist_coeffs_origin, - "camera_matrix_origin": camera_matrix_origin, - "image_dim_origin": np.array(origin_images[0].shape[:2]), - "dist_coeffs_reference": dist_coeffs_reference, - "camera_matrix_reference": camera_matrix_reference, - "image_dim_reference": np.array(reference_images[0].shape[:2]), - "R": R, - "T": T, - } - - if not no_poses: - logger.info("Attempting hand-eye calibation....") - # filtered_poses = np.array([np.linalg.inv(pose) for pose in filtered_poses]) - filtered_poses = np.array(filtered_poses) - # Use the filtered poses for the target-to-camera transformation - R_gripper2base = np.array([pose[:3, :3] for pose in filtered_poses]) - t_gripper2base = np.array([pose[:3, 3] for pose in filtered_poses]) - - R_handeye, T_handeye = cv2.calibrateHandEye( - R_gripper2base=R_gripper2base, - t_gripper2base=t_gripper2base, - R_target2cam=rmats_origin, - t_target2cam=tvecs_origin, - method=cv2.CALIB_HAND_EYE_DANIILIDIS, - ) - robot_to_cam = np.eye(4) # Initialize 4x4 identity matrix - robot_to_cam[:3, :3] = R_handeye # Populate rotation - robot_to_cam[:3, 3] = T_handeye.flatten() # Populate translation - - # Invert the homogeneous matrix - cam_to_robot = np.linalg.inv(robot_to_cam) - - # Extract rotation and translation from the inverted matrix - camera_to_robot_R = cam_to_robot[:3, :3] # Extract rotation - camera_to_robot_T = cam_to_robot[:3, 3] # Extract translation - logger.info("Hand-eye calibration completed.") - - # Add the hand-eye calibration results to the final result dictionary - result_dict["R_handeye"] = camera_to_robot_R - result_dict["T_handeye"] = camera_to_robot_T - return result_dict - else: - raise ValueError("Not enough valid points for stereo calibration.") - else: - raise ValueError("Not enough shared points for stereo calibration.") - - -def est_camera_t_charuco_board_center( - img: np.ndarray, - charuco_board: cv2.aruco_CharucoBoard, - aruco_dict: cv2.aruco_Dictionary, - camera_matrix: np.ndarray, - dist_coeffs: np.ndarray, -) -> Tuple[np.ndarray, np.ndarray]: - """ - Localizes the 6D pose of the checkerboard center using Charuco corners with OpenCV's solvePnP. - - The board pose's translation should be at the center of the board, with the orientation in OpenCV format, - where the +Z points out of the board with the other axes being parallel to the sides of the board. - - Args: - img (np.ndarray): The input image containing the checkerboard. - charuco_board (cv2.aruco_CharucoBoard): The Charuco board configuration. - aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary used to detect markers. - camera_matrix (np.ndarray): The camera matrix from calibration. - dist_coeffs (np.ndarray): The distortion coefficients from calibration. - - Returns: - Optional[Tuple[np.ndarray, np.ndarray]]: The rotation vector and translation vector - representing the 6D pose of the checkerboard center if found, else None. - """ - charuco_corners, charuco_ids = detect_charuco_corners(img, charuco_board, aruco_dict) - if charuco_corners is not None and charuco_ids is not None: - object_points = get_charuco_board_object_points(charuco_board, charuco_ids) - image_points = charuco_corners - - # Use solvePnP to estimate the pose of the Charuco board - success, rvec, tvec = cv2.solvePnP(object_points, np.array(image_points), camera_matrix, dist_coeffs) - - if success: - # Convert to the camera frame: Adjust the translation vector to be relative to the center of the board - x_trans = (charuco_board.getSquareLength() * charuco_board.getChessboardSize()[0]) / 2.0 - y_trans = (charuco_board.getSquareLength() * charuco_board.getChessboardSize()[1]) / 2.0 - center_trans = np.array([x_trans, y_trans, 0.0]).reshape((3, 1)) - rmat, _ = cv2.Rodrigues(rvec) - - tvec = tvec + rmat.dot(center_trans) - tvec_to_camera = tvec - return np.array(rmat), np.array(tvec_to_camera).ravel() - else: - raise ValueError("Pose estimation failed. You likely primed the robot too close to the board.") - else: - raise ValueError( - "Couldn't detect any Charuco boards in the image, " - "localization failed. Ensure the board is visible from the" - " primed pose." - ) - - -def convert_camera_t_viewpoint_to_origin_t_planning_frame( - origin_t_planning_frame: np.ndarray = np.eye(4), - planning_frame_t_opencv_camera: np.ndarray = np.eye(4), - opencv_camera_t_viewpoint: np.ndarray = np.eye(4), -) -> np.ndarray: - """ - Convert a camera viewpoint given in a camera frame, to the planning frame. - - Camera frame should be in OpenCV/ ROS format, where - +x should point to the right in the image - +y should point down in the image - +z should point into to plane of the image. - - Assuming that origin_t_planning_frame isn't aligned with this format, align it - so that the viewpoint can be localized as a hand pose command. - - Args: - origin_t_planning_frame (np.ndarray, optional): 4x4 homogenous transform - from origin to planning frame. Defaults to np.eye(4). - planning_frame_t_opencv_camera (np.ndarray, optional): 4x4 homogenous transform - from planning frame to the camera. Defaults to np.eye(4). - opencv_camera_t_viewpoint (np.ndarray, optional): 4x4 homogenous transform - from camera to the future viewpoint. Defaults to np.eye(4). - - Returns: - np.ndarray: 4x4 homogenous transform of the planning frame of a future viewpoint - expressed in the origin frame. - """ - origin_t_opencv_camera_pose = origin_t_planning_frame @ planning_frame_t_opencv_camera - origin_t_viewpoint = origin_t_opencv_camera_pose @ opencv_camera_t_viewpoint - origin_t_planning_frame_next = origin_t_viewpoint @ np.linalg.inv(planning_frame_t_opencv_camera) - return origin_t_planning_frame_next - - -def get_relative_viewpoints_from_board_pose_and_param( - R_board: np.ndarray, - tvec: np.ndarray, - distances_x: Optional[np.ndarray] = None, - distances_z: Optional[np.ndarray] = None, - x_axis_rots: Optional[np.ndarray] = None, - y_axis_rots: Optional[np.ndarray] = None, - z_axis_rots: Optional[np.ndarray] = None, - R_align_board_frame_with_camera: Optional[np.ndarray] = None, - degree_offset_rotations: bool = True, -) -> List[np.ndarray]: - """ - Given the position of charuco board, sample viewpoints facing the calibration board for - the robot's "principal" camera to visit. When the robot moves to these viewpoints, - it takes synchronized photos with all cameras to use for solving the calibration. - - ROS and OpenCV have the same camera format, where: - # +x should point to the right in the image - # +y should point down in the image - # +z should point into to plane of the image. - - All transforms to cameras are in this format. - - Args: - R_board (np.ndarray): the rotation of the calibration board in opencv notation relative - to the "principal" camera. - (See AutomaticCameraCalibration.localize_target_to_principal_camera) - tvec (np.ndarray): the translation of the calibration board in opencv notation relative to - the principal camera - distances (np.ndarray, optional): What distances - away from the calibration board's pose (along the Z axis) - to sample calibration viewpoints from. Defaults to np.arange(0.5, 0.8, 0.1). - x_axis_rots (np.ndarray, optional): What - x-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to np.arange(-21, 21, 5). - y_axis_rots (np.ndarray, optional): What - y-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to np.arange(-21, 21, 5). - z_axis_rots (np.ndarray, optional): What - z-axis rotations relative to the camera viewing the board orthogonally - to apply to sample viewpoints. Defaults to np.array([-21, 21, 5]). - R_align_board_frame_with_camera (np.ndarray, optional): By default, - the charuco pose rotation Z-axis points out of the pattern, - and into the camera. However, to sample viewpoints, the camera must face the board, - so viewpoints can't be sampled directly from the board pose as they would face - away from the board. - This rotates the board frame such that the Z axis points away - from the pattern, so that camera frames facing the board may be sampled - after applying a distance and offset rotation. - Defaults to np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]). - degree_offset_rotations (bool, optional): whether to use degree for the - sampling rotation parameters. Defaults to True. - - Returns: - List[np.ndarray]: a list of 4x4 homogenous transforms to visit in the "principal" cameras - frame - """ - if distances_x is None: - distances_x = np.arange(0.3, 0.7, 0.1) - if distances_z is None: - distances_z = np.arange(-0.2, 0.3, 0.1) - if x_axis_rots is None: - x_axis_rots = np.arange(-20, 21, 5) - if y_axis_rots is None: - y_axis_rots = np.arange(-20, 21, 5) - if z_axis_rots is None: - z_axis_rots = np.arange(-20, 21, 5) - if R_align_board_frame_with_camera is None: - R_align_board_frame_with_camera = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) - - if degree_offset_rotations: - x_axis_rots = [radians(deg) for deg in x_axis_rots] - y_axis_rots = [radians(deg) for deg in y_axis_rots] - z_axis_rots = [radians(deg) for deg in z_axis_rots] - - translations = [(tvec + R_board[:, 2] * dist + R_board[:, 0] * d2) for dist in distances_z for d2 in distances_x] - R_base = R_board @ R_align_board_frame_with_camera - - def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: - """ - Convert Euler angles to a rotation matrix using OpenCV, for the sake - of CLI convenience, conciseness, while not depending on an external transform lib. - """ - R_x = cv2.Rodrigues(np.array([roll, 0, 0]))[0] - R_y = cv2.Rodrigues(np.array([0, pitch, 0]))[0] - R_z = cv2.Rodrigues(np.array([0, 0, yaw]))[0] - return R_z @ R_y @ R_x - - poses = [] - for t in translations: - for x_axis_rot in x_axis_rots: - for y_axis_rot in y_axis_rots: - for z_axis_rot in z_axis_rots: - # Get the rotation matrix from Euler angles - R_rot = euler_to_rotation_matrix(x_axis_rot, y_axis_rot, z_axis_rot) - - # Combine with the base rotation - R_base_modified = R_base @ R_rot - - transform = np.eye(4) - transform[:-1, -1] = t.reshape((3,)) - transform[:-1, :-1] = R_base_modified - poses.append(transform) - - logger.info(f"Calculated {len(poses)} relative viewpoints to visit relative to the target.") - return poses - - -def create_calibration_save_folders(path: str, num_folders: int) -> None: - """ - Create a folder hierarchy to record a calibration - - Args: - path (str): The parent path - num_folders (int): How many cameras, a.k.a how many folder to create - - Raises: - ValueError: Not possible to create the folders, or no path specified - """ - if path is None: - raise ValueError("No path to save to :(") - else: - for idx in range(num_folders): - cam_path = os.path.join(path, str(idx)) - - logger.info(f"Creating image folder at {cam_path}") - os.makedirs(cam_path, exist_ok=True) - os.makedirs(os.path.join(path, "poses"), exist_ok=True) - logger.info(f"Done creating {num_folders} folders.") +directories = ["parent", "child", "poses", "depth"] def load_dataset_from_path(path: str) -> Tuple[np.ndarray, Optional[np.ndarray]]: @@ -1071,6 +86,8 @@ def save_calibration_parameters( output_path: str, num_images: int, tag: str, + parent_frame: str, + child_frame: str, parser_args: Optional[argparse.Namespace] = None, unsafe: bool = False, ) -> Dict: @@ -1095,11 +112,12 @@ def flatten_matrix(matrix: np.ndarray) -> List: def process_data_with_nested_dictionaries( data: Dict, ) -> Tuple[Dict, Dict]: - cameras = {} - relations = {} + cameras: Dict[int, Dict[str, List]] = {} + relations: Dict[int, Dict[int, Dict[str, List]]] = {} - for key, value in data.items(): - origin_cam, reference_cam = int(key.split("_")[0]), int(key.split("_")[1]) + for value in iter(data.values()): + origin_cam = 0 + reference_cam = 1 # Process origin camera data if origin_cam not in cameras: @@ -1180,28 +198,39 @@ def process_data_with_nested_dictionaries( # Process the new calibration data cameras, relations = process_data_with_nested_dictionaries(data) + first_result = next(iter(data.values())) + xform = np.eye(4) + if "R_handeye" in first_result and "T_handeye" in first_result: + xform[:3, :3] = first_result["R_handeye"] + xform[:3, 3] = np.array(first_result["T_handeye"]).flatten() + else: + xform[:3, :3] = first_result["R"] + xform[:3, 3] = first_result["T"].flatten() + # Prepare the output data under the specified tag - tagged_data = { - "intrinsic": cameras, - "extrinsic": relations, - "run_param": {}, - } - tagged_data["run_param"]["num_images"] = num_images - tagged_data["run_param"]["timestamp"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S") + run_params: Dict[str, Any] = {} + run_params["num_images"] = num_images + run_params["timestamp"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S") # Include parser parameters, excluding 'password' and 'username' if parser_args is not None: for arg in vars(parser_args): if arg not in ["password", "username", "result_path"]: - tagged_data["run_param"][arg] = getattr(parser_args, arg) + run_params[arg] = getattr(parser_args, arg) else: logger.warning("Saving calibration, but not the parameters used to obtain it.") - # Convert any tuples in run_param (like stereo_pairs) to lists - if "stereo_pairs" in tagged_data["run_param"]: - tagged_data["run_param"]["stereo_pairs"] = [list(pair) for pair in tagged_data["run_param"]["stereo_pairs"]] + # Convert any tuples in run_params (like stereo_pairs) to lists + if "stereo_pairs" in run_params: + run_params["stereo_pairs"] = [list(pair) for pair in run_params["stereo_pairs"]] # Save the updated data under the specified tag + tagged_data = { + "intrinsic": cameras, + "extrinsic": relations, + "run_params": run_params, + f"{child_frame}_t_{parent_frame}": flatten_matrix(xform), + } existing_data[tag] = tagged_data # Ensure the directory exists @@ -1219,80 +248,144 @@ def process_data_with_nested_dictionaries( return existing_data -def charuco_pose_sanity_check( - img: np.ndarray, charuco_board: cv2.aruco_CharucoBoard, aruco_dict: cv2.aruco_Dictionary -) -> np.ndarray: +def get_multiple_perspective_camera_calibration_dataset( + auto_cam_cal_robot: AutomaticCameraCalibrationRobot, + max_num_images: int = 10000, + distances_x: Optional[np.ndarray] = None, + distances_z: Optional[np.ndarray] = None, + x_axis_rots: Optional[np.ndarray] = None, + y_axis_rots: Optional[np.ndarray] = None, + z_axis_rots: Optional[np.ndarray] = None, + use_degrees: bool = True, + settle_time: float = 0.1, + data_path: str = os.path.expanduser("~"), + save_data: Optional[bool] = True, +) -> Tuple[np.ndarray, np.ndarray]: """ - This method: - 1. Defines the camera matrix as identity. - 2. Uses zero distortion coefficients. - 3. Estimates the Charuco board pose using `est_camera_t_charuco_board_center`. - 4. Transforms the pose into the camera frame. - 5. Visualizes the pose with 3D axes on the image using cv2 window. + Move the robot to multiple viewpoints and capture time-synchronized images from all cameras + at each viewpoint for use in calibration. Args: - img (np.ndarray): The input image containing the Charuco board. - charuco_board (cv2.aruco_CharucoBoard): The Charuco board configuration. - aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary used to detect markers. + auto_cam_cal_robot (AutomaticCameraCalibrationRobot): The robot to automatically calibrate. + max_num_images (int, optional): Maximum number of captures before cutting off the + viewpoint sweep early. Defaults to 10000. + distances_x (np.ndarray, optional): X-axis distances from the board to sample. + Defaults to None (uses charuco_board_detection defaults). + distances_z (np.ndarray, optional): Z-axis distances from the board to sample. + Defaults to None (uses charuco_board_detection defaults). + x_axis_rots (np.ndarray, optional): X-axis rotations to sample. Defaults to None. + y_axis_rots (np.ndarray, optional): Y-axis rotations to sample. Defaults to None. + z_axis_rots (np.ndarray, optional): Z-axis rotations to sample. Defaults to None. + use_degrees (bool, optional): Whether rotation parameters are in degrees. Defaults to True. + settle_time (float, optional): Seconds to wait after moving before capturing. + Defaults to 0.1. + data_path (str, optional): Directory to save the dataset if save_data is True. + Defaults to the user home directory. + save_data (Optional[bool], optional): Whether to save captured images and poses. + Defaults to True. Returns: - img_with_axes (np.ndarray): The image with the pose axes drawn. + Tuple[np.ndarray, np.ndarray]: Array of captured image sets and array of robot poses. """ + primed_pose = auto_cam_cal_robot.move_cameras_to_see_calibration_target() + logger.info("Primed arm...") + sleep(settle_time) + images = auto_cam_cal_robot.capture_images() - def is_z_axis_out_of_board(tvec): - """Determine if the Z-axis points out of the Charuco board (towards the camera).""" - return tvec[2] > 0 # If Z is positive, it points out of the board - - def visualize_pose_with_axis(img, rmat, tvec, camera_matrix, dist_coeffs, axis_length=0.115): - """Draws the 3D pose axes on the image and displays if the Z-axis is out or into the board.""" - axis = np.float32([[axis_length, 0, 0], [0, axis_length, 0], [0, 0, axis_length], [0, 0, 0]]).reshape(-1, 3) - - rmat_camera, tvec_camera = rmat, tvec - imgpts, _ = cv2.projectPoints(axis, rmat_camera, tvec_camera, camera_matrix, dist_coeffs) - - z_out_of_board = is_z_axis_out_of_board(tvec) - img_with_axes = img.copy() - origin = tuple(imgpts[3].ravel().astype(int)) - img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[0].ravel().astype(int)), (0, 0, 255), 3) # X - img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[1].ravel().astype(int)), (0, 255, 0), 3) # Y - img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[2].ravel().astype(int)), (255, 0, 0), 3) # Z - - if not z_out_of_board: - logger.warning("This tool assumes that Z axis is out of board, but it was detected as into board.") - window_name = f'Charuco Board Pose ({"Z-axis out of board" if z_out_of_board else "Z-axis into board"})' - cv2.imshow(window_name, img_with_axes) - elapsed_time = 0 - wait_interval = 100 # Wait 100 ms in each loop iteration - max_wait_time = 20 # 20 sec - while elapsed_time < max_wait_time * 1000: # Convert max_wait_time to milliseconds - if cv2.getWindowProperty(window_name, cv2.WND_PROP_VISIBLE) < 1: - break # Exit loop if window is closed - key = cv2.waitKey(wait_interval) # Check every 100ms - if key != -1: # If any key is pressed - break - elapsed_time += wait_interval - - cv2.destroyAllWindows() - - return img_with_axes - - # Camera matrix as identity and zero distortion coefficients - camera_matrix = np.eye(3, dtype=np.float32) - dist_coeffs = np.zeros(5, dtype=np.float32) - - # Estimate pose using the existing est_camera_t_charuco_board_center - rmat, tvec = est_camera_t_charuco_board_center(img, charuco_board, aruco_dict, camera_matrix, dist_coeffs) - - # Visualize the pose with 3D axes - return visualize_pose_with_axis(img, rmat, tvec, camera_matrix, dist_coeffs) + R_vision_to_target, tvec_vision_to_target = auto_cam_cal_robot.localize_target_to_principal_camera(images) + viewpoints = get_relative_viewpoints_from_board_pose_and_param( + R_vision_to_target, + tvec_vision_to_target, + distances_x=distances_x, + distances_z=distances_z, + x_axis_rots=x_axis_rots, + y_axis_rots=y_axis_rots, + z_axis_rots=z_axis_rots, + degree_offset_rotations=use_degrees, + ) + calibration_images = [] + logger.info("Beginning Calibration") + idx = 0 + poses = [] + while idx < max_num_images and idx < len(viewpoints): + logger.info(f"Visiting viewpoint {idx + 1} of {min(len(viewpoints), max_num_images)}") + viewpoint = viewpoints[idx] + _initial_pose, new_pose = auto_cam_cal_robot.offset_cameras_from_current_view( + transform_offset=viewpoint, + origin_t_planning_frame=primed_pose, + duration_sec=0.1, + ) + poses.append(new_pose) + logger.info("At viewpoint, waiting to settle") + sleep(settle_time) + images = auto_cam_cal_robot.capture_images() + logger.info("Snapped pics ;)") + calibration_images.append(images) + idx = len(calibration_images) + if save_data: + if idx == 1: + # Create numerical camera-index folders + for jdx in range(len(images)): + os.makedirs(os.path.join(data_path, str(jdx)), exist_ok=True) + os.makedirs(os.path.join(data_path, "poses"), exist_ok=True) + logger.info(f"Saving image batch {idx}") + for jdx, image in enumerate(images): + cv2.imwrite( + os.path.join(data_path, str(jdx), f"{idx}.png"), + image, + ) + np.save(os.path.join(data_path, "poses", f"{idx}.npy"), new_pose) + return (np.array(calibration_images, dtype=object), poses) -def create_ideal_charuco_image(charuco_board: cv2.aruco_CharucoBoard, dim=(500, 700), colorful=False): - if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: - img = charuco_board.draw(outSize=dim) - else: - img = charuco_board.generateImage(outSize=dim) - if colorful: - return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) - else: - return img +def calibration_helper( + images: Union[List[np.ndarray], np.ndarray], + args: argparse.Namespace, + charuco: cv2.aruco_CharucoBoard, + aruco_dict: cv2.aruco_Dictionary, + poses: np.ndarray, + result_path: str = None, + parent_frame: str = "body", + child_frame: str = "camera", +) -> dict: + logger.warning( + f"Calibrating from {len(images)} images.. for every " + f"{args.photo_utilization_ratio} recorded photos 1 is used to calibrate" + ) + if not args.allow_default_internal_corner_ordering: + logger.warning("Turning off corner swap (needed for localization) for calibration solution...") + logger.warning("Corner swap needed for initial localization, but breaks calibration.") + logger.warning("See https://github.com/opencv/opencv/issues/26126") + detect_charuco_corners( + create_ideal_charuco_image(charuco_board=charuco), + charuco_board=charuco, + aruco_dict=aruco_dict, + enforce_ascending_ids_from_bottom_left_corner=False, + ) + calibration = multistereo_calibration_charuco( + images[:: args.photo_utilization_ratio], + desired_stereo_pairs=args.stereo_pairs, + charuco_board=charuco, + aruco_dict=aruco_dict, + poses=poses, + ) + logger.info(f"Finished script, obtained {calibration}") + logger.info("Saving calibration param...") + + if result_path is None: + result_path = "calibration_result.yaml" # default file name if none provided + + args.result_path = result_path + + # Save the calibration parameters if a valid result path is provided + calibration_dict = save_calibration_parameters( + data=calibration, + output_path=args.result_path, + num_images=len(images[:: args.photo_utilization_ratio]), + tag=args.tag, + parent_frame=parent_frame, + child_frame=child_frame, + parser_args=args, + unsafe=args.unsafe_tag_save, + ) + return calibration_dict diff --git a/spot_wrapper/calibration/charuco_board_detection.py b/spot_wrapper/calibration/charuco_board_detection.py new file mode 100644 index 00000000..d2856e4f --- /dev/null +++ b/spot_wrapper/calibration/charuco_board_detection.py @@ -0,0 +1,1073 @@ +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +import logging +import time +from copy import deepcopy +from math import radians +from typing import Dict, List, Optional, Tuple, Union + +import cv2 +import numpy as np +from calibration_helpers import CalibrationResults + +logging.basicConfig( + level=logging.INFO, +) + +logger = logging.getLogger(__name__) + +SPOT_DEFAULT_ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) +OPENCV_VERSION = tuple(map(int, cv2.__version__.split("."))) +OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION = (4, 7, 0) + + +def create_charuco_board( + num_checkers_width: int, + num_checkers_height: int, + checker_dim: float, + marker_dim: float, + aruco_dict: cv2.aruco_Dictionary, + legacy: bool = True, +) -> cv2.aruco_CharucoBoard: + """ + Create a Charuco board using the provided parameters and Aruco dictionary. + Issues a deprecation warning if using the older 'CharucoBoard_create' method. + + Args: + num_checkers_width (int): Number of checkers along the width of the board. + num_checkers_height (int): Number of checkers along the height of the board. + checker_dim (float): Size of the checker squares. + marker_dim (float): Size of the Aruco marker squares. + aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary to use for marker generation. + + Returns: + charuco (cv2.aruco_CharucoBoard): The generated Charuco board. + """ + + opencv_version = tuple(map(int, cv2.__version__.split("."))) + + if opencv_version < (4, 7, 0): + logger.warning( + ( + "Creating Charuco Board..." + "You're using an older version of OpenCV requires the additional OpenCV Modules. " + "This will not work without the additional modules (opencv-contrib-python). " + "Consider upgrading to OpenCV >= 4.7.0 to enable " + "the use of this tool with base opencv (opencv-python) " + "without relying on additional modules." + ), + ) + + # Create Charuco board using the older method + charuco = cv2.aruco.CharucoBoard_create( + num_checkers_width, + num_checkers_height, + checker_dim, + marker_dim, + aruco_dict, + ) + + else: + # Create Charuco board using the newer method + charuco = cv2.aruco_CharucoBoard( + (num_checkers_width, num_checkers_height), + checker_dim, + marker_dim, + aruco_dict, + ) + charuco.setLegacyPattern(legacy) + + return charuco + + +SPOT_DEFAULT_CHARUCO = create_charuco_board( + num_checkers_width=9, + num_checkers_height=4, + checker_dim=0.115, + marker_dim=0.09, + aruco_dict=SPOT_DEFAULT_ARUCO_DICT, + legacy=True, +) + + +def multistereo_calibration_charuco( + images: np.ndarray, + desired_stereo_pairs: Optional[List[Tuple[int, int]]] = None, + charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, + aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, + camera_matrices: Optional[Dict] = {}, + dist_coeffs: Optional[Dict] = {}, + poses: Union[np.ndarray, None] = None, +) -> Dict: + """ + Calibrates the intrinsic and extrinsic parameters for multiple stereo camera pairs + using Charuco board markers. This function performs stereo calibration on a series of + time-synchronized images + captured by multiple cameras. It uses a Charuco board for calibration and returns + the calibrated intrinsic and extrinsic parameters for the specified stereo camera pairs. + + Args: + images (np.ndarray): A 2D array of time-synchronized camera captures. The array should + be of shape (n, m), where 'n' represents the number of time-synchronized captures + and 'm' represents the number of cameras. Each internal list corresponds to a set + of images captured at the same time across multiple cameras. + + desired_stereo_pairs (Optional[List[Tuple[int, int]]], optional): A list of tuples + specifying the pairs of camera indices to be calibrated. The indices refer to the + cameras based on their positions in the `images` array. For example, index `0` + corresponds to the first camera, index `1` to the second camera, and so forth, + up to `n-1` (where `n` is the number of cameras). + If None, all cameras are registered to the first camera (index `0`). Defaults to None. + + The relationship between `desired_stereo_pairs` and `images` is crucial. Each tuple in + `desired_stereo_pairs` should be in the form + `(origin_camera_idx, reference_camera_idx)`, + where `origin_camera_idx` and `reference_camera_idx` correspond to the + indices of the cameras in the `images` array. For each stereo pair, + the function attempts to calibrate the `origin_camera_idx` + relative to the `reference_camera_idx`. If no pairs are provided, + the function defaults to registering + all cameras to the first camera (index `0`). + + charuco_board (cv2.aruco_CharucoBoard, optional): The Charuco board configuration used + for calibration. Defaults to SPOT_DEFAULT_CHARUCO. + + aruco_dict (cv2.aruco_Dictionary, optional): The Aruco dictionary used + to detect the markers. Defaults to SPOT_DEFAULT_ARUCO_DICT. + + camera_matrices (Optional[Dict], optional): A dictionary mapping camera indices + to their existing camera matrices. If a matrix is not provided for a camera, + it will be computed during calibration + if that camera is part of a stereo pair. Defaults to an empty dictionary. + + dist_coeffs (Optional[Dict], optional): A dictionary mapping camera indices + to their distortion coefficients. + If coefficients are not provided for a camera, they will be computed during calibration. + Defaults to an empty dictionary. + poses (Union[np.ndarray, None]): Either a list of 4x4 homogenous transforms from which + pictures where taken, or None if unknown. Needs to be supplied for robot to camera cal. + (planning frame to base frame), or None + + Raises: + ValueError: Raised if fewer than two cameras are provided, as stereo calibration + requires at least two cameras. + ValueError: Raised if a stereo pair includes the same camera twice. + ValueError: Raised if a camera in a stereo pair cannot be calibrated. + + Returns: + Dict: A dictionary containing the calibrated intrinsic and extrinsic parameters + for the specified stereo pairs. + The keys are formatted as "origin_camera_idx_reference_camera_idx", + and the values contain the calibration data. + + """ + calibration = {} + num_cams = images.shape[1] + if num_cams == 0 or num_cams == 1: + raise ValueError("Stereo Madness requires more than one camera. Use the monocular method instead.") + else: # More than one camera - Multi-Stereo Madness ;p + for idx in range(num_cams): + if idx not in camera_matrices: + camera_matrices[idx] = None + if idx not in dist_coeffs: + dist_coeffs[idx] = None + + if desired_stereo_pairs is None: + desired_stereo_pairs = [] + for idx in range(1, num_cams): + """ + Desired stereo pairs is a mapping, where the 0th index is the origin (base) + frame, where the reference frame is defined within the origin (base) frame's + coordinate system. The reference frame is the 1st index. + + By Default, if desired_stereo_pairs is None, + then each camera's intrinsic is defined in the 0th camera's frame, + so this is set as the origin for all stereo calibrations + """ + desired_stereo_pairs.append((0, idx)) # just register all in 0th cam frame + + for idx, pair in enumerate(desired_stereo_pairs): + for index in pair: + if index < 0 or index > num_cams: + raise ValueError( + f"Requested to stereo register non-existent camera index {index} out of {num_cams}" + ) + origin_camera_idx = pair[0] + reference_camera_idx = pair[1] + + if pair[0] == pair[1]: + logging.warning( + "You requested to register a camera to itself. Ignoring,and continuing to the next stereo pair." + ) + continue + logging.info( + f"Attempting to register {origin_camera_idx} " + f"to {reference_camera_idx}, pair {idx} of " + f" {len(desired_stereo_pairs)}" + ) + try: + key = str(origin_camera_idx) + "_" + str(reference_camera_idx) + stereo_dict = stereo_calibration_charuco( + parent_images={str(i): img for i, img in enumerate(images[:, origin_camera_idx])}, + child_images={str(i): img for i, img in enumerate(images[:, reference_camera_idx])}, + charuco_board=charuco_board, + aruco_dict=aruco_dict, + camera_matrix_parent=camera_matrices[origin_camera_idx], + dist_coeffs_parent=dist_coeffs[origin_camera_idx], + camera_matrix_child=camera_matrices[reference_camera_idx], + dist_coeffs_child=dist_coeffs[reference_camera_idx], + poses=poses, + ) + camera_matrices[origin_camera_idx] = stereo_dict["camera_matrix_origin"] + dist_coeffs[origin_camera_idx] = stereo_dict["dist_coeffs_origin"] + camera_matrices[reference_camera_idx] = stereo_dict["camera_matrix_reference"] + dist_coeffs[reference_camera_idx] = stereo_dict["dist_coeffs_reference"] + + calibration[key] = stereo_dict + except ValueError as ve: # maybe could keep going here instead for partial calibration? + raise ValueError(f"Failed to calibrate pair {pair} : {ve}") + + return calibration + + +def get_correlation_map_to_enforce_ascending_ids_from_bottom_left_corner( + all_charuco_corners: List, + all_charuco_ids: List, + charuco_board: cv2.aruco_CharucoBoard, +) -> List: + """ + This is needed only for OpenCV versions > 4.7.0. Basically, + this determines the needed correlation + to ensure that internal corners are numbered left to right bottom + up as opposed to left to right top down. + + For more info see https://github.com/opencv/opencv/issues/26126 + + Args: + all_charuco_corners (List): All charuco corners of the board. + all_charuco_ids (List): All charuco ids of the board, as returned by the Charuco detector + for an ideal board with a full view. Expected to be a list ascending + from 0 but could be otherwise + charuco_board (cv2.aruco_CharucoBoard): the charuco board to utilize + to construct the correlation map + + Returns: + List: the correlation map where the indicies represent to the original + ordering of corners, and the values at each index represent the new + ordering index of a corner. Can be used to ensure bottom up + left to right ordering of internal corners. + """ + + num_checker_width, num_checker_height = charuco_board.getChessboardSize() + num_interior_corners = (num_checker_width - 1) * (num_checker_height - 1) + correlation_map = np.array([i for i in range(num_interior_corners)]) + # Adjust indexing to account for nested arrays + first_corner_height = all_charuco_corners[all_charuco_ids[0][0]][0][1] + last_corner_height = all_charuco_corners[all_charuco_ids[-1][0]][0][1] + row_num_a = 0 + row_num_b = num_checker_height - 2 + + if first_corner_height < last_corner_height: + logger.warning( + "Detected Charuco Configuration " + "where internal corners (detected checker corners) are numbered top down, " + "(left to right) as opposed to bottom up (left to right). " + "Enforcing bottom up numbering instead. " + "This ensures that the Z axis points out of the board " + "As opposed to -180 degrees about the X axis " + "relative to the Z out of the board" + ) + else: + logger.warning( + "You have selected to enforce ascending ids from the bottom left corner " + "But it seems as if your ids are already in that form " + "Returning identity correlation map" + ) + return [int(mapping) for mapping in correlation_map] + + while row_num_a < row_num_b: + row_num_a_correlation_slice = slice( + row_num_a * (num_checker_width - 1), (row_num_a * (num_checker_width - 1) + (num_checker_width - 1)), 1 + ) + + row_num_b_correlation_slice = slice( + row_num_b * (num_checker_width - 1), ((row_num_b * (num_checker_width - 1)) + (num_checker_width - 1)), 1 + ) + # Record A + precopy_row_a = deepcopy(correlation_map[row_num_a_correlation_slice]) + # Copy B onto A + correlation_map[row_num_a_correlation_slice] = correlation_map[row_num_b_correlation_slice] + # copy old A into B + correlation_map[row_num_b_correlation_slice] = precopy_row_a + row_num_a += 1 + row_num_b -= 1 + return [int(mapping) for mapping in correlation_map] + + +def detect_charuco_corners( + img: np.ndarray, + charuco_board: cv2.aruco_CharucoBoard, + aruco_dict: cv2.aruco_Dictionary, + enforce_ascending_ids_from_bottom_left_corner: Union[bool, None] = None, +) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]: + """ + Detect the Charuco Corners and their IDs in an image, with support for OpenCV versions before and after 4.7.0. + + Args: + img (np.ndarray): The image to find Charuco corners in. + charuco_board (cv2.aruco_CharucoBoard, optional): The Charuco board to look for. + Defaults to SPOT_DEFAULT_CHARUCO. + aruco_dict (cv2.aruco_Dictionary, optional): The Aruco dictionary to use. + Defaults to SPOT_DEFAULT_ARUCO_DICT. + enforce_ascending_ids_from_bottom_left_corner (Union[bool, None]): whether to + ensure that internal charuco corners are numbered left to right bottom up + (only ensures bottom up, assumes already left to right). + + Returns: + Tuple[Optional[np.ndarray], Optional[np.ndarray]]: The detected corners and their IDs, + or (None, None) if not found. + """ + # Convert the image to grayscale if it's not already + if len(img.shape) == 2: + gray = img + else: + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: + # Older OpenCV version + corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict) + if ids is not None: + _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco( + corners, ids, gray, charuco_board, minMarkers=0 + ) + return charuco_corners, charuco_ids + else: + return None, None + else: + # Newer OpenCV version with charuco_detector + detector_params = cv2.aruco.CharucoParameters() + detector_params.minMarkers = 0 + detector_params.tryRefineMarkers = True + charuco_detector = cv2.aruco.CharucoDetector(charuco_board, detector_params) + charuco_detector.setBoard(charuco_board) + charuco_corners, charuco_ids, _, _ = charuco_detector.detectBoard(gray) + + if charuco_ids is None: + return None, None + + enforce_ids = enforce_ascending_ids_from_bottom_left_corner + if enforce_ids is not None and hasattr(detect_charuco_corners, "enforce_ids"): + logger.warning( + "Previously, for detecting internal charuco corners, the enforce " + "ascending from bottom left corner id policy was set to: " + f"{detect_charuco_corners.enforce_ids}" + f"it will now be set to {enforce_ids}" + ) + detect_charuco_corners.enforce_ids = enforce_ids + elif enforce_ids is not None and not hasattr(detect_charuco_corners, "enforce_ids"): + logger.warning( + "For detecting charuco corners, the enforce " + "ascending from bottom left corner id policy is set to: " + f"{enforce_ids}. For this call, and future calls until set otherwise." + ) + detect_charuco_corners.enforce_ids = enforce_ids + + # Create the identity correlation map... + num_checker_width, num_checker_height = charuco_board.getChessboardSize() + num_interior_corners = (num_checker_width - 1) * (num_checker_height - 1) + correlation_map = np.array([i for i in range(num_interior_corners)]) + + if ( + hasattr(detect_charuco_corners, "enforce_ids") + and detect_charuco_corners.enforce_ids + and not hasattr(detect_charuco_corners, "corr_map") + ): # correlation map not computed + ideal_charuco = charuco_board.generateImage(outSize=(1000, 1000)) + all_charuco_corners, all_charuco_ids, _, _ = charuco_detector.detectBoard(ideal_charuco) + + detect_charuco_corners.corr_map = get_correlation_map_to_enforce_ascending_ids_from_bottom_left_corner( + all_charuco_corners, all_charuco_ids, charuco_board + ) + + if ( + hasattr(detect_charuco_corners, "enforce_ids") + and detect_charuco_corners.enforce_ids + and hasattr(detect_charuco_corners, "corr_map") + ): # correlation map computed + logger.warning("Using cached correlation map to order IDs") + correlation_map = detect_charuco_corners.corr_map # grab the map + + reworked_charuco_ids = [] + for charuco_id in charuco_ids: + reworked_charuco_ids.append([correlation_map[charuco_id[0]]]) + + return charuco_corners, reworked_charuco_ids + + +def get_charuco_board_object_points( + charuco_board: cv2.aruco_CharucoBoard, + corners_ids: Union[List, np.ndarray], +) -> np.ndarray: + """ + From a charuco board, and corner ids, generate the object points for use + in a calibration sequence to solve the perspective and point problem. + + Args: + charuco_board (cv2.aruco_CharucoBoard): the charuco board + to generate object points for + corners_ids (Union[List, np.ndarray]): which corner ids to generate points for + + Returns: + np.ndarray: the object points + """ + if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: + corners = charuco_board.chessboardCorners + else: + corners = charuco_board.getChessboardCorners() + object_points = [] + for idx in corners_ids: + object_points.append(corners[idx]) + return np.array(object_points, dtype=np.float32) + + +def calibrate_single_camera_charuco( + images: List[np.ndarray], + charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, + aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, + debug_str: str = "", +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """ + Calibrate a monocular camera from a charuco board. + + Args: + images (List[np.ndarray]): A list of images of the board + charuco_board (cv2.aruco_CharucoBoard, optional): which + board to look for in the images. Defaults to SPOT_DEFAULT_CHARUCO. + aruco_dict (cv2.aruco_Dictionary, optional): which + aruco dict to look for in the images. Defaults to SPOT_DEFAULT_ARUCO_DICT. + debug_str (str, optional): what to add to the logging + to differentiate this monocular calibration from others, for the sake + of stereo calibrations. Defaults to "". + + Raises: + ValueError: Not enough data to calibrate + + Returns: + Tuple[np.ndarray, np.ndarray]: the camera matrix, distortion coefficients, + rotation matrices, tvecs + """ + all_corners = [] + all_ids = [] + img_size = None + for idx, img in enumerate(images): + if img_size is None: + img_size = img.shape[:2][::-1] + + charuco_corners, charuco_ids = detect_charuco_corners(img, charuco_board, aruco_dict) + + if charuco_corners is not None and len(charuco_corners) >= 8: + all_corners.append(charuco_corners) + all_ids.append(charuco_ids) + else: + logger.warning(f"Not enough corners detected in image index {idx} {debug_str}; ignoring") + + if len(all_corners) > 1: + obj_points_all = [] + img_points = [] + ids = [] + for corners, ids in zip(all_corners, all_ids): + obj_points = get_charuco_board_object_points(charuco_board, ids) + obj_points_all.append(obj_points) + img_points.append(corners) + logger.info(f"About to process {len(obj_points_all)} points for single camera cal | {debug_str}") + # Long term improvement: could pull tvec for use to localize camera relative to robot? + _, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( # use LU flag critical for speed + obj_points_all, + img_points, + img_size, + None, + None, + flags=cv2.CALIB_USE_LU, + ) + rmats = np.array([cv2.Rodrigues(rvec)[0] for rvec in rvecs]) + return camera_matrix, dist_coeffs, rmats, tvecs + else: + raise ValueError(f"Not enough valid points to individually calibrate {debug_str}") + + +def stereo_calibration_charuco( + parent_images: Dict[str, np.ndarray], + child_images: Dict[str, np.ndarray], + charuco_board: cv2.aruco_CharucoBoard = SPOT_DEFAULT_CHARUCO, + aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, + camera_matrix_parent: Optional[np.ndarray] = None, + dist_coeffs_parent: Optional[np.ndarray] = None, + camera_matrix_child: Optional[np.ndarray] = None, + dist_coeffs_child: Optional[np.ndarray] = None, + poses: Union[np.ndarray, None] = None, +) -> CalibrationResults: + """ + Perform a stereo calibration from a set of synchronized stereo images of a charuco calibration + board. + + Args: + parent_images (List[np.ndarray]): A list of images synced to reference images from camera 0 + child_images (List[np.ndarray]): A list of images synced to origin images from camera 1 + charuco_board (cv2.aruco_CharucoBoard, optional): What charuco board to + use for the cal. Defaults to SPOT_DEFAULT_CHARUCO. + aruco_dict (cv2.aruco_Dictionary, optional): What aruco board to use for the cal. + Defaults to SPOT_DEFAULT_ARUCO_DICT. + camera_matrix_parent (Optional[np.ndarray], optional): What camera + matrix to assign to camera 0. If none, is computed. Defaults to None. + dist_coeffs_parent (Optional[np.ndarray], optional): What distortion coefficients + to assign to camera 0. If None, is computed. Defaults to None. + projection_matrix_origin (Optional[np.ndarray], optional): What projection + matrix to assign to camera 0. If None, is computed. Defaults to None. + camera_matrix_child (Optional[np.ndarray], optional): What camera + matrix to assign to camera 1. If none, is computed. . Defaults to None. + dist_coeffs_child (Optional[np.ndarray], optional): What distortion coefficients + to assign to camera 1. If None, is computed. Defaults to None. + poses (Union[np.ndarray, None]): Either a list of 4x4 homogenous transforms from which + pictures where taken, or None if unknown. Needs to be supplied for robot to camera cal. + (planning frame to base frame), or None + Raises: + ValueError: Could not calibrate a camera individually + ValueError: Not enough points to stereo calibrate + ValueError: Could not stereo calibrate + + Returns: + Dict: _description_ + """ + camera_child: np.ndarray = np.eye(3) + coeffs_child: np.ndarray = np.zeros((5,)) + camera_parent: np.ndarray = np.eye(3) + coeffs_parent: np.ndarray = np.zeros((5,)) + + # Pre-filter parent images to frames with enough corners, and simultaneously + # filter poses so they remain aligned with the rmats/tvecs returned by + # calibrate_single_camera_charuco (which silently drops frames with < 8 corners). + valid_parent_imgs: Dict[str, np.ndarray] = {} + poses_aligned_with_parent: Optional[List] = [] if poses is not None else None + for i, (fname, img) in enumerate(parent_images.items()): + corners, _ = detect_charuco_corners(img, charuco_board, aruco_dict) + if corners is not None and len(corners) >= 8: + valid_parent_imgs[fname] = img + if poses is not None: + poses_aligned_with_parent.append(poses[i]) + + # first do single camera cal for parent. rotation and tvecs are used, and camera matrix, dist coeffs + logger.info("Calibrating Parent Camera individually") + (camera_matrix_parent_new, dist_coeffs_parent_new, rmats_origin, tvecs_origin) = calibrate_single_camera_charuco( + images=valid_parent_imgs.values(), + charuco_board=charuco_board, + aruco_dict=aruco_dict, + debug_str="for parent camera", + ) + + if camera_matrix_parent_new is None: + raise ValueError("Could not obtain cam matrix parent camera to charuco board, needed for pose") + if dist_coeffs_parent_new is None: + raise ValueError("Could not obtain dist coeffs of parent camera to charuco board, needed for pose") + + # if None passed in, use these newly computed values + if camera_matrix_parent is None: + camera_parent = camera_matrix_parent_new + else: + camera_parent = camera_matrix_parent + if dist_coeffs_parent is None: + coeffs_parent = dist_coeffs_parent_new + else: + coeffs_parent = dist_coeffs_parent + + # next do single camera cal for child. rotation and tvecs are used, and camera matrix, dist coeffs + start_time = time.perf_counter() + logger.info("Calibrating Child Camera individually") + (camera_matrix_child_new, dist_coeffs_child_new, _, _) = calibrate_single_camera_charuco( + images=child_images.values(), + charuco_board=charuco_board, + aruco_dict=aruco_dict, + debug_str="for child camera", + ) + elapsed_time = time.perf_counter() - start_time + logger.info(f"Finished calibration child camera in {elapsed_time:.4f} seconds") + + if camera_matrix_child_new is None: + raise ValueError("Could not obtain cam matrix child camera to charuco board, needed for pose") + if dist_coeffs_child_new is None: + raise ValueError("Could not obtain dist coeffs of child camera to charuco board, needed for pose") + + logger.info("Finished individual camera calibrations, starting stereo calibration") + + # if None passed in, use these newly computed values + if camera_matrix_child is None: + camera_child = camera_matrix_child_new + else: + camera_child = camera_matrix_child + if dist_coeffs_child is None: + coeffs_child = dist_coeffs_child_new + else: + coeffs_child = dist_coeffs_child + + all_corners_parent = [] + all_corners_child = [] + all_ids_parent = [] + all_ids_child = [] + parent_img_size = None + child_img_size = None + + # let gets corresponding images + parent_keys = set(parent_images.keys()) + child_keys = set(child_images.keys()) + common_keys = parent_keys.intersection(child_keys) + + logger.info("Collecting shared points for stereo calibration") + if len(common_keys) == 0: + raise ValueError("No common images between parent and child cameras for stereo calibration") + + for fname in list(common_keys): + origin_img = parent_images[fname] + reference_img = child_images[fname] + if parent_img_size is None: + parent_img_size = origin_img.shape[:2][::-1] + if child_img_size is None: + child_img_size = reference_img.shape[:2][::-1] + + origin_charuco_corners, origin_charuco_ids = detect_charuco_corners(origin_img, charuco_board, aruco_dict) + reference_charuco_corners, reference_charuco_ids = detect_charuco_corners( + reference_img, charuco_board, aruco_dict + ) + + if origin_charuco_corners is not None and reference_charuco_corners is not None: + all_corners_parent.append(origin_charuco_corners) + all_corners_child.append(reference_charuco_corners) + all_ids_parent.append(origin_charuco_ids) + all_ids_child.append(reference_charuco_ids) + + if len(all_corners_parent) == 0: + raise ValueError("Not enough shared points for stereo calibration.") + logger.info("about to find common ids...") + obj_points_all = [] + img_points_origin = [] + img_points_reference = [] + for ( + origin_corners, + reference_corners, + origin_ids, + reference_ids, + ) in zip( + all_corners_parent, + all_corners_child, + all_ids_parent, + all_ids_child, + strict=True, + ): + common_ids = np.intersect1d(origin_ids, reference_ids) # sorted flat array + if len(common_ids) >= 6: # Ensure there are at least 6 points + obj_points = get_charuco_board_object_points(charuco_board, common_ids) + # Build id->corner-index maps so we can look up corners in the same + # sorted order as common_ids / obj_points. Using np.isin preserves + # the original detection order, which may differ from intersect1d's + # sorted order, causing a 2D-3D correspondence mismatch → NaN R/T. + origin_id_to_idx = {int(np.ravel(id_)[0]): i for i, id_ in enumerate(origin_ids)} + reference_id_to_idx = {int(np.ravel(id_)[0]): i for i, id_ in enumerate(reference_ids)} + selected_origin = np.array( + [origin_corners[origin_id_to_idx[int(cid)]] for cid in common_ids], dtype=np.float32 + ) + selected_reference = np.array( + [reference_corners[reference_id_to_idx[int(cid)]] for cid in common_ids], dtype=np.float32 + ) + obj_points_all.append(obj_points) + img_points_origin.append(selected_origin) + img_points_reference.append(selected_reference) + + # sanity check + if len(obj_points_all) == 0: + raise ValueError("Not enough valid points for stereo calibration.") + + logger.info( + f"Collected {len(obj_points_all)} shared point sets for stereo calibration. Starting stereo calibration..." + ) + # Compute the stereo extrinsic (parent-to-child) via per-view PnP rather than + # cv2.stereoCalibrate, which is prone to NaN when the two cameras have very + # different resolutions or when the IR/depth images produce noisy detections + # that destabilise the Essential-matrix initialisation used internally by + # stereoCalibrate. Per-view PnP + SVD averaging is more robust. + start_time = time.perf_counter() + R_rel_list: List[np.ndarray] = [] + T_rel_list: List[np.ndarray] = [] + reproj_errors: List[float] = [] + for op, ip_parent, ip_child in zip(obj_points_all, img_points_origin, img_points_reference): + ok_p, rvec_p, tvec_p = cv2.solvePnP(op, ip_parent, camera_parent, coeffs_parent, flags=cv2.SOLVEPNP_ITERATIVE) + ok_c, rvec_c, tvec_c = cv2.solvePnP(op, ip_child, camera_child, coeffs_child, flags=cv2.SOLVEPNP_ITERATIVE) + if not ok_p or not ok_c: + logger.warning("solvePnP failed for a view; skipping.") + continue + R_p, _ = cv2.Rodrigues(rvec_p) + R_c, _ = cv2.Rodrigues(rvec_c) + # child_R_parent, child_T_parent (X_child = R @ X_parent + T) + R_i = R_c @ R_p.T + T_i = tvec_c.flatten() - R_i @ tvec_p.flatten() + R_rel_list.append(R_i) + T_rel_list.append(T_i) + # per-view reprojection error in parent camera for a rough quality metric + proj, _ = cv2.projectPoints(op, rvec_p, tvec_p, camera_parent, coeffs_parent) + reproj_errors.append(float(np.mean(np.linalg.norm(proj.reshape(-1, 2) - ip_parent.reshape(-1, 2), axis=1)))) + + if len(R_rel_list) < 3: + raise ValueError( + f"Only {len(R_rel_list)} views produced valid PnP solutions; need at least 3 for stereo extrinsic." + ) + + T_arr = np.array(T_rel_list) # (N, 3) + R_arr = np.array(R_rel_list) # (N, 3, 3) + + # --- outlier rejection on T --- + # Each T_i = t_child - R_i @ t_parent. Individual tvec values can be O(1 m), + # so a few noisy IR-camera PnP solutions cause catastrophic cancellation and + # produce wildly large T_i. Use the per-component MAD to find and discard them. + T_median = np.median(T_arr, axis=0) + abs_dev = np.abs(T_arr - T_median) # (N, 3) + mad = np.median(abs_dev, axis=0) # (3,) + mad = np.where(mad < 1e-9, 1e-9, mad) # avoid division by zero + # A view is an inlier only if every component is within 5 MADs of the median + inlier_mask = np.all(abs_dev / mad < 5.0, axis=1) + n_inliers = int(inlier_mask.sum()) + if n_inliers < 3: + logger.warning(f"Outlier rejection left only {n_inliers} T inliers; using all {len(T_arr)} views.") + inlier_mask = np.ones(len(T_arr), dtype=bool) + + T_inliers = T_arr[inlier_mask] + R_inliers = R_arr[inlier_mask] + reproj_inliers = [e for e, ok in zip(reproj_errors, inlier_mask) if ok] + logger.info(f"Stereo extrinsic: {n_inliers}/{len(T_arr)} views kept after outlier rejection.") + + # Robust translation: median of inliers + T = np.median(T_inliers, axis=0).reshape(3, 1) + + # Average rotation matrices over inliers and project back to SO(3) via SVD + R_mean = np.mean(R_inliers, axis=0) + U, _, Vt = np.linalg.svd(R_mean) + R = U @ Vt + if np.linalg.det(R) < 0: # fix improper rotation (reflection) + R = U @ np.diag([1.0, 1.0, -1.0]) @ Vt + + err = float(np.mean(reproj_inliers)) # mean per-view reprojection error (parent camera) + elapsed_time = time.perf_counter() - start_time + logger.info( + f"Stereo extrinsic estimated from {n_inliers} inlier views in {elapsed_time:.4f} s, " + f"mean parent reproj error: {err:.3f} px." + ) + + # now we will estimate these + camera_to_robot_R = np.eye(3) # Extract rotation + camera_to_robot_T = np.eye(3) # Extract translation + + if poses is not None: + logger.info("Attempting hand-eye calibation....") + # poses_aligned_with_parent was pre-filtered above to match rmats_origin/tvecs_origin + filtered_poses = np.array(poses_aligned_with_parent) + # Use the filtered poses for the target-to-camera transformation + R_gripper2base = np.array([pose[:3, :3] for pose in filtered_poses]) + t_gripper2base = np.array([pose[:3, 3] for pose in filtered_poses]) + + R_handeye, T_handeye = cv2.calibrateHandEye( + R_gripper2base=R_gripper2base, + t_gripper2base=t_gripper2base, + R_target2cam=rmats_origin, + t_target2cam=tvecs_origin, + method=cv2.CALIB_HAND_EYE_DANIILIDIS, + ) + robot_to_cam = np.eye(4) # Initialize 4x4 identity matrix + robot_to_cam[:3, :3] = R_handeye # Populate rotation + robot_to_cam[:3, 3] = T_handeye.flatten() # Populate translation + + # Invert the homogeneous matrix + cam_to_robot = np.linalg.inv(robot_to_cam) + + # Extract rotation and translation from the inverted matrix + camera_to_robot_R = cam_to_robot[:3, :3] # Extract rotation + camera_to_robot_T = cam_to_robot[:3, 3] # Extract translation + logger.info("Hand-eye calibration completed.") + + # save off our work + calibration_results: CalibrationResults = { + "camera_matrix_reference": camera_child, + "dist_coeffs_reference": coeffs_child, + "dist_coeffs_origin": coeffs_parent, + "camera_matrix_origin": camera_parent, + "image_dim_origin": np.array(parent_img_size), + "image_dim_reference": np.array(child_img_size), + "R": R, + "T": T, + "R_handeye": camera_to_robot_R, + "T_handeye": camera_to_robot_T, + "average_reprojection_error": float(np.linalg.norm(err)), + } + return calibration_results + + +def est_camera_t_charuco_board_center( + img: np.ndarray, + charuco_board: cv2.aruco_CharucoBoard, + aruco_dict: cv2.aruco_Dictionary, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, +) -> Tuple[np.ndarray, np.ndarray]: + """ + Localizes the 6D pose of the checkerboard center using Charuco corners with OpenCV's solvePnP. + + The board pose's translation should be at the center of the board, with the orientation in OpenCV format, + where the +Z points out of the board with the other axes being parallel to the sides of the board. + + Args: + img (np.ndarray): The input image containing the checkerboard. + charuco_board (cv2.aruco_CharucoBoard): The Charuco board configuration. + aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary used to detect markers. + camera_matrix (np.ndarray): The camera matrix from calibration. + dist_coeffs (np.ndarray): The distortion coefficients from calibration. + + Returns: + Optional[Tuple[np.ndarray, np.ndarray]]: The rotation vector and translation vector + representing the 6D pose of the checkerboard center if found, else None. + """ + charuco_corners, charuco_ids = detect_charuco_corners(img, charuco_board, aruco_dict) + if charuco_corners is not None and charuco_ids is not None: + object_points = get_charuco_board_object_points(charuco_board, charuco_ids) + image_points = charuco_corners + + # Use solvePnP to estimate the pose of the Charuco board + success, rvec, tvec = cv2.solvePnP(object_points, np.array(image_points), camera_matrix, dist_coeffs) + + if success: + # Convert to the camera frame: Adjust the translation vector to be relative to the center of the board + x_trans = (charuco_board.getSquareLength() * charuco_board.getChessboardSize()[0]) / 2.0 + y_trans = (charuco_board.getSquareLength() * charuco_board.getChessboardSize()[1]) / 2.0 + center_trans = np.array([x_trans, y_trans, 0.0]).reshape((3, 1)) + rmat, _ = cv2.Rodrigues(rvec) + + tvec = tvec + rmat.dot(center_trans) + tvec_to_camera = tvec + return np.array(rmat), np.array(tvec_to_camera).ravel() + else: + raise ValueError("Pose estimation failed. You likely primed the robot too close to the board.") + else: + raise ValueError( + "Couldn't detect any Charuco boards in the image, " + "localization failed. Ensure the board is visible from the" + " primed pose." + ) + + +def convert_camera_t_viewpoint_to_origin_t_planning_frame( + origin_t_planning_frame: np.ndarray = np.eye(4), + planning_frame_t_opencv_camera: np.ndarray = np.eye(4), + opencv_camera_t_viewpoint: np.ndarray = np.eye(4), +) -> np.ndarray: + """ + Convert a camera viewpoint given in a camera frame, to the planning frame. + + Camera frame should be in OpenCV/ ROS format, where + +x should point to the right in the image + +y should point down in the image + +z should point into to plane of the image. + + Assuming that origin_t_planning_frame isn't aligned with this format, align it + so that the viewpoint can be localized as a hand pose command. + + Args: + origin_t_planning_frame (np.ndarray, optional): 4x4 homogenous transform + from origin to planning frame. Defaults to np.eye(4). + planning_frame_t_opencv_camera (np.ndarray, optional): 4x4 homogenous transform + from planning frame to the camera. Defaults to np.eye(4). + opencv_camera_t_viewpoint (np.ndarray, optional): 4x4 homogenous transform + from camera to the future viewpoint. Defaults to np.eye(4). + + Returns: + np.ndarray: 4x4 homogenous transform of the planning frame of a future viewpoint + expressed in the origin frame. + """ + origin_t_opencv_camera_pose = origin_t_planning_frame @ planning_frame_t_opencv_camera + origin_t_viewpoint = origin_t_opencv_camera_pose @ opencv_camera_t_viewpoint + origin_t_planning_frame_next = origin_t_viewpoint @ np.linalg.inv(planning_frame_t_opencv_camera) + return origin_t_planning_frame_next + + +def get_relative_viewpoints_from_board_pose_and_param( + R_board: np.ndarray, + tvec: np.ndarray, + distances_x: Optional[np.ndarray] = None, + distances_z: Optional[np.ndarray] = None, + x_axis_rots: Optional[np.ndarray] = None, + y_axis_rots: Optional[np.ndarray] = None, + z_axis_rots: Optional[np.ndarray] = None, + R_align_board_frame_with_camera: Optional[np.ndarray] = None, + degree_offset_rotations: bool = True, +) -> List[np.ndarray]: + """ + Given the position of charuco board, sample viewpoints facing the calibration board for + the robot's "principal" camera to visit. When the robot moves to these viewpoints, + it takes synchronized photos with all cameras to use for solving the calibration. + + ROS and OpenCV have the same camera format, where: + # +x should point to the right in the image + # +y should point down in the image + # +z should point into to plane of the image. + + All transforms to cameras are in this format. + + Args: + R_board (np.ndarray): the rotation of the calibration board in opencv notation relative + to the "principal" camera. + (See AutomaticCameraCalibration.localize_target_to_principal_camera) + tvec (np.ndarray): the translation of the calibration board in opencv notation relative to + the principal camera + distances (np.ndarray, optional): What distances + away from the calibration board's pose (along the Z axis) + to sample calibration viewpoints from. Defaults to np.arange(0.5, 0.8, 0.1). + x_axis_rots (np.ndarray, optional): What + x-axis rotations relative to the camera viewing the board orthogonally + to apply to sample viewpoints. Defaults to np.arange(-21, 21, 5). + y_axis_rots (np.ndarray, optional): What + y-axis rotations relative to the camera viewing the board orthogonally + to apply to sample viewpoints. Defaults to np.arange(-21, 21, 5). + z_axis_rots (np.ndarray, optional): What + z-axis rotations relative to the camera viewing the board orthogonally + to apply to sample viewpoints. Defaults to np.array([-21, 21, 5]). + R_align_board_frame_with_camera (np.ndarray, optional): By default, + the charuco pose rotation Z-axis points out of the pattern, + and into the camera. However, to sample viewpoints, the camera must face the board, + so viewpoints can't be sampled directly from the board pose as they would face + away from the board. + This rotates the board frame such that the Z axis points away + from the pattern, so that camera frames facing the board may be sampled + after applying a distance and offset rotation. + Defaults to np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]). + degree_offset_rotations (bool, optional): whether to use degree for the + sampling rotation parameters. Defaults to True. + + Returns: + List[np.ndarray]: a list of 4x4 homogenous transforms to visit in the "principal" cameras + frame + """ + if distances_x is None: + distances_x = np.arange(0.3, 0.7, 0.1) + if distances_z is None: + distances_z = np.arange(-0.2, 0.3, 0.1) + if x_axis_rots is None: + x_axis_rots = np.arange(-20, 21, 5) + if y_axis_rots is None: + y_axis_rots = np.arange(-20, 21, 5) + if z_axis_rots is None: + z_axis_rots = np.arange(-20, 21, 5) + if R_align_board_frame_with_camera is None: + R_align_board_frame_with_camera = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) + + if degree_offset_rotations: + x_axis_rots = [radians(deg) for deg in x_axis_rots] + y_axis_rots = [radians(deg) for deg in y_axis_rots] + z_axis_rots = [radians(deg) for deg in z_axis_rots] + + translations = [(tvec + R_board[:, 2] * dist + R_board[:, 0] * d2) for dist in distances_z for d2 in distances_x] + R_base = R_board @ R_align_board_frame_with_camera + + def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: + """ + Convert Euler angles to a rotation matrix using OpenCV, for the sake + of CLI convenience, conciseness, while not depending on an external transform lib. + """ + R_x = cv2.Rodrigues(np.array([roll, 0, 0]))[0] + R_y = cv2.Rodrigues(np.array([0, pitch, 0]))[0] + R_z = cv2.Rodrigues(np.array([0, 0, yaw]))[0] + return R_z @ R_y @ R_x + + poses = [] + for t in translations: + for x_axis_rot in x_axis_rots: + for y_axis_rot in y_axis_rots: + for z_axis_rot in z_axis_rots: + # Get the rotation matrix from Euler angles + R_rot = euler_to_rotation_matrix(x_axis_rot, y_axis_rot, z_axis_rot) + + # Combine with the base rotation + R_base_modified = R_base @ R_rot + + transform = np.eye(4) + transform[:-1, -1] = t.reshape((3,)) + transform[:-1, :-1] = R_base_modified + poses.append(transform) + + logger.info(f"Calculated {len(poses)} relative viewpoints to visit relative to the target.") + return poses + + +def create_ideal_charuco_image(charuco_board: cv2.aruco_CharucoBoard, dim=(500, 700), colorful=False): + if OPENCV_VERSION < OPENCV_CHARUCO_LIBRARY_CHANGE_VERSION: + img = charuco_board.draw(outSize=dim) + else: + img = charuco_board.generateImage(outSize=dim) + if colorful: + return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + else: + return img + + +def charuco_pose_sanity_check( + img: np.ndarray, charuco_board: cv2.aruco_CharucoBoard, aruco_dict: cv2.aruco_Dictionary +) -> np.ndarray: + """ + Visualize the Charuco board pose detection on an image for sanity-checking. + + Uses an identity camera matrix and zero distortion to estimate and display + the 6D pose of the board center, overlaying 3D axes on the image. + + Args: + img (np.ndarray): The input image containing the Charuco board. + charuco_board (cv2.aruco_CharucoBoard): The Charuco board configuration. + aruco_dict (cv2.aruco_Dictionary): The Aruco dictionary used to detect markers. + + Returns: + img_with_axes (np.ndarray): The image with the pose axes drawn. + """ + + def is_z_axis_out_of_board(tvec: np.ndarray) -> bool: + """Determine if the Z-axis points out of the Charuco board (towards the camera).""" + return tvec[2] > 0 + + def visualize_pose_with_axis( + img: np.ndarray, + rmat: np.ndarray, + tvec: np.ndarray, + camera_matrix: np.ndarray, + dist_coeffs: np.ndarray, + axis_length: float = 0.115, + ) -> np.ndarray: + """Draws the 3D pose axes on the image and displays whether the Z-axis is out or into the board.""" + axis = np.float32([[axis_length, 0, 0], [0, axis_length, 0], [0, 0, axis_length], [0, 0, 0]]).reshape(-1, 3) + imgpts, _ = cv2.projectPoints(axis, rmat, tvec, camera_matrix, dist_coeffs) + z_out_of_board = is_z_axis_out_of_board(tvec) + img_with_axes = img.copy() + origin = tuple(imgpts[3].ravel().astype(int)) + img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[0].ravel().astype(int)), (0, 0, 255), 3) # X + img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[1].ravel().astype(int)), (0, 255, 0), 3) # Y + img_with_axes = cv2.line(img_with_axes, origin, tuple(imgpts[2].ravel().astype(int)), (255, 0, 0), 3) # Z + if not z_out_of_board: + logger.warning("This tool assumes that Z axis is out of board, but it was detected as into board.") + window_name = f'Charuco Board Pose ({"Z-axis out of board" if z_out_of_board else "Z-axis into board"})' + cv2.imshow(window_name, img_with_axes) + elapsed_time = 0 + wait_interval = 100 + max_wait_time = 20 + while elapsed_time < max_wait_time * 1000: + if cv2.getWindowProperty(window_name, cv2.WND_PROP_VISIBLE) < 1: + break + key = cv2.waitKey(wait_interval) + if key != -1: + break + elapsed_time += wait_interval + cv2.destroyAllWindows() + return img_with_axes + + camera_matrix = np.eye(3, dtype=np.float32) + dist_coeffs = np.zeros(5, dtype=np.float32) + rmat, tvec = est_camera_t_charuco_board_center(img, charuco_board, aruco_dict, camera_matrix, dist_coeffs) + return visualize_pose_with_axis(img, rmat, tvec, camera_matrix, dist_coeffs) diff --git a/spot_wrapper/calibration/registration_qualitative_example.jpg b/spot_wrapper/calibration/docs/registration_qualitative_example.jpg similarity index 100% rename from spot_wrapper/calibration/registration_qualitative_example.jpg rename to spot_wrapper/calibration/docs/registration_qualitative_example.jpg diff --git a/spot_wrapper/calibration/spot_eye_in_hand_setup.jpg b/spot_wrapper/calibration/docs/spot_eye_in_hand_setup.jpg similarity index 100% rename from spot_wrapper/calibration/spot_eye_in_hand_setup.jpg rename to spot_wrapper/calibration/docs/spot_eye_in_hand_setup.jpg diff --git a/spot_wrapper/calibration/in_depth_calibration_doc.md b/spot_wrapper/calibration/in_depth_calibration_doc.md deleted file mode 100644 index ae8524ea..00000000 --- a/spot_wrapper/calibration/in_depth_calibration_doc.md +++ /dev/null @@ -1,324 +0,0 @@ -# Automatic Robotic Stereo Camera Calibration Utility with Charuco Target (a.k.a Multi-Stereo Madness) - -### Recommended Setup - -![spot eye in hand cal](spot_eye_in_hand_setup.jpg) - -### Reference Image - -![side by side comparison](registration_qualitative_example.jpg) - -# Table of Contents - -1. [***Overview***](#overview) -2. [***Adapting Automatic Collection and Calibration to Your Scenario***](#adapting-automatic-data-collection-and-calibration-to-your-scenario) -3. [***Check if you have a Legacy Charuco Board***](#check-if-you-have-a-legacy-charuco-board) -4. [***Calibrate Spot Manipulator Eye-In-Hand Cameras With the CLI Tool***](#calibrate-spot-manipulator-eye-in-hand-cameras-with-the-cli-tool) - - [Robot and Target Setup](#robot-and-target-setup) - - [Example Usage](#example-usage-aka-hand-specific-live-incantations) - - [Improving Calibration Quality](#improving-calibration-quality) - - [Using the Registered Information with Spot ROS 2](#using-the-registered-information-with-spot-ros-2) -5. [***Using the CLI Tool To Calibrate On an Existing Dataset***](#using-the-cli-tool-to-calibrate-on-an-existing-dataset) -6. [***Understanding the Output Calibration Config File from the CLI***](#understanding-the-output-calibration-config-file-from-the-cli) -7. [Recreate the Core Calibration CLI Tool Without Depending On Spot Wrapper](#recreate-the-core-calibration-cli-tool-without-depending-on-spot-wrapper) - -# Overview - -This utility streamlines automatic -camera calibration to **solve for the intrinsic and extrinsic parameters for two or more -cameras mounted in a fixed pose relative to each other on a robot** -based off of moving the robot to view a Charuco target from different poses. If you already -have an existing dataset of synchronized stereo (or multi-stereo) photos of a Charuco target from different viewpoints, -you can use the CLI tool to compute the intrinsic/extrinsic parameters. Additionally, if you have saved the poses the images are taken at (as homogenous 4x4 transforms from the "world" frame [most likely the robot base] to the robot planning frame [most likely the -robot end-effector]), you can also calibrate -the camera to robot extrinsic (eye-to-hand registration). If you don't have a dataset, -you can use this tool both to generate the dataset and calibrate the cameras. - -The CLI tool's saving capability allows to store multiple unique calibration runs in one configuration file -with calibration metadata, to document related runs with different setups or parameters. - -This was developed to calibrate the two cameras at the -end of Spot's optional manipulator payload, while being **as general as possible -to be easily adapted to new robots and camera configurations. The core calibration utilities depend only on NumPy and OpenCV.** - -This utility works by sampling viewpoints relative to the Charuco calibration board, -visiting those viewpoints, and snapping photos with all cameras at each viewpoint. Then, -these images are used to calibrate the desired cameras both individually and with respect to each -other. This utility is particularly good at handling partial views of the board -due to it's use of a charuco board. For more info, see ```calibration_util.py```, where the functions -```get_multiple_perspective_camera_calibration_dataset``` and ```multistereo_calibration_charuco``` -do most of the heavy lifting. - -# Adapting Automatic Data Collection and Calibration to Your Scenario - -Assuming that you have a charuco board you'd like to automatically calibrate your cameras/robot with: - -**To calibrate a new robot with new cameras using the utility**, implement the abstract class ```AutomaticCameraCalibrationRobot``` from ```automatic_camera_calibration_robot.py``` -(Only five methods, that are likely analogous to what's needed for automatic calibration even if this utility isn't used. -in Spot's case , excluding comments, it's under ~250 lines of code, see ```spot_in_hand_camera_calibration.py```), -and pass the implemented class as an argument to ```get_multiple_perspective_camera_calibration_dataset``` from ```calibration_util.py``` (see ```calibrate_spot_hand_camera_cli.py``` for an example). - -**Adding a new camera to register with Spot's existing hand cameras** is as easy as adding a call to append -the new camera image in ```SpotInHandCalibration.capture_images``` in ```spot_in_hand_camera_calibration.py``` to the existing -list of images obtained with the default cameras (assuming that the new camera -is fixed relative to the existing cameras.). - -# Check if you have a Legacy Charuco Board - -You only need to do this if using an opencv version after ```4.7```( -check with```python3 -c "import cv2; print(cv2.__version__)"```) - -Through using the CLI tool (```python3 calibrate_multistereo_cameras_with_charuco_cli.py -h```), you can check if you have a legacy board through visually comparing the generated drawn virtual board to your physical charuco board target. Some legacy boards have an aruco tag in the top -left corner, whether as some non-legacy boards have a checker in the top left corner. -Also, check to see that the aruco tags match between virtual and physical boards. -It is important that the virtual board matches the physical board, otherwise this calibration -will not work. - -``` -python3 calibrate_multistereo_cameras_with_charuco_cli.py --check_board_pattern --legacy_charuco_pattern t -``` - -There should be an axis at the center of the board, where the Y axis (green) -points upwards, the X axis (red) points to the right, and the figure should be labelled -as Z-axis out of board. If it isn't then try without legacy (```--legacy_charuco_pattern f```). - -If you are using the default Spot Calibation board, and there is an aruco marker -in the top left corner, then it legacy (so supply true argument to legacy.) - -# Calibrate Spot Manipulator Eye-In-Hand Cameras With the CLI tool - -There is an [existing method to calibrate the gripper cameras with the Spot Api](https://dev.bostondynamics.com/protos/bosdyn/api/proto_reference#bosdyn-api-spot-GripperCameraCalibrationCommandRequest). However, you don't have -as much control and extensibility in the existing method as with this custom procedure. - -In Spot's hand, there is an RGB camera, as well as a ToF camera, which ```calibrate_spot_hand_camera_cli.py``` -automatically co-registers using the calibration utility via the default calibration board included -with most Spots. You can also use a different board if you'd like, just set the right CLI parameters -(see example usage below) - -## Robot and Target Setup - -Have Spot sit on the ground with the arm stowed such that nothing is within a meter of the robot. -Spot should NOT be on its charging dock. - -No program should have robot control authority. Make sure Spot is sitting -with the arm stowed before releasing robot authority. -If you are using the ROS 2 Driver, disconnect from Spot. If you are using the tablet to control Spot, -select power icon (top) -> Advanced -> Release control. - -Place the Spot default calibration board on the ground leaning against something in front of Spot, so -that the calibration board's pattern is facing Spot front (where the stowed arm points). -The up arrow should point in the direction of the sky. The board should be tilted away from -Spot at a 45 degree angle, so that the bottom of the board is closer Spot than the top of the board. The ground beneath -the board should be at about a 45 degree angle from the board. The board's bottom should be about a meter away -from the front of Spot while sitting. Nothing should be within a meter of the robot. - -**See the first reference image at the top of this README to see good board placement relative to Spot.** - -When calibrating, Spot will stand up, ready its arm, lower its base slightly, and -lower its arm slightly. As soon as this happens, if Spot -can see a Charuco board it will start to move the arm around for the calibration. - -If the calibration board isn't placed at the right location, or there is more -than one board visible to the robot, this may lead to potentially unsafe behavior, so be ready to -E-Stop the robot at any moment. If the robot starts exhibiting unexpected behavior, stopping the program, -turning off the computer running the calibration, and hijacking control from the tablet can help stop robot movement. - -It is possible to calibrate at further distances -(see ```--dist_from_board_viewpoint_range``` arg), but the sampling of the viewpoint -distance from the board must be feasible to reach with where the base of the -robot is started relative to the board. The previously recommended Spot and Target setup is what worked well -in testing for the default distance viewpoint range. - -After the calibration is finished, Spot stows its arm and sits back down. At this point, -it is safe to take control of Spot from the tablet or ROS 2 , even if the calibration script is still -running. Just don't stop the script or it will stop calculating the parameters :) - -If Spot is shaking while moving the arm around, it is likely that -your viewpoint range is too close or too far (most often, adjusting -```--dist_from_board_viewpoint_range``` will help with that). You can -also try to drive the Spot to a better location to start the calibration -that fits the distance from viewpoint range better. - -## Example Usage (a.k.a Hand Specific Live Incantations) - -For all possible arguments to the Hand Specific CLI tool, run ```python3 calibrate_spot_hand_camera_cli.py -h```. -Many parameters are customizable. - -If you'd like to calibrate depth to rgb, with rgb at default resolution, saving photos to ```~/my_collection/calibrated.yaml```, -here is an example CLI command template, under the default tag (recommended for first time). -Note that the default Spot Board is a legacy pattern for OpenCV > 4.7, so ensure to pass -the --legacy_charuco_pattern flag - -``` -python3 calibrate_spot_hand_camera_cli.py --ip -u user -pw --data_path ~/my_collection/ \ ---save_data --result_path ~/my_collection/calibrated.yaml --photo_utilization_ratio 1 --stereo_pairs "[(1,0)]" --legacy_charuco_pattern True \ ---spot_rgb_photo_width=640 --spot_rgb_photo_height=480 --tag default -``` - -If you'd like to load photos, and run the calibration with slightly different parameters, -while saving both the results and the parameters to same the config file as in the previous example. -Here is an example CLI command template (from recorded images, no data collection) - -``` -python3 calibrate_multistereo_cameras_with_charuco_cli.py --data_path ~/my_collection/ ---result_path ~/my_collection/bottle_calibrated.yaml --photo_utilization_ratio 2 --stereo_pairs "[(1,0)]" --legacy_charuco_pattern True \ ---spot_rgb_photo_width=640 --spot_rgb_photo_height=480 --tag less_photos_used_test_v1 -``` - -If you'd like to calibrate depth to rgb, at a greater resolution, while sampling -viewpoints at finer X-rotation steps relative to the board, and slightly further from the board -with finer steps, here is an example CLI command template. Also, -to demonstrate the stereo pairs argument, let's assume that you also want to find rgb to depth (redundant for -demonstration purposes), while writing to the same config files as above. - -``` -python3 calibrate_spot_hand_camera_cli.py --ip -u user -pw --data_path ~/my_collection/ \ ---save_data --result_path ~/my_collection/calibrated.yaml --photo_utilization_ratio 1 --stereo_pairs "[(1,0), (0,1)]" --legacy_charuco_pattern True\ ---spot_rgb_photo_width=1920 --spot_rgb_photo_height=1080 --x_axis_rot_viewpoint_range -10 10 1 \ ---dist_from_board_viewpoint_range .6 .9 .1 -``` - -> ***NOTE:*** If you would like to save the new calibration parameters to the robot itself, just add the `-send` flag and they will be sent to the robot. - -## Improving Calibration Quality - -If you find that the calibration quality isn't high enough, try a longer calibration -with a wider variety of viewpoints (decrease the step size, increase the bounds). -The default calibration viewpoint parameters are meant to facilitate a quick calibration -even on more inexpensive hardware, and as such uses a minimal amount of viewpoints. - -However, in calibration, less is more. It is better to collect fewer high quality -viewpoints then many low quality ones. Play with the viewpoint sampling parameters -to find what takes the most diverse high quality photos of the board. - -Also, [make you are checking if your board is legacy, and if you can -allow default corner ordering](#check-if-you-have-a-legacy-charuco-board). - -If you are using a robot to collect your dataset, such as Spot, you can -also try increasing the settle time prior to taking an image (see ```--settle_time```) - -## Using the Registered Information with Spot ROS 2 - -If you have the [Spot ROS 2 Driver](https://github.com/bdaiinstitute/spot_ros2) installed, -you can leverage the output of the automatic calibration to publish a depth image registered -to the RGB image frame. For more info, see the ```Optional Automatic Eye-in-Hand Stereo Calibration Routine for Manipulator (Arm) Payload``` -section in the [spot_ros2 main README](https://github.com/bdaiinstitute/spot_ros2?tab=readme-ov-file#optional-automatic-eye-in-hand-stereo-calibration-routine-for-manipulator-arm-payload) - -# Using the CLI Tool To Calibrate On an Existing Dataset - -To use the CLI Tool, please ensure that you have one parent folder, -where each camera has a folder under the parent (Numbered from 0 to N). Synchronized photos -for each camera should appear in their respective directories, where matching photos have ids, ascending -upwards, starting from 0. The file structure should appear something like the following: - -``` -existing_dataset/ -├── 0/ -│ ├── 0.png # taken at viewpoint 0 -│ ├── 1.png -│ └── 2.png -├── 1/ -│ ├── 0.png # taken at viewpoint 0 -│ ├── 1.png -│ └── 2.png -├── 2/ -│ ├── 0.png # taken at viewpoint 0 -│ ├── 1.png -│ └── 2.png -├── poses/ # optional, for camera to robot cal -│ ├── 0.npy # base to planning frame 4x4 homgenous transform at viewpoint 0 -│ ├── 1.npy # .npy files generated with np.save(FILENAME, 4x4_POSE) -│ └── 2.npy -``` - -Optionally, you can also include pose information, to find the camera to robot extrinsic. - -To see all possible arguments for calibration, please run ```python3 calibrate_multistereo_cameras_with_charuco_cli.py -h```. - Many parameters such as board proportions and Agmruco dictionary are customizable. - -If you'd like to register camera 1 to camera 0, and camera 2 to camera 0, you could do the following: - -``` -python3 calibrate_multistereo_cameras_with_charuco_cli.py --data_path ~/existing_dataset/ \ ---result_path ~/existing_dataset/eye_in_hand_calib.yaml --photo_utilization_ratio 1 --stereo_pairs "[(1,0), (2, 0)]" \ ---legacy_charuco_pattern=SUPPLY_CHECK_BOARD_FLAG_TO_SEE_IF_LEGACY_NEEDED \ ---tag default --unsafe_tag_save -``` - -# Understanding the Output Calibration Config File from the CLI - -A calibration produced with ```multistereo_calibration_charuco``` from ```calibration_util``` -can be saved as a ```.yaml```file with ```save_calibration_parameters``` from ```calibration_util```. -Here is a demonstration output calibration config file for example purposes: - -``` -default: - intrinsic: - 1: - camera_matrix: flattened_3x3_camera_matrix_now_9x1 - dist_coeffs: flat_5x1_opencv_distortion_coeffs - image_dim: [height, width] - 0: - camera_matrix: flattened_3x3_camera_matrix_now_9x1 - dist_coeffs: flat_5x1_opencv_distortion_coeffs - image_dim: [480, 640] - extrinsic: - 1: # primary camera index (origin/base frame), first sublevel - 0: # reference camera index, second sublevel - R: flattened_3x3_rotation_matrix_from_primary_to_reference_camera_now_9x1 - T: 3x1_translation_matrix_from_primary_to_reference_camera - planning_frame: # the planning frame (in Spot's case, the hand) - R: flattened_3x3_rotation_matrix_from_primary_to_robot_planning_frame_now_9x1 - T: 3x1_translation_matrix_from_primary_to_robot_planning_frame - run_param: - num_images: 729 - timestamp: '2024-08-19 03:43:06' - stereo_pairs: - - [1, 0] - photo_utilization_ratio: 2 - num_checkers_width: 9 - num_checkers_height: 4 - dict_size: DICT_4X4_50 - checker_dim: 0.115 - marker_dim: 0.09 -``` - -Each calibration run -that creates or modifies a config file is tagged with a unique name, to allow for tracking of several experiments, -which is the main title (in the above case, ```default```). -Under the main title, there are the fields relevant to the calibration. -Under ```intrinsic```, the intrinsic (camera matrix, distortion coefficents, and image height/width) -for each camera are recorded as a flattened representation -under the camera's index number as it appears in the list of images -returned by ```capture_images```. For example, if ```capture_images``` produces a list that is -```[image_by_primary_camera, image_by_reference_camera]```, then the intrinsic for the ```primary_camera``` -would be stored under ```0```, and the intrinsic for ```reference_camera``` would be stored under ```1```. - -Under ```extrinsic```, the first sublevel, again a camera index, corresponds to the camera index of which camera -is the origin for the ```extrinsic``` transform between two cameras as the first -field in a requested stereo pair. The second sublevel corresponds to the index of which camera -the ```extrinsic``` maps to from the origin camera. See the comments in the example config file -above for more information. -Additionally, arguments from an argparser can also be dumped into the yaml, which will be saved -under ```run_param``` (excluding args that are: ```password``` or ```username```) - -# Recreate the Core Calibration CLI Tool Without Depending On Spot Wrapper - -If you like the core tools of this utility, and you'd like a more portable version (```standalone_cli.py```) for -use independent of Spot that doesn't depend on Spot Wrapper, you could recreate -the CLI tool with no dependency on Spot Wrapper with the following command: - -``` -cat calibration_util.py <(tail -n +3 automatic_camera_calibration_robot.py) <(tail -n +26 calibrate_multistereo_cameras_with_charuco_cli.py) > standalone_cli.py -``` - -The core capability above depends primarily on NumPy, OpenCV and standard Python libraries. - -# Contributors - -Gary Lvov - -# Special Thanks To - -Michael Pickett, Katie Hughes, Tiffany Cappellari, Andrew Messing, -Emmanuel Panov, Eric Rosen, Brian Okorn, Joseph St Germain and Ken Baker. diff --git a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py index 94facd6a..c7e7dde9 100644 --- a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py +++ b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py @@ -1,6 +1,4 @@ -# Copyright (c) 2025 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. - -# Copy reference (c) 2024 Boston Dynamics AI Institute LLC. All references reserved. +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. import logging from time import sleep @@ -41,10 +39,10 @@ from bosdyn.client.robot_state import RobotStateClient from bosdyn.client.time_sync import TimedOutError -from spot_wrapper.calibration.automatic_camera_calibration_robot import ( +from spot_wrapper.calibration.calibration_helpers import ( AutomaticCameraCalibrationRobot, ) -from spot_wrapper.calibration.calibration_util import ( +from spot_wrapper.calibration.charuco_board_detection import ( convert_camera_t_viewpoint_to_origin_t_planning_frame, est_camera_t_charuco_board_center, ) @@ -124,13 +122,13 @@ def extract_calibration_parameters(self, calibration_dict: dict, tag: str) -> di calibration["rgb_intrinsic"] = np.asarray(calibration_dict[tag]["intrinsic"][0]["camera_matrix"]).reshape( (3, 3) ) - rgb_to_depth_T = np.array(calibration_dict[tag]["extrinsic"][1][0]["T"]).reshape((3, 1)) - rgb_to_depth_R = np.array(calibration_dict[tag]["extrinsic"][1][0]["R"]).reshape((3, 3)) + rgb_to_depth_T = np.array(calibration_dict[tag]["extrinsic"][0][1]["T"]).reshape((3, 1)) + rgb_to_depth_R = np.array(calibration_dict[tag]["extrinsic"][0][1]["R"]).reshape((3, 3)) calibration["rgb_to_depth"] = np.vstack( (np.hstack((rgb_to_depth_R, rgb_to_depth_T)), np.array([0, 0, 0, 1])) ) - depth_to_hand_T = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["T"]).reshape((3, 1)) - depth_to_hand_R = np.array(calibration_dict[tag]["extrinsic"][1]["planning_frame"]["R"]).reshape((3, 3)) + depth_to_hand_T = np.array(calibration_dict[tag]["extrinsic"][0]["planning_frame"]["T"]).reshape((3, 1)) + depth_to_hand_R = np.array(calibration_dict[tag]["extrinsic"][0]["planning_frame"]["R"]).reshape((3, 3)) calibration["depth_to_hand"] = np.vstack( (np.hstack((depth_to_hand_R, depth_to_hand_T)), np.array([0, 0, 0, 1])) ) @@ -157,8 +155,10 @@ def write_calibration_to_robot(self, cal_dict: dict, tag: str = "default") -> No def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSource.PinholeModel: """Converts a 3x3 intrinsic matrix to a PinholeModel protobuf.""" pinhole_model = ImageSource.PinholeModel() - pinhole_model.CameraIntrinsics.focal_length = intrinsic_matrix[0, :1] - pinhole_model.CameraIntrinsics.principal_point = (intrinsic_matrix[0, 2], intrinsic_matrix[1, 2]) + pinhole_model.intrinsics.focal_length.x = float(intrinsic_matrix[0, 0]) + pinhole_model.intrinsics.focal_length.y = float(intrinsic_matrix[1, 1]) + pinhole_model.intrinsics.principal_point.x = float(intrinsic_matrix[0, 2]) + pinhole_model.intrinsics.principal_point.y = float(intrinsic_matrix[1, 2]) return pinhole_model hand_t_wr1_pose = get_a_tform_b( @@ -181,11 +181,17 @@ def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSou rgb_intrinsics = cal["rgb_intrinsic"] rgb_t_depth = cal["rgb_to_depth"] - depth_to_planning = cal["depth_to_hand"] @ hand_t_planning - rgb_to_planning = rgb_t_depth @ depth_to_planning + # cal["depth_to_hand"] is rgb_M_hand: the hand frame expressed in RGB camera coords. + # hand_t_planning is hand_M_wr1 (hand->wr1 transform built above). + # rgb_M_wr1 = rgb_M_hand @ hand_M_wr1 + rgb_M_wr1 = cal["depth_to_hand"] @ hand_t_planning + # rgb_t_depth is depth_M_rgb (stereo extrinsic: maps RGB points → depth frame). + # depth_M_wr1 = depth_M_rgb @ rgb_M_wr1 + depth_M_wr1 = rgb_t_depth @ rgb_M_wr1 - planning_t_depth = np.linalg.inv(depth_to_planning) - planning_t_rgb = np.linalg.inv(rgb_to_planning) + # API wants wr1_tform_sensor = wr1_M_sensor for each camera + planning_t_rgb = np.linalg.inv(rgb_M_wr1) # wr1_M_rgb → color.wr1_tform_sensor + planning_t_depth = np.linalg.inv(depth_M_wr1) # wr1_M_depth → depth.wr1_tform_sensor # Converting calibration data to protobuf format depth_intrinsics_proto = convert_pinhole_intrinsic_to_proto(depth_intrinsics) @@ -211,19 +217,26 @@ def convert_pinhole_intrinsic_to_proto(intrinsic_matrix: np.ndarray) -> ImageSou ), ) ) + logger.info(f" Request to send: \n{set_req}\n") # Send the request to the robot try: result = self.gripper_camera_client.set_camera_calib(set_req) - logger.info(f" Set Parameters: \n{result}") + logger.info(f" Set Parameters: \n{result}\n") except Exception as e: raise ValueError(f"Failed to set calibration parameters on the robot: {e}") - # Optionally, verify by retrieving the parameters back with the following lines - # get_req = gripper_camera_param_pb2.GripperCameraGetParamRequest() - # cal = self.gripper_camera_client.get_camera_calib(get_req) - # logger.info(f"Post-Set Cal (get cam param req): \n {cal}") + # Optionally, verify by retrieving the parameters back with the following line + # self.get_calibration_from_robot() logger.info("Calibration parameters successfully sent to the robot.") + def get_calibration_from_robot(self) -> None: + """Utility function to retrieve and print the current gripper camera calibration + from the robot for verification. + """ + get_req = gripper_camera_param_pb2.GripperCameraGetParamRequest() + cal = self.gripper_camera_client.get_camera_calib(get_req) + logger.info(f"Retrieved calibration from robot: \n{cal}\n") + def capture_images( self, encodings: Optional[List[int]] = None, @@ -236,7 +249,7 @@ def capture_images( if image_responses: if len(encodings) != len(image_responses): raise ValueError("Need to specify an encoding for each image request") - for idx, (response, encoding) in enumerate(zip(image_responses, encodings)): + for response, encoding in zip(image_responses, encodings): image_data = response.shot.image.data image_data = np.frombuffer(image_data, np.uint8) image_data = cv2.imdecode(image_data, encoding)