From a67a09d8b2d0fb3829b9bf4e8f390a1c29ea8f63 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Thu, 27 Nov 2025 21:47:44 +0100 Subject: [PATCH] [cam_pose_charuco_board] - refactored [cam_pose_charuco_diamond] - working version [General] - refactored --- cam_pose_charuco_board.py | 42 +++----------------- cam_pose_charuco_diamond.py | 79 ++++++++++++++++++++++++++----------- utils.py | 36 +++++++++++++++++ 3 files changed, 98 insertions(+), 59 deletions(-) create mode 100644 utils.py diff --git a/cam_pose_charuco_board.py b/cam_pose_charuco_board.py index 6b9bd22..4e93f01 100644 --- a/cam_pose_charuco_board.py +++ b/cam_pose_charuco_board.py @@ -5,40 +5,16 @@ import argparse from aruco_types import ARUCO_DICT import math -from cam_pose_checker import draw_gizmo +from utils import to_board_pose_euler, cap_init CAM_CALIB_FILE = "results/cam_calib_charuco.npz" -def rotation_matrix_to_euler_angles(R): - sy = math.sqrt(R[0,0]**2 + R[1,0]**2) - singular = sy < 1e-6 - - if not singular: - x = math.atan2(R[2,1] , R[2,2]) - y = math.atan2(-R[2,0], sy) - z = math.atan2(R[1,0], R[0,0]) - else: - # Handle singularity - x = math.atan2(-R[1,2], R[1,1]) - y = math.atan2(-R[2,0], sy) - z = 0 - return np.array([x, y, z]) - -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) - 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): +def factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0, margin_length=0): aruco_dict = cv.aruco.getPredefinedDictionary(dict_type) board = cv.aruco.CharucoBoard(board_size, square_length, marker_length, aruco_dict) params = cv.aruco.CharucoParameters() detector = cv.aruco.CharucoDetector(board, params) - image = board.generateImage((800, 600), None, 0, 1) + image = board.generateImage((800, 600), None, margin_length, 1) cv.imwrite("results/charuco_board.png", image) return detector, board @@ -86,13 +62,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa print("Cannot open camera") exit() - cap.set(cv.CAP_PROP_SETTINGS, 1) - cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G')) - 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, 30) - cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) + cap_init(cap) img_size = None last_rotation = [0,0,0] @@ -174,9 +144,9 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im # Draw marker axis if draw_marker_axis: - rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist) + _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) + 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) diff --git a/cam_pose_charuco_diamond.py b/cam_pose_charuco_diamond.py index dffa620..a447953 100644 --- a/cam_pose_charuco_diamond.py +++ b/cam_pose_charuco_diamond.py @@ -1,16 +1,17 @@ import sys import numpy as np import cv2 as cv +import math import argparse +from utils import to_board_pose_euler, cap_init from aruco_types import ARUCO_DICT -def factory(dict_type: int, square_length: float=200.0, marker_length: float=120.0, margin_length=10): +def factory(dict_type: int, square_length: float=5.0, marker_length: float=3.0, margin_length=0): aruco_dict = cv.aruco.getPredefinedDictionary(dict_type) board = cv.aruco.CharucoBoard((3,3), square_length, marker_length, aruco_dict) params = cv.aruco.CharucoParameters() detector = cv.aruco.CharucoDetector(board, params) - side_len = int(3.0*square_length + 2.0*margin_length) - image = board.generateImage((side_len, side_len), None, margin_length, 1) + image = board.generateImage((800, 600), None, margin_length, 1) cv.imwrite("results/charuco_diamond_board.png", image) return detector, board @@ -38,48 +39,80 @@ def main(): print("[INFO] start processing...") process_video(detector, board) -def process_video(detector: cv.aruco.CharucoDetector, board): - with np.load('results/cam.npz') as X: +def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard): + with np.load('results/cam_calib_charuco.npz') as X: mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')] # Open camera - video = cv.VideoCapture(0) - if not video.isOpened(): + cap = cv.VideoCapture(0) + if not cap.isOpened(): print("Cannot open camera") exit() + cap_init(cap) + + last_rotation = [0,0,0] while True: # Read a new frame - ok, img = video.read() + ok, img = cap.read() if not ok: return - process(detector, board, img, mtx, dist) k = cv.waitKey(1) & 0xFF if k == ord('q'): break + 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: + r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec) + 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='') + last_rotation = rot -def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist): # Load previously saved data + cv.imshow('img', img) + +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 + use_detect_diamonds = 0 + r_vec = None + t_vec = None gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) - diamond_corners, diamond_ids, marker_corners, marker_ids = detector.detectDiamonds(gray) + diamond_corners, diamond_ids, marker_corners, marker_ids = detector.detectBoard(gray) if diamond_corners is not None: - # Draw a square around the markers - cv.aruco.drawDetectedCornersCharuco(img, diamond_corners, diamond_ids) + if use_detect_diamonds: + diamond_corners1, diamond_ids1, marker_corners1, marker_ids1 = detector.detectDiamonds(gray) + diamond_corners = diamond_corners1[0] + diamond_ids = np.transpose(diamond_ids1[0]) + marker_corners = marker_corners1 + marker_ids = marker_ids1[0] - # Draw a square around the markers - cv.aruco.drawDetectedMarkers(img, marker_corners) + # Draw marker box + if draw_marker_box: + cv.aruco.drawDetectedMarkers(img, marker_corners) + + # Draw marker id + if draw_marker_id: + cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids) + + # 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(diamond_ids) >= len(board.getIds()): - # Draw a square around the markers - cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids) - if False and diamond_ids is not None and len(diamond_ids) > 0: - obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids) - pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None) + # Estimate diamond pose + obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids) + offset = int(board.getMarkerLength() * board.getSquareLength()) + obj_points -= [offset/2, offset/2, 0] + 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 - cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10) - - cv.imshow('img', img) + cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10) + return r_vec, t_vec if __name__ == '__main__': main() diff --git a/utils.py b/utils.py new file mode 100644 index 0000000..e98187d --- /dev/null +++ b/utils.py @@ -0,0 +1,36 @@ +import numpy as np +import cv2 as cv +import math + +def rotation_matrix_to_euler_angles(R): + sy = math.sqrt(R[0,0]**2 + R[1,0]**2) + singular = sy < 1e-6 + + if not singular: + x = math.atan2(R[2,1] , R[2,2]) + y = math.atan2(-R[2,0], sy) + z = math.atan2(R[1,0], R[0,0]) + else: + # Handle singularity + x = math.atan2(-R[1,2], R[1,1]) + y = math.atan2(-R[2,0], sy) + z = 0 + return np.array([x, y, z]) + +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) + return res, r_mat[:,3] + +def cap_init(cap: cv.VideoCapture): + cap.set(cv.CAP_PROP_SETTINGS, 1) + cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G')) + 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, 30) + cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)