From 47d216b6d3152684c48e1e631d958df577b8af31 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 26 Nov 2025 14:21:25 +0100 Subject: [PATCH] refactored --- README.md | 6 +++++- cam_pose_charuco_board.py | 10 +++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 26bd19f..389c8e4 100644 --- a/README.md +++ b/README.md @@ -15,4 +15,8 @@ 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 +https://stackoverflow.com/questions/13823296/converting-opencv-rotation-and-translation-vectors-to-xyz-rotation-and-xyz-posit + + +# Camera pose to object pose +https://answers.opencv.org/question/64315/solvepnp-object-to-camera-pose/ \ No newline at end of file diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py index bd737d8..808095b 100644 --- a/cam_pose_charuco_board.py +++ b/cam_pose_charuco_board.py @@ -22,14 +22,14 @@ def rotation_matrix_to_euler_angles(R): z = 0 return np.array([x, y, z]) -def board_rotation(r_vec, t_vec): +def to_board_pose_euler(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 + res = rotation_matrix_to_euler_angles(r_mat) + return res, r_mat[:,3] 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) @@ -118,8 +118,8 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa else: r_vec, t_vec = process(detector, board, img, mtx, dist, None, None) 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] + r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec) + rot = [int(180.0 / math.pi * v) for v in r_vec_obj] if rot != last_rotation: print(f"Rotation Tilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}") last_rotation = rot