convenience function to detect charco board pose in an image (#88)

Co-authored-by: Thomas Lips <[email protected]>

airo-camera-toolkit/airo_camera_toolkit/calibration/fiducial_markers.py

@@ -143,6 +143,38 @@ def get_pose_of_charuco_board(
     return charuco_pose_in_camera_frame
 
 
+def detect_charuco_board(
+    image: OpenCVIntImageType,
+    camera_matrix: CameraIntrinsicsMatrixType,
+    dist_coeffs: Optional[np.ndarray] = None,
+    aruco_markers: ArucoDictType = AIRO_DEFAULT_ARUCO_DICT,
+    charuco_board: CharucoDictType = AIRO_DEFAULT_CHARUCO_BOARD,
+) -> Optional[HomogeneousMatrixType]:
+    """Detect the pose of a charuco board from an image and the camera's intrinsics.
+
+    Args:
+        image: An image that might contain a charuco board.
+        camera_matrix: The intrinsics of the camera that took the image.
+        dist_coeffs: The distortion coefficients of the camera that took the image.
+        aruco_markers: The dictionary from OpenCV that specifies the aruco marker parameters.
+        charuco_board: The dictionary from OpenCV that specifies the charuco board parameters.
+
+    Returns:
+        Optional[HomogeneousMatrixType]: The pose of the charuco board in the camera frame, if it was detected.
+    """
+
+    aruco_result = detect_aruco_markers(image, aruco_markers)
+    if not aruco_result:
+        return None
+
+    charuco_result = detect_charuco_corners(image, aruco_result, charuco_board)
+    if not charuco_result:
+        return None
+
+    charuco_pose = get_pose_of_charuco_board(charuco_result, charuco_board, camera_matrix, dist_coeffs)
+    return charuco_pose
+
+
 #################
 # visualization #
 #################
@@ -193,7 +225,6 @@ def visualize_marker_detections(
         charuco_y_count: Optional[int] = None,
         charuco_tile_size: Optional[int] = None,
     ) -> None:
-
         aruco_dict = AIRO_DEFAULT_ARUCO_DICT
         detect_charuco = charuco_x_count is not None and charuco_y_count is not None and charuco_tile_size is not None
         if detect_charuco:

airo-camera-toolkit/docs/live_charuco_pose.py

@@ -0,0 +1,21 @@
+import cv2
+from airo_camera_toolkit.calibration.fiducial_markers import detect_charuco_board, draw_frame_on_image
+from airo_camera_toolkit.cameras.zed2i import Zed2i
+from airo_camera_toolkit.utils import ImageConverter
+
+camera = Zed2i(fps=30)
+intrinsics = camera.intrinsics_matrix()
+
+window_name = "Charuco Pose"
+cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
+
+while True:
+    image = camera.get_rgb_image_as_int()
+    image = ImageConverter.from_numpy_int_format(image).image_in_opencv_format
+    pose = detect_charuco_board(image, intrinsics)
+    if pose is not None:
+        draw_frame_on_image(image, pose, intrinsics)
+    cv2.imshow(window_name, image)
+    key = cv2.waitKey(1)
+    if key == ord("q"):
+        break