Add camera to robot calibration support

spot_wrapper/calibration/README.md

@@ -27,7 +27,11 @@ camera calibration to **solve for the intrinsic and extrinsic parameters for two
 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,
+you can use the CLI tool to compute the intrinsic/extrinsic parameters. 
+
+Additionally, if you have saved the poses the images are taken at, you can also calibrate
+the camera to robot extrinsic (eye-to-hand registration).
+
 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.
 
@@ -205,8 +209,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
+│   └── 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 +251,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,9 @@ 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

@@ -13,7 +13,7 @@
 )
 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,
@@ -25,35 +25,41 @@
 
 logger = logging.getLogger(__name__)
 
+def create_robot(args, charuco, aruco_dict): # 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():
+    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,42 +73,16 @@ 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",
         "-dfbvr",
@@ -167,22 +147,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",
@@ -202,6 +166,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()