jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactored

Browse Source
This commit is contained in:
jens
2025-12-03 20:58:55 +01:00
parent 580a2ca267
commit 9c3eadfca1
2 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board_main.py
+1 -2
View File
@@ -161,8 +161,7 @@ class Detector(abc.ABC):
for det in self.detector: for det in self.detector:
success, r_vec, t_vec = det.process(img) success, r_vec, t_vec = det.process(img)
if success: if success:
rot_e = to_euler(to_tf(r_vec, t_vec), axis_order="XYZ") rot = to_euler(to_tf(r_vec, t_vec), axis_order="ZXY")
rot = [round(180.0 / math.pi * v, 1) for v in rot_e]
self.print_angle(img, str(det), rot, (30, y_pos)) self.print_angle(img, str(det), rot, (30, y_pos))
y_pos += 30 y_pos += 30
ar_tag_pose/utils.py
+2 -2
View File
@@ -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]))) r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
return r_mat 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 # Transform t2 to t1 coordinate system
if t2 is None: if t2 is None:
tf = np.linalg.inv(t1) 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 # Convert to euler angles
r = R.from_rotvec(r_vec) 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 return res