Calibration fixes: Supporting OpenCV prior to and after breaking chan…

…ges 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:
opencv/opencv#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:
opencv/opencv#26126

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

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 <IP> -u user -pw <SECRET> --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,16 +160,26 @@ 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 <IP> -u user -pw <SECRET> --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
 ```
 
 ## 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.
 

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: