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

added smoothing for rotation display

Browse Source
This commit is contained in:
jens
2025-12-11 21:03:44 +01:00
parent 6c8fc36819
commit a374c3b3fa
1 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ar_tag_pose/detector_board_main.py
+6 -2
View File
@@ -154,6 +154,7 @@ class Detector(abc.ABC):
frame_num = 0 frame_num = 0
out = None out = None
pose_center = True pose_center = True
rot = np.array([0,0,0])
while cap.isOpened(): while cap.isOpened():
if self.src_images: if self.src_images:
cap.set(cv.CAP_PROP_POS_FRAMES, frame_num) cap.set(cv.CAP_PROP_POS_FRAMES, frame_num)
@@ -195,16 +196,19 @@ class Detector(abc.ABC):
k = 800 / img.shape[1] k = 800 / img.shape[1]
y_pos = 30 y_pos = 30
alpha_rot = 0.3
for det in self.detector: for det in self.detector:
success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center) success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center)
if success: if success:
if len(self.rotation_order) == 3: if len(self.rotation_order) == 3:
rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order) _rot = to_euler(to_tf(r_vec, t_vec), rotation_order=self.rotation_order)
rot = (1-alpha_rot)*rot + alpha_rot*_rot
self.print_angle(img, f"{str(det)}-{self.rotation_order}", rot, (30, int(y_pos/k)), self.rotation_order, 1./k) self.print_angle(img, f"{str(det)}-{self.rotation_order}", rot, (30, int(y_pos/k)), self.rotation_order, 1./k)
y_pos += 30 y_pos += 30
else: else:
for rotation_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']: for rotation_order in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX']:
rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rotation_order) _rot = to_euler(to_tf(r_vec, t_vec), rotation_order=rotation_order)
rot = (1 - alpha_rot) * rot + alpha_rot * _rot
self.print_angle(img,f"{str(det)}-{rotation_order}", rot, (30, int(y_pos/k)), rotation_order, 1./k) self.print_angle(img,f"{str(det)}-{rotation_order}", rot, (30, int(y_pos/k)), rotation_order, 1./k)
y_pos += 30 y_pos += 30