From 89ce7d43375f4c16707e93816c70e2ba59d69adc Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 3 Dec 2025 17:19:44 +0100 Subject: [PATCH] fixed axis draw for board center --- ar_tag_pose/detector_board_aruco.py | 3 +++ ar_tag_pose/detector_board_charuco.py | 5 +++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py index 878b4ca..98348f1 100644 --- a/ar_tag_pose/detector_board_aruco.py +++ b/ar_tag_pose/detector_board_aruco.py @@ -78,6 +78,9 @@ 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: # Draw Axis diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py index 4aff43f..1658e89 100644 --- a/ar_tag_pose/detector_board_charuco.py +++ b/ar_tag_pose/detector_board_charuco.py @@ -76,8 +76,9 @@ class BoardDetectorCharuco(BoardDetector): 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] + 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)