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

added sophisticated print of rotation orders

Browse Source
This commit is contained in:
jens
2025-12-03 21:59:15 +01:00
parent cea4edb96b
commit 4cbf5c4623
1 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board_main.py
+16 -9
View File
@@ -25,7 +25,7 @@ def cap_init(cap: cv.VideoCapture):
class Detector(abc.ABC): class Detector(abc.ABC):
def __init__(self, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str="XYZ"): def __init__(self, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str=''):
self.detector = detector self.detector = detector
self.out_file = out_file self.out_file = out_file
self.rotation_order = rotation_order self.rotation_order = rotation_order
@@ -59,11 +59,12 @@ class Detector(abc.ABC):
cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
@staticmethod @staticmethod
def print_angle(img: np.ndarray, title: str, angle: np.array, org: tuple[int, int]): def print_angle(img: np.ndarray, title: str, angle: np.array, org: tuple[int, int], rot_order: str = 'XYZ'):
indices = {k: n for n, k in enumerate(rot_order)}
colors = [(64, 64, 255), (64, 255, 64), (255, 64, 64)]
size = draw_text(img, f"{title}:", org, color=(128, 128, 128)) size = draw_text(img, f"{title}:", org, color=(128, 128, 128))
size = draw_text(img, f"X: {angle[0]:.1f}", (size[0] + 10, org[1]), color=(64, 64, 255)) for axis, color in zip(list("XYZ"), colors):
size = draw_text(img, f"Y: {angle[1]:.1f}", (size[0] + 10, org[1]), color=(64, 255, 64)) size = draw_text(img, f"{axis}: {angle[indices[axis]]:.1f}", (size[0] + 10, org[1]), color=color)
draw_text(img, f"Z: {angle[2]:.1f}", (size[0] + 10, org[1]), color=(255, 64, 64))
def board_image_generate(self, margin_length: int=0): def board_image_generate(self, margin_length: int=0):
for det in self.detector: for det in self.detector:
@@ -162,10 +163,16 @@ class Detector(abc.ABC):
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)
if success: if success:
for rot_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']: if len(self.rotation_order) == 3:
rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rot_order) rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order)
self.print_angle(img, f"{str(det)}-{rot_order}", rot, (30, y_pos)) self.print_angle(img, f"{str(det)}-{self.rotation_order}", rot, (30, y_pos), self.rotation_order)
y_pos += 30 y_pos += 30
else:
for rotation_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']:
rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rotation_order)
self.print_angle(img, f"{str(det)}-{rotation_order}", rot, (30, y_pos), rotation_order)
y_pos += 30
img_scaled = cv.resize(img, dsize=None, fx=0.5, fy=0.5) img_scaled = cv.resize(img, dsize=None, fx=0.5, fy=0.5)
cv.imshow('img', img_scaled) cv.imshow('img', img_scaled)
@@ -230,7 +237,7 @@ def main():
action='store_true', action='store_true',
help="Enable video output") help="Enable video output")
ap.add_argument("-r", "--rotation_order", type=str, ap.add_argument("-r", "--rotation_order", type=str,
default='XYZ', default='',
help="Order of Euler rotations") help="Order of Euler rotations")
args = ap.parse_args() args = ap.parse_args()
var_args = vars(args) var_args = vars(args)