- refactore utils

- added code for experiemntal single axis rotaion (Zwischenstand)

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_board_pose_euler
+from ar_tag_pose.utils import to_tf, to_board_pose_euler
 from detector_board import BoardDetector
 from detector_board_aruco import BoardDetectorAruco
 from detector_board_charuco import BoardDetectorCharuco
@@ -103,6 +103,10 @@ class Detector(abc.ABC):
 		frame_go = True
 		frame_num = 0
 		out = None
+		r_vec = None
+		t_vec = None
+		tvec_world = None
+		rvec_world = None
 		while cap.isOpened():
 			if self.src_images:
 				cap.set(cv.CAP_PROP_POS_FRAMES, frame_num)
@@ -141,13 +145,23 @@ class Detector(abc.ABC):
 			elif k == ord('x'):
 				for det in self.detector:
 					det.calibration_clear()
+			elif k == ord('e'):
+				if t_vec is not None and r_vec is not None:
+					rvec_world, tvec_world = r_vec, t_vec
+			elif k == ord('r'):
+				tvec_world = None
+				rvec_world = None
 
 			y_pos = 30
 			for det in self.detector:
 				success, r_vec, t_vec = det.process(img)
 				if success:
-					r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec)
-					rot = [round(180.0 / math.pi * v, 1) for v in r_vec_obj]
+					if rvec_world is not None:
+						rot_e = to_board_pose_euler(to_tf(rvec_world, tvec_world), to_tf(r_vec, t_vec))
+					else:
+						rot_e = to_board_pose_euler(to_tf(r_vec, t_vec))
+
+					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)
 					y_pos += 30

ar_tag_pose/utils.py

@@ -1,8 +1,11 @@
 import numpy as np
 import cv2 as cv
 import math
+import vg
+from scipy.spatial.transform import RigidTransform as Tf
+from scipy.spatial.transform import Rotation as R
 
-def rotation_matrix_to_euler_angles(R):
+def rotation_matrix_to_euler_angles(R: np.ndarray) -> np.ndarray:
 	sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
 	singular = sy < 1e-6
 
@@ -17,11 +20,22 @@ def rotation_matrix_to_euler_angles(R):
 		z = 0
 	return np.array([x, y, z])
 
-def to_board_pose_euler(r_vec, t_vec):
+def to_tf(r_vec: np.ndarray, t_vec: np.ndarray) -> np.ndarray:
 	r_mat = np.ndarray((3, 3))
 	cv.Rodrigues(r_vec, r_mat)
 	r_mat = np.hstack((r_mat, t_vec))
 	r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
-	r_mat = np.linalg.inv(r_mat)
-	res = rotation_matrix_to_euler_angles(r_mat)
-	return res, r_mat[:,3]
+	return r_mat
+
+def to_board_pose_euler(t1: np.ndarray, t2: np.ndarray | None = None) -> np.ndarray:
+	if t2 is None:
+		tf = np.linalg.inv(t1)
+	else:
+		tf = np.linalg.inv(t1)*t2
+
+	res = np.array([0,0,0], dtype=np.float64)
+	#	res = rotation_matrix_to_euler_angles(tf)
+	cv.Rodrigues(tf[0:3, 0:3], res)
+
+	return res
+