From e8bb60d828237de93c5db3ec8239c6abc5c29d15 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Sat, 13 Dec 2025 10:46:22 +0100
Subject: [PATCH] - fixed buggy filtering of detector rotation - added
 SmoothRotation class - rotation object is member of each detector

---
 ar_tag_pose/detector_board.py         |  2 ++
 ar_tag_pose/detector_board_aruco.py   |  1 +
 ar_tag_pose/detector_board_charuco.py |  1 +
 ar_tag_pose/detector_board_main.py    | 12 ++++--------
 ar_tag_pose/smooth_rotation.py        | 18 ++++++++++++++++++
 5 files changed, 26 insertions(+), 8 deletions(-)
 create mode 100644 ar_tag_pose/smooth_rotation.py

diff --git a/ar_tag_pose/detector_board.py b/ar_tag_pose/detector_board.py
index aa89fd1..90ae044 100644
--- a/ar_tag_pose/detector_board.py
+++ b/ar_tag_pose/detector_board.py
@@ -1,6 +1,7 @@
 import abc
 import numpy as np
 from calibration import Calibration
+from smooth_rotation import SmoothRotation
 
 RESULTS_FOLDER = "results"
 
@@ -11,6 +12,7 @@ class BoardDetector(abc.ABC):
 	def __init__(self, name: str, board_id: str, n_syms: int):
 		self.name = name
 		self.board_id = board_id
+		self.rotation = SmoothRotation()
 
 		# Create IDs
 		offset = int(board_id)*n_syms
diff --git a/ar_tag_pose/detector_board_aruco.py b/ar_tag_pose/detector_board_aruco.py
index 1e8a4fd..ba4164b 100644
--- a/ar_tag_pose/detector_board_aruco.py
+++ b/ar_tag_pose/detector_board_aruco.py
@@ -88,6 +88,7 @@ class BoardDetectorAruco(BoardDetector):
 			if pose:
 				# Draw Axis
 				cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, self.board.getMarkerLength()*1.5)
+				self.rotation.set_vec(r_vec, t_vec)
 
 		return pose, r_vec, t_vec
 
diff --git a/ar_tag_pose/detector_board_charuco.py b/ar_tag_pose/detector_board_charuco.py
index 059cb90..7337001 100644
--- a/ar_tag_pose/detector_board_charuco.py
+++ b/ar_tag_pose/detector_board_charuco.py
@@ -88,6 +88,7 @@ class BoardDetectorCharuco(BoardDetector):
 					if pose:
 						# Draw
 						cv.drawFrameAxes(image, calibration.mtx, calibration.dist, r_vec, t_vec, 10)
+						self.rotation.set_vec(r_vec, t_vec)
 
 		return pose, r_vec, t_vec
 
diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py
index d471f12..c135a47 100644
--- a/ar_tag_pose/detector_board_main.py
+++ b/ar_tag_pose/detector_board_main.py
@@ -6,7 +6,7 @@ import argparse
 import mimetypes
 import json
 from pathlib import Path
-from ar_tag_pose.utils import to_tf, to_euler, draw_text
+from ar_tag_pose.utils import draw_text
 from calibration import Calibration
 from detector_board import BoardDetector
 from detector_board_aruco import BoardDetectorAruco
@@ -171,9 +171,7 @@ class Detector(abc.ABC):
 		frame_num = 0
 		out = None
 		pose_center = True
-		rot = np.array([0,0,0])
-		# ToDo: fixed filter with multiple detectors
-		alpha_rot = 1.0
+		alpha_rot = 0.3
 		scale_target_width = 800
 		show_undistorted_image = False
 		read_key = ReadKey()
@@ -229,14 +227,12 @@ class Detector(abc.ABC):
 				success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center)
 				if success:
 					if len(self.rotation_order) == 3:
-						_rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order)
-						rot = (1-alpha_rot)*rot + alpha_rot*_rot
+						rot = det.rotation.get(self.rotation_order, alpha_rot)
 						self.print_angle(img, f"{str(det)}-{self.rotation_order}", rot, (30, int(y_pos/k_scale)), self.rotation_order, 1./k_scale)
 						y_pos += 30
 					else:
 						for rotation_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']:
-							_rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rotation_order)
-							rot = (1 - alpha_rot) * rot + alpha_rot * _rot
+							rot = det.rotation.get(rotation_order, alpha_rot)
 							self.print_angle(img,f"{str(det)}-{rotation_order}", rot, (30, int(y_pos/k_scale)), rotation_order, 1./k_scale)
 							y_pos += 30
 
diff --git a/ar_tag_pose/smooth_rotation.py b/ar_tag_pose/smooth_rotation.py
new file mode 100644
index 0000000..f38e5c8
--- /dev/null
+++ b/ar_tag_pose/smooth_rotation.py
@@ -0,0 +1,18 @@
+import numpy as np
+from ar_tag_pose.utils import to_tf, to_euler
+
+
+class SmoothRotation:
+	def __init__(self):
+		self.rot = np.array([0,0,0])
+		self.r_vec = np.array([0,0,0])
+		self.t_vec = np.array([0,0,0])
+
+	def set_vec(self, r_vec: np.ndarray, t_vec: np.ndarray):
+		self.r_vec = r_vec
+		self.t_vec = t_vec
+
+	def get(self, rotation_order: str, alpha: float=1.0) -> np.ndarray:
+		rot = to_euler(to_tf(self.r_vec, self.t_vec), rotation_order=rotation_order)
+		self.rot = (1 - alpha) * self.rot + alpha * rot
+		return self.rot