added print_angle

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)

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]