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

fixed: per axis Euler angles are now correct!

Browse Source
This commit is contained in:
jens
2025-12-03 17:32:45 +01:00
parent e47018229f
commit facc115e30
3 changed files with 13 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/board_pose.py
+2 -2
View File
@@ -4,7 +4,7 @@ import math
import cv2 as cv
import numpy as np
from pathlib import Path
from ar_tag_pose.utils import to_board_pose_euler
from ar_tag_pose.utils import to_euler
import mimetypes
RESULTS_FOLDER = "results"
@@ -155,7 +155,7 @@ class BoardPose(abc.ABC):
success, r_vec, t_vec = self._process(img)
if success:
r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec)
r_vec_obj, _ = to_euler(r_vec, t_vec)
rot = [round(180.0 / math.pi * v, 1) for v in r_vec_obj]
print("\r ", end='')
print(f"\rTilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}", end='')
ar_tag_pose/detector_board_main.py
+2 -8
View File
@@ -7,7 +7,7 @@ import argparse
import mimetypes
import json
from pathlib import Path
from ar_tag_pose.utils import to_tf, to_board_pose_euler
from ar_tag_pose.utils import to_tf, to_euler
from detector_board import BoardDetector
from detector_board_aruco import BoardDetectorAruco
from detector_board_charuco import BoardDetectorCharuco
@@ -105,8 +105,6 @@ class Detector(abc.ABC):
out = None
r_vec = None
t_vec = None
tvec_world = None
rvec_world = None
while cap.isOpened():
if self.src_images:
cap.set(cv.CAP_PROP_POS_FRAMES, frame_num)
@@ -156,11 +154,7 @@ class Detector(abc.ABC):
for det in self.detector:
success, r_vec, t_vec = det.process(img)
if success:
if rvec_world is not None:
rot_e = to_board_pose_euler(to_tf(rvec_world, tvec_world), to_tf(r_vec, t_vec))
else:
rot_e = to_board_pose_euler(to_tf(r_vec, t_vec))
rot_e = to_euler(to_tf(r_vec, t_vec))
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)
ar_tag_pose/utils.py
+9 -6
View File
@@ -1,8 +1,6 @@
import numpy as np
import cv2 as cv
import math
import vg
from scipy.spatial.transform import RigidTransform as Tf
from scipy.spatial.transform import Rotation as R
def rotation_matrix_to_euler_angles(R: np.ndarray) -> np.ndarray:
@@ -27,15 +25,20 @@ 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_board_pose_euler(t1: np.ndarray, t2: np.ndarray | None = None) -> np.ndarray:
def to_euler(t1: np.ndarray, t2: np.ndarray | None = None, euler_axis: str= "XYZ") -> np.ndarray:
# Transform t2 to t1 coordinate system
if t2 is None:
tf = np.linalg.inv(t1)
else:
tf = np.linalg.inv(t1)*t2
res = np.array([0,0,0], dtype=np.float64)
# res = rotation_matrix_to_euler_angles(tf)
cv.Rodrigues(tf[0:3, 0:3], res)
# Get new r_vec from Rmat = tf[0:3, 0:3]
r_vec = np.array([0,0,0], dtype=np.float64)
cv.Rodrigues(tf[0:3, 0:3], r_vec)
# Convert to euler angles
r = R.from_rotvec(r_vec)
res = R.as_euler(r, euler_axis)
return res