From 0a58923909daf3c03a8fed18a74c0b4c2e8a7317 Mon Sep 17 00:00:00 2001 From: glvov-bdai Date: Mon, 9 Sep 2024 14:57:27 -0400 Subject: [PATCH] Calibration fixes: Supporting OpenCV prior to and after breaking changes in 4.7 (#138) In OpenCV 4.7.0, charuco was moved from the contrib part of opencv to the core part of opencv (https://github.com/opencv/opencv/pull/22986/files#diff-09508dfcb283f5b8f2e58538584a58ce28d369f8c7e23705f004f7d15e6f40e9). Previously, the code assumed an OpenCV version prior to 4.7.0. This adds an explicit check for the opencv version prior to prior to many charuco operations, so that the correct charuco method can be called for opencv>=4.7 despite the breaking change in 4.7 More information about OpenCV changes: https://github.com/opencv/opencv/issues/26126 This also adds an explicit dependency on opencv. Previously, this dependency was likely implicitly included if you use the spot_ros2 package (due to dependency on things like cv bridge), but this not cover the use case of using spot_wrapper completely independently of ROS2. It is better to check OpenCV version, and adjust behavior accordingly, then just depending on opencv-contrib-python, as depending on contrib can break existing versions of OpenCV on your system when the requirements.txt is installed Testing: Tested on physical robot. For code changes with OpenCV, verified the results in prior to 4.7 and after: https://github.com/opencv/opencv/issues/26126 --- requirements.txt | 2 + spot_wrapper/calibration/README.md | 65 ++- ...te_multistereo_cameras_with_charuco_cli.py | 134 +++++- .../calibrate_spot_hand_camera_cli.py | 32 +- spot_wrapper/calibration/calibration_util.py | 401 +++++++++++++++--- .../spot_in_hand_camera_calibration.py | 6 +- 6 files changed, 526 insertions(+), 114 deletions(-) diff --git a/requirements.txt b/requirements.txt index faddbb9..f68dc67 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,9 +7,11 @@ bosdyn-mission==4.0.2 grpcio==1.59.3 image==1.5.33 inflection==0.5.1 +opencv-python>=4.5.4 open3d==0.18.0 protobuf==4.22.1 pytest==7.3.1 pytest-cov==4.1.0 pytest-xdist==3.5.0 +pyyaml>=6.0 setuptools==59.6.0 diff --git a/spot_wrapper/calibration/README.md b/spot_wrapper/calibration/README.md index 06e5418..2208ef0 100644 --- a/spot_wrapper/calibration/README.md +++ b/spot_wrapper/calibration/README.md @@ -11,14 +11,15 @@ 1. [***Overview***](#overview) 2. [***Adapting Automatic Collection and Calibration to Your Scenario***](#adapting-automatic-data-collection-and-calibration-to-your-scenario) -3. [***Calibrate Spot Manipulator Eye-In-Hand Cameras With the CLI Tool***](#calibrate-spot-manipulator-eye-in-hand-cameras-with-the-cli-tool) +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) -4. [***Using the CLI Tool To Calibrate On an Existing Dataset***](#using-the-cli-tool-to-calibrate-on-an-existing-dataset) -5. [***Understanding the Output Calibration Config File from the CLI***](#understanding-the-output-calibration-config-file-from-the-cli) -6. [Recreate the Core Calibration CLI Tool Without Depending On Spot Wrapper](#recreate-the-core-calibration-cli-tool-without-depending-on-spot-wrapper) +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 @@ -56,6 +57,27 @@ the new camera image in ```SpotInHandCalibration.capture_images``` in ```spot_in 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 @@ -104,23 +126,31 @@ After the calibration is finished, Spot stows its arm and sits back down. At thi 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) +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)]" \ +--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 resuls 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_spot_hand_camera_cli.py --data_path ~/my_collection/ --from_data \ ---result_path ~/my_collection/bottle_calibrated.yaml --photo_utilization_ratio 2 --stereo_pairs "[(1,0)]" \ +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 @@ -130,7 +160,7 @@ to demonstrate the stereo pairs argument, let's assume that you also want to fin 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)]" \ +--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 ``` @@ -138,8 +168,18 @@ python3 calibrate_spot_hand_camera_cli.py --ip -u user -pw --data_ ## 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. +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, @@ -174,6 +214,7 @@ If you'd like to register camera 1 to camera 0, and camera 2 to camera 0, you co ``` 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 ``` @@ -236,7 +277,7 @@ If you like the core tools of this utility, and you'd like a more portable versi 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 +21 calibrate_multistereo_cameras_with_charuco_cli.py) > standalone_cli.py +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. diff --git a/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py b/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py index 864602d..9e1314b 100644 --- a/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py +++ b/spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py @@ -9,6 +9,10 @@ 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_images_from_path, multistereo_calibration_charuco, save_calibration_parameters, @@ -17,7 +21,42 @@ logging.basicConfig( level=logging.INFO, ) + logger = logging.getLogger(__name__) +# + + +def setup_calibration_param(parser): + 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( @@ -30,6 +69,16 @@ def calibration_helper( 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, @@ -38,6 +87,18 @@ def calibration_helper( ) logger.info(f"Finished script, obtained {calibration}") logger.info("Saving calibration param") + + # If result path is not provided, prompt the user for one + if args.result_path is None: + result_path = input("Please provide a path to save the calibration results (or type 'No' to skip): ") + + if result_path.lower() == "no": + logger.warning("Ran the calibration, but user opted not to save parameters.") + return + else: + args.result_path = result_path + + # Save the calibration parameters if a valid result path is provided save_calibration_parameters( data=calibration, output_path=args.result_path, @@ -50,21 +111,14 @@ def calibration_helper( def main(): parser = calibrator_cli() - 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 = cv2.aruco.CharucoBoard_create( - args.num_checkers_width, - args.num_checkers_height, - args.checker_dim, - args.marker_dim, - aruco_dict, - ) + args, aruco_dict, charuco = setup_calibration_param(parser) logger.info(f"Loading images from {args.data_path}") - images = load_images_from_path(args.data_path) - calibration_helper(images=images, args=args, charuco=charuco, aruco_dict=aruco_dict) + + if args.data_path is not None: + images = load_images_from_path(args.data_path) + calibration_helper(images=images, args=args, charuco=charuco, aruco_dict=aruco_dict) + else: + logger.warning("Could not load any images to calibrate from, supply --data_path") def calibrator_cli() -> argparse.ArgumentParser: @@ -74,7 +128,7 @@ def calibrator_cli() -> argparse.ArgumentParser: "--stereo_pairs", "-sp", type=parse_tuple_list, - required=True, + required=False, default=[(1, 0)], help=( "Capture images returns a list of images. Stereo pairs correspond to" @@ -86,6 +140,39 @@ def calibrator_cli() -> argparse.ArgumentParser: ), ) + 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", @@ -177,7 +264,7 @@ def calibrator_cli() -> argparse.ArgumentParser: "-rp", dest="result_path", type=str, - required=True, + required=False, help="Where to store calibration result as file", ) @@ -207,6 +294,21 @@ def calibrator_cli() -> argparse.ArgumentParser: 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: diff --git a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py b/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py index 2c48a28..4803396 100644 --- a/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py +++ b/spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py @@ -4,12 +4,12 @@ import logging from time import sleep -import cv2 import numpy as np 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, @@ -28,18 +28,7 @@ def spot_main() -> None: parser = spot_cli(calibrator_cli()) - 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 = cv2.aruco.CharucoBoard_create( - args.num_checkers_width, - args.num_checkers_height, - args.checker_dim, - args.marker_dim, - aruco_dict, - ) + args, aruco_dict, charuco = setup_calibration_param(parser) if not args.from_data: logger.warning("This script moves the robot around. !!! USE AT YOUR OWN RISK !!!") @@ -71,7 +60,7 @@ def spot_main() -> None: 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.degrees, + use_degrees=args.use_degrees, settle_time=args.settle_time, data_path=args.data_path, save_data=args.save_data, @@ -93,7 +82,7 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: dest="ip", type=str, help="The IP address of the Robot to calibrate", - required=False, + required=True, ) parser.add_argument( "--user", @@ -102,7 +91,7 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: dest="username", type=str, help="Robot Username", - required=False, + required=True, ) parser.add_argument( "--pass", @@ -111,7 +100,7 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: dest="password", type=str, help="Robot Password", - required=False, + required=True, ) parser.add_argument( @@ -120,7 +109,7 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: nargs="+", type=float, dest="dist_from_board_viewpoint_range", - default=[0.6, 0.7, 0.2], + 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. " @@ -143,13 +132,14 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: action="store_false", help="Use radians for rotation ranges", ) - for axis in ["x", "y", "z"]: + 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=[-30, 31, 10], + 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. " @@ -173,7 +163,7 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser: "-st", dest="settle_time", type=float, - default=0.5, + default=1.0, help="How long to wait after movement to take a picture; don't want motion blur", ) diff --git a/spot_wrapper/calibration/calibration_util.py b/spot_wrapper/calibration/calibration_util.py index 946c553..19951c1 100644 --- a/spot_wrapper/calibration/calibration_util.py +++ b/spot_wrapper/calibration/calibration_util.py @@ -4,6 +4,7 @@ import logging import os import re +from copy import deepcopy from datetime import datetime from glob import glob from math import radians @@ -25,16 +26,86 @@ logger = logging.getLogger(__name__) SPOT_DEFAULT_ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) -SPOT_DEFAULT_CHARUCO = cv2.aruco.CharucoBoard_create(9, 4, 0.115, 0.09, SPOT_DEFAULT_ARUCO_DICT) +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: np.ndarray = np.arange(0.5, 0.8, 0.1), - x_axis_rots: np.ndarray = np.arange(-21, 21, 5), - y_axis_rots: np.ndarray = np.arange(-21, 21, 5), - z_axis_rots: np.ndarray = np.array([-21, 21, 5]), + distances: 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("~"), @@ -53,16 +124,16 @@ def get_multiple_perspective_camera_calibration_dataset( 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 np.arange(0.5, 0.8, 0.1). + 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 np.arange(-21, 21, 5). + 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 np.arange(-21, 21, 5). + 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 aply to sample viewpoints. Defaults to np.array([-21, 21, 5]). + 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 @@ -260,44 +331,178 @@ def multistereo_calibration_charuco( 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 = SPOT_DEFAULT_CHARUCO, - aruco_dict: cv2.aruco_Dictionary = SPOT_DEFAULT_ARUCO_DICT, + 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. + 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): What - charuco board to look for. Defaults to SPOT_DEFAULT_CHARUCO. - aruco_dict (cv2.aruco_Dictionary, optional): What aruco dict to look for. - Defaults to SPOT_DEFAULT_ARUCO_DICT. + 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]]: Either the corners, and their - ids, or (None,None) if corners weren't found. + 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) - corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict) - if ids is not None: - """ - Here, setting the minMarkers=0 flag is critical for a good calibration due to the partial - board views. Otherwise, no corners near the border of the board are used, which with the - large default spot board, results in no points near the border of the image being collected. - Without points near the border of the image, the distortion model will be inaccurate. - """ - _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco( - corners, ids, gray, charuco_board, minMarkers=0 - ) + 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, charuco_ids - return None, None + return charuco_corners, reworked_charuco_ids def get_charuco_board_object_points( @@ -316,9 +521,13 @@ def get_charuco_board_object_points( 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(charuco_board.chessboardCorners[idx]) + object_points.append(corners[idx]) return np.array(object_points, dtype=np.float32) @@ -397,7 +606,7 @@ def stereo_calibration_charuco( dist_coeffs_reference: Optional[np.ndarray] = None, ) -> Dict: """ - Perform a stereo calibration from a set of syncrhonized stereo images of a charuco calibration + Perform a stereo calibration from a set of synchronized stereo images of a charuco calibration board. Args: @@ -414,7 +623,7 @@ def stereo_calibration_charuco( 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 - toa ssign to camera 1. If None, is computed. Defaults to None. + to assign to camera 1. If None, is computed. Defaults to None. Raises: ValueError: Could not calibrate a camera individually @@ -489,6 +698,7 @@ def stereo_calibration_charuco( if len(obj_points_all) > 0: logger.info(f"Collected {len(obj_points_all)} shared point sets for stereo calibration.") + # logger.info(f"{np.array(obj_points_all).shape = } {np.array()}") _, _, _, _, _, R, T, _, _ = cv2.stereoCalibrate( obj_points_all, img_points_origin, @@ -531,11 +741,10 @@ def est_camera_t_charuco_board_center( dist_coeffs: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """ - Localizes the 6D pose of the checkerboard center using Charuco corners. + 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 axis being parallel to the sides of the board. + 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. @@ -549,37 +758,28 @@ def est_camera_t_charuco_board_center( 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: - # Estimate the pose of the Charuco board - rvec = np.zeros((3, 1)) - tvec = np.zeros((3, 1)) - retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard( - charuco_corners, - charuco_ids, - charuco_board, - camera_matrix, - dist_coeffs, - rvec, - tvec, - ) - if retval: - # Compute the translations needed to transform to the center + 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 - # Adjust tvec to be relative to the center center_trans = np.array([x_trans, y_trans, 0.0]).reshape((3, 1)) rmat, _ = cv2.Rodrigues(rvec) - tvec = tvec + rmat.dot(center_trans) - return np.array(rmat), np.array(tvec).ravel() + tvec = tvec + rmat.dot(center_trans) + tvec_to_camera = tvec + return np.array(rmat), np.array(tvec_to_camera).ravel() else: - raise ValueError( - "Corners were found, but failed to localize. You likely primed the robot too close to the board" - ) + 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, " + "Couldn't detect any Charuco boards in the image, " "localization failed. Ensure the board is visible from the" " primed pose." ) @@ -934,3 +1134,82 @@ def process_data_with_nested_dictionaries( sort_keys=False, ) logger.info(f"Saved calibration to file {output_path} under tag '{tag}'") + + +def charuco_pose_sanity_check( + img: np.ndarray, charuco_board: cv2.aruco_CharucoBoard, aruco_dict: cv2.aruco_Dictionary +) -> 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. + + 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): + """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) + + +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 diff --git a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py index d3693fe..0adc7d8 100644 --- a/spot_wrapper/calibration/spot_in_hand_camera_calibration.py +++ b/spot_wrapper/calibration/spot_in_hand_camera_calibration.py @@ -48,8 +48,6 @@ ) logger = logging.getLogger(__name__) -SPOT_DEFAULT_ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) -SPOT_DEFAULT_CHARUCO = cv2.aruco.CharucoBoard_create(9, 4, 0.115, 0.09, SPOT_DEFAULT_ARUCO_DICT) class SpotInHandCalibration(AutomaticCameraCalibrationRobot): @@ -141,8 +139,8 @@ def localize_target_to_principal_camera(self, images: Union[List, np.ndarray]) - self.estimated_camera_matrix, self.estimated_camera_distort_coeffs, ) - except AttributeError: - raise ValueError("Must call _set_localization_param prior to localizing") + except AttributeError as e: + raise ValueError(f"Must call _set_localization_param prior to localizing: {e}") def move_cameras_to_see_calibration_target(self) -> np.ndarray: def adjust_standing_height(height: float) -> None: