From 2b0e81bf190049d58138673d0c559a502a341825 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Wed, 3 Dec 2025 20:27:09 +0100 Subject: [PATCH] added print_angle --- ar_tag_pose/detector_board_main.py | 12 +++++++++--- ar_tag_pose/utils.py | 7 +++++++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index e8e5ddc..8f274b5 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -7,7 +7,7 @@ import argparse import mimetypes import json from pathlib import Path -from ar_tag_pose.utils import to_tf, to_euler +from ar_tag_pose.utils import to_tf, to_euler, draw_text from detector_board import BoardDetector from detector_board_aruco import BoardDetectorAruco from detector_board_charuco import BoardDetectorCharuco @@ -57,6 +57,13 @@ class Detector(abc.ABC): cap.set(cv.CAP_PROP_EXPOSURE, 30) cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1) + @staticmethod + def print_angle(img: np.ndarray, title: str, angle: np.array, org: tuple[int, int]): + 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)) + size = draw_text(img, f"Y: {angle[0]:.1f}", (size[0] + 10, org[1]), color=(64, 255, 64)) + draw_text(img, f"Z: {angle[0]:.1f}", (size[0] + 10, org[1]), color=(255, 64, 64)) + def board_image_generate(self, margin_length: int=0): for det in self.detector: image = det.get_board_image(margin_length) @@ -156,8 +163,7 @@ class Detector(abc.ABC): if success: rot_e = to_euler(to_tf(r_vec, t_vec), axis_order="XYZ") rot = [round(180.0 / math.pi * v, 1) for v in rot_e] - cv.putText(img, f"{det}: Tilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}", - (30, y_pos), cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 255, 0), 2) + self.print_angle(img, str(det), rot, (30, y_pos)) y_pos += 30 img_scaled = cv.resize(img, dsize=None, fx=0.5, fy=0.5) diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py index 1dc28de..266d9be 100644 --- a/ar_tag_pose/utils.py +++ b/ar_tag_pose/utils.py @@ -42,3 +42,10 @@ def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, axis_order: str= "XYZ return res +def draw_text(img: np.ndarray, text: str, org: tuple[int,int], font: int=cv.FONT_HERSHEY_SIMPLEX, + font_scale: float=0.75, color: tuple[float,float,float]=(0, 255, 0), thickness: int=2, line_type: int=0, + bottom_left_origin: bool=False): + + size = cv.getTextSize(text, font, font_scale, thickness) + cv.putText(img, text, org, font, font_scale, color, thickness, line_type, bottom_left_origin) + return size[0][0] + org[0], size[0][1]