jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- toggle pose axes at board center or upper left

Browse Source 
- BoardDetector: removed r_vecs, t_vecs frim patrameter
This commit is contained in:
jens
2025-12-05 17:21:04 +01:00
parent cbceee1c61
commit c08aa2d9bf
4 changed files with 17 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board.py
+1 -1
View File
@@ -48,7 +48,7 @@ class BoardDetector(abc.ABC):
pass pass
@abc.abstractmethod @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 pass
def is_calibrated(self): def is_calibrated(self):
ar_tag_pose/detector_board_aruco.py
+6 -5
View File
@@ -48,7 +48,7 @@ class BoardDetectorAruco(BoardDetector):
return success, obj_points, img_points 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 pose = False
r_vec = None r_vec = None
t_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) # 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) obj_points, img_points = self.board.matchImagePoints(corners, ids)
offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) if pose_center:
offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation()) offset_w = self.board.getGridSize()[0] * (self.board.getMarkerLength() + self.board.getMarkerSeparation())
obj_points -= [offset_w/2, offset_h/2, 0] offset_h = self.board.getGridSize()[1] * (self.board.getMarkerLength() + self.board.getMarkerSeparation())
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, rvec=r_vecs, tvec=t_vecs) 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: if pose:
# Draw Axis # Draw Axis
cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5) cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5)
ar_tag_pose/detector_board_charuco.py
+6 -5
View File
@@ -49,7 +49,7 @@ class BoardDetectorCharuco(BoardDetector):
return success, obj_points, img_points 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 pose = False
r_vec = None r_vec = None
t_vec = None t_vec = None
@@ -76,12 +76,13 @@ class BoardDetectorCharuco(BoardDetector):
if len(charuco_ids) >= len(self.board.getIds()): if len(charuco_ids) >= len(self.board.getIds()):
obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids) obj_points, img_points = self.board.matchImagePoints(charuco_corners, charuco_ids)
offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength() if pose_center:
offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength() offset_w = self.board.getChessboardSize()[0] * self.board.getSquareLength()
obj_points -= [offset_w/2, offset_h/2, 0] offset_h = self.board.getChessboardSize()[1] * self.board.getSquareLength()
obj_points -= [offset_w/2, offset_h/2, 0]
if self.is_calibrated(): if self.is_calibrated():
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, self.mtx, self.dist, useExtrinsicGuess=False, 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: if pose:
# Draw # Draw
cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10) cv.drawFrameAxes(image, self.mtx, self.dist, r_vec, t_vec, 10)
ar_tag_pose/detector_board_main.py
+4 -1
View File
@@ -135,6 +135,7 @@ class Detector(abc.ABC):
frame_go = True frame_go = True
frame_num = 0 frame_num = 0
out = None out = None
pose_center = True
while cap.isOpened(): while cap.isOpened():
if self.src_images: if self.src_images:
cap.set(cv.CAP_PROP_POS_FRAMES, frame_num) cap.set(cv.CAP_PROP_POS_FRAMES, frame_num)
@@ -173,11 +174,13 @@ class Detector(abc.ABC):
elif k == ord('x'): elif k == ord('x'):
for det in self.detector: for det in self.detector:
det.calibration_clear() det.calibration_clear()
elif k == ord('f'):
pose_center = not pose_center
img_scaled = img img_scaled = img
y_pos = 30 y_pos = 30
for det in self.detector: 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) img_scaled = cv.resize(img, dsize=None, fx=0.25, fy=0.25)
if success: if success:
if len(self.rotation_order) == 3: if len(self.rotation_order) == 3: