Add camera to robot calibration support (#140)

* Add camera to robot calibration support

* allow for relative naming

* formatting

* cleanup

* final

* fixes to address brandons review

Author: glvov-bdai

spot_wrapper/calibration/README.md

@@ -1,5 +1,8 @@
 # Automatic Robotic Stereo Camera Calibration Utility with Charuco Target (a.k.a Multi-Stereo Madness)
 
+## Find where your cameras are relative to each other, and relative to your robot
+##  Find how your cameras project 3D points into pixels
+
 ### Recommended Setup
 
 ![spot eye in hand cal](spot_eye_in_hand_setup.jpg)
@@ -22,13 +25,18 @@
 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,
-the CLI tool's saving capability allows to store multiple unique calibration runs in one configuration file
+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 
@@ -205,8 +213,15 @@ existing_dataset/
 │   ├── 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.
 
@@ -240,6 +255,9 @@ default:
       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'

spot_wrapper/calibration/calibrate_multistereo_cameras_with_charuco_cli.py

@@ -13,7 +13,7 @@
     create_charuco_board,
     create_ideal_charuco_image,
     detect_charuco_corners,
-    load_images_from_path,
+    load_dataset_from_path,
     multistereo_calibration_charuco,
     save_calibration_parameters,
 )
@@ -64,6 +64,7 @@ def calibration_helper(
     args: argparse.Namespace,
     charuco: cv2.aruco_CharucoBoard,
     aruco_dict: cv2.aruco_Dictionary,
+    poses: np.ndarray,
 ):
     logger.warning(
         f"Calibrating from {len(images)} images.. for every "
@@ -84,6 +85,7 @@ def calibration_helper(
         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")
@@ -115,8 +117,8 @@ def main():
     logger.info(f"Loading images from {args.data_path}")
 
     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)
+        images, poses = load_dataset_from_path(args.data_path)
+        calibration_helper(images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses)
     else:
         logger.warning("Could not load any images to calibrate from, supply --data_path")
 

spot_wrapper/calibration/calibrate_spot_hand_camera_cli.py

@@ -2,18 +2,20 @@
 
 import argparse
 import logging
-from time import sleep
+from typing import Tuple
 
+import cv2
 import numpy as np
 
+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_images_from_path,
+    load_dataset_from_path,
 )
 from spot_wrapper.calibration.spot_in_hand_camera_calibration import (
     SpotInHandCalibration,
@@ -26,34 +28,46 @@
 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 = spot_cli(calibrator_cli())
+    parser = create_robot_parser()
     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 !!!")
         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)
-
-        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,
-            ),
-        )
+        # sleep(5)
 
-        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"
+        in_hand_bot, args = create_robot(args, charuco=charuco, aruco_dict=aruco_dict)
 
-        images = get_multiple_perspective_camera_calibration_dataset(
+        images, poses = get_multiple_perspective_camera_calibration_dataset(
             auto_cam_cal_robot=in_hand_bot,
             max_num_images=args.max_num_images,
             distances=np.arange(*args.dist_from_board_viewpoint_range),
@@ -67,41 +81,14 @@ def spot_main() -> None:
         )
     else:
         logger.info(f"Loading images from {args.data_path}")
-        images = load_images_from_path(args.data_path)
+        images, poses = load_dataset_from_path(args.data_path)
 
-    calibration_helper(images=images, args=args, charuco=charuco, aruco_dict=aruco_dict)
+    calibration_helper(images=images, args=args, charuco=charuco, aruco_dict=aruco_dict, poses=poses)
 
 
-def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser:
+def calibrate_robot_cli(parser: argparse.ArgumentParser = None) -> argparse.ArgumentParser:
     if parser is None:
         parser = calibrator_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(
         "--dist_from_board_viewpoint_range",
@@ -167,22 +154,6 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser:
         help="How long to wait after movement to take a picture; don't want motion blur",
     )
 
-    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",
-    )
     # path things
     parser.add_argument(
         "--save_data",
@@ -203,5 +174,56 @@ def spot_cli(parser=argparse.ArgumentParser) -> argparse.ArgumentParser:
     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()