From c08aa2d9bfe18e8b530c665b2b099b8af6d01cb2 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Fri, 5 Dec 2025 17:21:04 +0100 Subject: [PATCH] - toggle pose axes at board center or upper left - BoardDetector: removed r_vecs, t_vecs frim patrameter --- ar_tag_pose/detector_board.py | 2 +- ar_tag_pose/detector_board_aruco.py | 11 ++++++----- ar_tag_pose/detector_board_charuco.py | 11 ++++++----- ar_tag_pose/detector_board_main.py | 5 ++++- 4 files changed, 17 insertions(+), 12 deletions(-) diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py index b053b31..fa1d36c 100644 --- a/ar_tag_pose/detector_board.py +++ b/ar_tag_pose/detector_board.py @@ -48,7 +48,7 @@ class BoardDetector(abc.ABC): pass @abc.abstractmethod - def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True) -> (bool, np.ndarray, np.ndarray): + def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True) -> (bool, np.ndarray, np.ndarray): pass def is_calibrated(self): diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 98348f1..68a4500 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -48,7 +48,7 @@ class BoardDetectorAruco(BoardDetector): return success, obj_points, img_points - def process(self, image: np.ndarray, r_vecs: np.ndarray=None, t_vecs: np.ndarray=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False): + def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): pose = False r_vec = None t_vec = None @@ -78,10 +78,11 @@ class BoardDetectorAruco(BoardDetector): # Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients) obj_points, img_points = self.board.matchImagePoints(corners, ids) - offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) - offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) - obj_points -= [offset_w/2, offset_h/2, 0] - pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=r_vecs, tvec=t_vecs) + if pose_center: + offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) + offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) + obj_points -= [offset_w/2, offset_h/2, 0] + pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=None, tvec=None) if pose: # Draw Axis cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5) diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py index 1658e89..0ac721a 100644 --- a/ar_tag_pose/detector_board_charuco.py +++ b/ar_tag_pose/detector_board_charuco.py @@ -49,7 +49,7 @@ class BoardDetectorCharuco(BoardDetector): return success, obj_points, img_points - def process(self, image: np.ndarray, r_vecs: np.ndarray=None, t_vecs: np.ndarray=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False): + def process(self, image: np.ndarray, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False, pose_center: bool = True): pose = False r_vec = None t_vec = None @@ -76,12 +76,13 @@ class BoardDetectorCharuco(BoardDetector): if len(charuco_ids) >= len(self.board.getIds()): obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids) - offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength() - offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength() - obj_points -= [offset_w/2, offset_h/2, 0] + if pose_center: + offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength() + offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength() + obj_points -= [offset_w/2, offset_h/2, 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) + rvec=None, tvec=None, flags=cv.SOLVEPNP_ITERATIVE) if pose: # Draw cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10) diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 44e451b..a2bbdae 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -135,6 +135,7 @@ class Detector(abc.ABC): frame_go = True frame_num = 0 out = None + pose_center = True while cap.isOpened(): if self.src_images: cap.set(cv.CAP_PROP_POS_FRAMES, frame_num) @@ -173,11 +174,13 @@ class Detector(abc.ABC): elif k == ord('x'): for det in self.detector: det.calibration_clear() + elif k == ord('f'): + pose_center = not pose_center img_scaled = img y_pos = 30 for det in self.detector: - success, r_vec, t_vec = det.process(img) + success, r_vec, t_vec = det.process(img, True, False, False, pose_center) img_scaled = cv.resize(img, dsize=None, fx=0.25, fy=0.25) if success: if len(self.rotation_order) == 3: