From 066c633615c38d9d13940efddd24b62db0745de7 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 26 Nov 2025 18:23:24 +0100 Subject: [PATCH] improved marker drawing --- cam_pose_charuco_board.py | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py index 0963982..6b9bd22 100644 --- a/cam_pose_charuco_board.py +++ b/cam_pose_charuco_board.py @@ -5,6 +5,8 @@ import argparse from aruco_types import ARUCO_DICT import math +from cam_pose_checker import draw_gizmo + CAM_CALIB_FILE = "results/cam_calib_charuco.npz" def rotation_matrix_to_euler_angles(R): @@ -119,7 +121,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa r_vec, t_vec = process(detector, board, img, mtx, dist, None, None) if r_vec is not None and t_vec is not None: r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec) - rot = [int(180.0 / math.pi * v) for v in r_vec_obj] + rot = [round(180.0 / math.pi * v, 0) for v in r_vec_obj] if rot != last_rotation: print("\r ", end='') print(f"\rTilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}", end='') @@ -155,23 +157,26 @@ def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvec return mtx, dist, rvecs, tvecs -def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, img, mtx, dist, rvecs=None, tvecs=None): # Load previously saved data +def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, img, mtx, dist, rvecs=None, tvecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False): # Load previously saved data r_vec = None t_vec = None gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray) if charuco_corners is not None: - # Draw a square around the markers - cv.aruco.drawDetectedCornersCharuco(img, charuco_corners, charuco_ids) + # Draw marker box + if draw_marker_box: + cv.aruco.drawDetectedMarkers(img, marker_corners) - # Draw a square around the markers - cv.aruco.drawDetectedMarkers(img, marker_corners) + # Draw marker id + if draw_marker_id: + cv.aruco.drawDetectedCornersCharuco(img, charuco_corners, charuco_ids) - # Draw an axis around the markers - rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist) - for i in range(len(rvecs)): - cv.drawFrameAxes(img, mtx, dist, rvecs[i], tvecs[i],0.05) + # Draw marker axis + if draw_marker_axis: + rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist) + for i in range(len(rvecs)): + cv.drawFrameAxes(img, mtx, dist, rvecs[i], tvecs[i],0.05) if len(charuco_ids) >= len(board.getIds()): obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids) @@ -180,7 +185,7 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im if mtx is not None and dist is not None: pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, useExtrinsicGuess=False, rvec=rvecs, tvec=tvecs, flags=cv.SOLVEPNP_ITERATIVE) if pose: - # Draw Axis + # Draw cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10) return r_vec, t_vec