diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 89aeb71..07ad452 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -161,8 +161,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), axis_order="XYZ") - rot = [round(180.0 / math.pi * v, 1) for v in rot_e] + 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 diff --git a/ar_tag_pose/utils.py b/ar_tag_pose/utils.py index 266d9be..459e15c 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") -> np.ndarray: +def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, axis_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) + res = R.as_euler(r, axis_order, degrees=unit_is_degree) return res