From cea4edb96bb63b68a860dc6b165c0015027479ad Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Wed, 3 Dec 2025 21:29:41 +0100
Subject: [PATCH] display all rotation orders

---
 ar_tag_pose/detector_board_main.py | 15 ++++++++++-----
 ar_tag_pose/utils.py               |  4 ++--
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py
index 07ad452..8f1054d 100644
--- a/ar_tag_pose/detector_board_main.py
+++ b/ar_tag_pose/detector_board_main.py
@@ -25,9 +25,10 @@ def cap_init(cap: cv.VideoCapture):
 
 
 class Detector(abc.ABC):
-	def __init__(self, source: str, detector: list[BoardDetector], out_file: str|None = None):
+	def __init__(self, source: str, detector: list[BoardDetector], out_file: str|None = None, rotation_order: str="XYZ"):
 		self.detector = detector
 		self.out_file = out_file
+		self.rotation_order = rotation_order
 		self.device_id = None
 		self.src_video = None
 		self.src_images = None
@@ -161,9 +162,10 @@ class Detector(abc.ABC):
 			for det in self.detector:
 				success, r_vec, t_vec = det.process(img)
 				if success:
-					rot = to_euler(to_tf(r_vec, t_vec), axis_order="ZXY")
-					self.print_angle(img, str(det), rot, (30, y_pos))
-					y_pos += 30
+					for rot_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']:
+						rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rot_order)
+						self.print_angle(img, f"{str(det)}-{rot_order}", rot, (30, y_pos))
+						y_pos += 30
 
 			img_scaled = cv.resize(img, dsize=None, fx=0.5, fy=0.5)
 			cv.imshow('img', img_scaled)
@@ -227,6 +229,9 @@ def main():
 	ap.add_argument("-o", "--output",
 		action='store_true',
 		help="Enable video output")
+	ap.add_argument("-r", "--rotation_order", type=str,
+		default='XYZ',
+		help="Order of Euler rotations")
 	args = ap.parse_args()
 	var_args = vars(args)
 
@@ -260,7 +265,7 @@ def main():
 		out_file = f"{var_args["name"]}.mp4"
 
 	# Create Detector app
-	detector = Detector(var_args["source"], board_detector, out_file=out_file)
+	detector = Detector(var_args["source"], board_detector, out_file=out_file, rotation_order=var_args["rotation_order"])
 	detector.board_image_generate(margin_length=var_args["margin_length"])
 	detector.calibrate_load()
 	detector.process()
diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py
index 459e15c..7dcf744 100644
--- a/ar_tag_pose/utils.py
+++ b/ar_tag_pose/utils.py
@@ -25,7 +25,7 @@ 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_euler(t1: np.ndarray, t2: np.ndarray | None = None, axis_order: str= "XYZ", unit_is_degree: bool=True) -> np.ndarray:
+def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, rotation_order: str= "XYZ", unit_is_degree: bool=True) -> np.ndarray:
 	# Transform t2 to t1 coordinate system
 	if t2 is None:
 		tf = np.linalg.inv(t1)
@@ -38,7 +38,7 @@ def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, axis_order: str= "XYZ
 
 	# Convert to euler angles
 	r = R.from_rotvec(r_vec)
-	res = R.as_euler(r, axis_order, degrees=unit_is_degree)
+	res = R.as_euler(r, rotation_order, degrees=unit_is_degree)
 
 	return res