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
@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):
ar_tag_pose/detector_board_aruco.py
+6 -5
View File
@@ -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)
ar_tag_pose/detector_board_charuco.py
+6 -5
View File
@@ -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)
ar_tag_pose/detector_board_main.py
+4 -1
View File
@@ -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: