From a6ea630f60f51582c4d6ae61488657ee66cfb23f Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Sun, 30 Nov 2025 15:17:04 +0100 Subject: [PATCH] - add BoardDetectorCharuco - refactored --- ar_tag_pose/detector_board.py | 2 - ar_tag_pose/detector_board_aruco.py | 2 + ar_tag_pose/detector_board_charuco.py | 88 +++++++++++++++++++++++++++ ar_tag_pose/detector_board_main.py | 27 ++++---- 4 files changed, 104 insertions(+), 15 deletions(-) create mode 100644 ar_tag_pose/detector_board_charuco.py diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index 93447ed..9ae5a32 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -10,8 +10,6 @@ import mimetypes RESULTS_FOLDER = "results" class BoardDetector(abc.ABC): - detector: cv.aruco.ArucoDetector | cv.aruco.CharucoDetector = None - board: cv.aruco.GridBoard | cv.aruco.CharucoBoard = None name = None mtx = None dist = None diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 3ae4ce8..20c971a 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -11,6 +11,8 @@ def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100 return detector, board class BoardDetectorAruco(BoardDetector): + detector: cv.aruco.ArucoDetector = None + board: cv.aruco.GridBoard = None dict_type = None def __init__(self, name: str, dict_type: int, board_size: tuple[int, int], marker_ids: list[int]=None): BoardDetector.__init__(self, name) diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py new file mode 100644 index 0000000..340d37f --- /dev/null +++ b/ar_tag_pose/detector_board_charuco.py @@ -0,0 +1,88 @@ +import cv2 as cv +import numpy as np +from ar_tag_pose.detector_board import BoardDetector +from ar_tag_pose.aruco_types import ARUCO_DICT + +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) + params = cv.aruco.CharucoParameters() + board = cv.aruco.CharucoBoard(size=board_size, squareLength=square_length, markerLength=marker_length, dictionary=aruco_dict) + detector = cv.aruco.CharucoDetector(board, params) + return detector, board + +class BoardDetectorCharuco(BoardDetector): + detector: cv.aruco.CharucoDetector = None + board: cv.aruco.CharucoBoard = None + dict_type = None + def __init__(self, name: str, dict_type: int, board_size: tuple[int, int], marker_ids: list[int]=None): + BoardDetector.__init__(self, name) + self.dict_type = dict_type + self.board_size = board_size + self.detector, self.board = factory(dict_type, board_size) + + def get_board_name(self): + grid_size = self.board.getChessboardSize() + keyname = [key for key, val in ARUCO_DICT.items() if val == self.dict_type][0] + return f"{self.name}_w{grid_size[0]}_h{grid_size[1]}_{keyname}" + + def get_board_image(self, margin_length: int=10) -> np.ndarray: + image_size = self._image_size(margin_length) + image = self.board.generateImage(image_size, None, margin_length, 1) + return image + + def match_points(self, image: cv.typing.MatLike): + success = False + obj_points = None + img_points = None + corners, ids, _, _ = self.detector.detectBoard(image) + if corners is not None: + if len(ids) >= len(self.board.getIds()): + _obj_points, _img_points = self.board.matchImagePoints(corners, ids) + if len(_obj_points) > 0 and len(_img_points) > 0: + success = True + obj_points = _obj_points + img_points = _img_points + + return success, obj_points, img_points + + def process(self, image: cv.typing.MatLike, r_vecs: np.ndarray=None, t_vecs: np.ndarray=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False): + pose = False + r_vec = None + t_vec = None + + gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY) + charuco_corners, charuco_ids, marker_corners, marker_ids = self.detector.detectBoard(gray) + + if charuco_corners is not None: + + # Draw marker box + if draw_marker_box: + cv.aruco.drawDetectedMarkers(image, marker_corners) + + # Draw marker id + if draw_marker_id: + cv.aruco.drawDetectedCornersCharuco(image, charuco_corners, charuco_ids) + + # Draw marker axis + if draw_marker_axis and self.is_calibrated(): + _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1, + cameraMatrix=self.mtx, distCoeffs=self.dist) + for i in range(len(_r_vecs)): + cv.drawFrameAxes(image, self.mtx, self.dist, _r_vecs[i], _t_vecs[i], 0.05) + + if len(charuco_ids) >= len(self.board.getIds()): + obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids) + offset = int(self.board.getMarkerLength() * self.board.getSquareLength()) + obj_points -= [offset, offset, 0] + if self.is_calibrated(): + pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, useExtrinsicGuess=False, + rvec=r_vecs, tvec=t_vecs, flags=cv.SOLVEPNP_ITERATIVE) + if pose: + # Draw + cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10) + + return pose, r_vec, t_vec + + def _image_size(self, margin_length: int): + return 800, 600 + diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 31b4119..df6ef0d 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -3,17 +3,16 @@ import abc import math import cv2 as cv import numpy as np -from pathlib import Path - -from ar_tag_pose.detector_board_aruco import BoardDetectorAruco -from ar_tag_pose.utils import to_board_pose_euler -import mimetypes -from detector_board import BoardDetector import argparse +import mimetypes +from pathlib import Path +from ar_tag_pose.utils import to_board_pose_euler +from detector_board import BoardDetector +from detector_board_aruco import BoardDetectorAruco +from detector_board_charuco import BoardDetectorCharuco RESULTS_FOLDER = "results" - 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')) @@ -178,15 +177,17 @@ def main(): help="Margin length of board [Pixels]") args = vars(ap.parse_args()) - board_detector = None + board_detector = [] if "Aruco" in args["type"]: - board_detector = (BoardDetectorAruco - ("Det-0", args["dict_type"], (args["board_width"], args["board_height"]))) + for n in range(2): + det = BoardDetectorAruco(f"Aruco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"])) + board_detector.append(det) elif "Charuco" in args["type"]: - board_detector = (BoardDetectorAruco - ("Det-0", args["dict_type"], (args["board_width"], args["board_height"]))) + for n in range(2): + det = BoardDetectorCharuco(f"Charuco_det-{n}", args["dict_type"], (args["board_width"], args["board_height"])) + board_detector.append(det) - detector = Detector(args["source"], [board_detector]) + detector = Detector(args["source"], board_detector) detector.board_image_generate(margin_length=args["margin_length"]) detector.calibrate_load() detector.process()