From 0aa7e0feddaf28c441208ac823d8b427f2608a16 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 26 Nov 2025 14:10:11 +0100 Subject: [PATCH] [cam_pose_charuco_board] - convert cam pose to board rotation --- README.md | 4 ++++ cam_pose_charuco_board.py | 38 ++++++++++++++++++++++++-------------- 2 files changed, 28 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index cd0a487..26bd19f 100644 --- a/README.md +++ b/README.md @@ -12,3 +12,7 @@ https://github.com/opencv/opencv/tree/4.x/samples/cpp/tutorial_code/objectDetect ## Python https://github.com/opencv/opencv_contrib/tree/4.x/modules/aruco + + +# Rotation +https://stackoverflow.com/questions/13823296/converting-opencv-rotation-and-translation-vectors-to-xyz-rotation-and-xyz-posit \ No newline at end of file diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py index 65af6f2..bd737d8 100644 --- a/cam_pose_charuco_board.py +++ b/cam_pose_charuco_board.py @@ -22,6 +22,15 @@ def rotation_matrix_to_euler_angles(R): z = 0 return np.array([x, y, z]) +def board_rotation(r_vec, t_vec): + r_mat = np.ndarray((3, 3)) + cv.Rodrigues(r_vec, r_mat) + r_mat = np.hstack((r_mat, t_vec)) + r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1]))) + r_mat = np.linalg.inv(r_mat) + res = rotation_matrix_to_euler_angles(r_mat[0:3, 0:3]) + return res + def factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0): aruco_dict = cv.aruco.getPredefinedDictionary(dict_type) board = cv.aruco.CharucoBoard(board_size, square_length, marker_length, aruco_dict) @@ -55,7 +64,7 @@ def main(): print("[INFO] start processing...") process_video(detector, board) -def process_video(detector: cv.aruco.CharucoDetector, board): +def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard): mtx = None dist = None obj_points_list = [] @@ -80,12 +89,11 @@ def process_video(detector: cv.aruco.CharucoDetector, board): cap.set(cv.CAP_PROP_FPS, 60.0) cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280) cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024) - cap.set(cv.CAP_PROP_EXPOSURE, 20) + cap.set(cv.CAP_PROP_EXPOSURE, 30) cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) img_size = None last_rotation = [0,0,0] - use_euler_angle = True while True: # Read a new frame ok, img = cap.read() @@ -109,16 +117,11 @@ def process_video(detector: cv.aruco.CharucoDetector, board): img_points_list = [] else: r_vec, t_vec = process(detector, board, img, mtx, dist, None, None) - if r_vec is not None: - if use_euler_angle: - r_mat = np.ndarray((3,3)) - cv.Rodrigues(r_vec, r_mat) - res = rotation_matrix_to_euler_angles(r_mat) - else: - res = r_vec + if r_vec is not None and t_vec is not None: + res = board_rotation(r_vec, t_vec) rot = [int(180.0 / math.pi * v) for v in res] - if rot != last_rotation or True: - print(f"Rotation X|Y|Z: {rot[0]:.1f} | {rot[1]:.1f} | {rot[2]:.1f}") + if rot != last_rotation: + print(f"Rotation Tilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}") last_rotation = rot cv.imshow('img', img) @@ -151,7 +154,7 @@ 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.Board, 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): # Load previously saved data r_vec = None t_vec = None gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) @@ -164,10 +167,17 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, # Draw a square around the markers cv.aruco.drawDetectedMarkers(img, marker_corners) + # 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) + if len(charuco_ids) >= len(board.getIds()): obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids) + offset = int(board.getMarkerLength() * board.getSquareLength()) + obj_points -= [offset, offset, 0] if mtx is not None and dist is not None: - pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, rvec=rvecs, tvec=tvecs) + 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 cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)