diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 1aba9a9..e8e5ddc 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -154,7 +154,7 @@ class Detector(abc.ABC): for det in self.detector: success, r_vec, t_vec = det.process(img) if success: - rot_e = to_euler(to_tf(r_vec, t_vec)) + 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) diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py index f3a333a..1dc28de 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, euler_axis: str= "XYZ") -> np.ndarray: +def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, axis_order: str= "XYZ") -> 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, euler_axis: str= "XYZ # Convert to euler angles r = R.from_rotvec(r_vec) - res = R.as_euler(r, euler_axis) + res = R.as_euler(r, axis_order) return res