From facc115e30f0cdd8920727314c3adafb7798b0a5 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Wed, 3 Dec 2025 17:32:45 +0100
Subject: [PATCH] fixed: per axis Euler angles are now correct!

---
 ar_tag_pose/board_pose.py          |  4 ++--
 ar_tag_pose/detector_board_main.py | 10 ++--------
 ar_tag_pose/utils.py               | 15 +++++++++------
 3 files changed, 13 insertions(+), 16 deletions(-)

diff --git a/ar_tag_pose/board_pose.py b/ar_tag_pose/board_pose.py
index 4d5a965..91c27e7 100644
--- a/ar_tag_pose/board_pose.py
+++ b/ar_tag_pose/board_pose.py
@@ -4,7 +4,7 @@ import math
 import cv2 as cv
 import numpy as np
 from pathlib import Path
-from ar_tag_pose.utils import to_board_pose_euler
+from ar_tag_pose.utils import to_euler
 import mimetypes
 
 RESULTS_FOLDER = "results"
@@ -155,7 +155,7 @@ class BoardPose(abc.ABC):
 
 			success, r_vec, t_vec = self._process(img)
 			if success:
-				r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec)
+				r_vec_obj, _ = to_euler(r_vec, t_vec)
 				rot = [round(180.0 / math.pi * v, 1) for v in r_vec_obj]
 				print("\r                                                                                 ", end='')
 				print(f"\rTilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}", end='')
diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py
index 884f91a..1aba9a9 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_board_pose_euler
+from ar_tag_pose.utils import to_tf, to_euler
 from detector_board import BoardDetector
 from detector_board_aruco import BoardDetectorAruco
 from detector_board_charuco import BoardDetectorCharuco
@@ -105,8 +105,6 @@ class Detector(abc.ABC):
 		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)
@@ -156,11 +154,7 @@ class Detector(abc.ABC):
 			for det in self.detector:
 				success, r_vec, t_vec = det.process(img)
 				if success:
-					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_e = to_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)
diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py
index acc1f72..f3a333a 100644
--- a/ar_tag_pose/utils.py
+++ b/ar_tag_pose/utils.py
@@ -1,8 +1,6 @@
 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: np.ndarray) -> np.ndarray:
@@ -27,15 +25,20 @@ def to_tf(r_vec: np.ndarray, t_vec: np.ndarray) -> np.ndarray:
 	r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
 	return r_mat
 
-def to_board_pose_euler(t1: np.ndarray, t2: np.ndarray | None = None) -> np.ndarray:
+def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, euler_axis: str= "XYZ") -> np.ndarray:
+	# Transform t2 to t1 coordinate system
 	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)
+	# Get new r_vec from Rmat = tf[0:3, 0:3]
+	r_vec = np.array([0,0,0], dtype=np.float64)
+	cv.Rodrigues(tf[0:3, 0:3], r_vec)
+
+	# Convert to euler angles
+	r = R.from_rotvec(r_vec)
+	res = R.as_euler(r, euler_axis)
 
 	return res