diff --git a/ar_tag_pose/detector_board_main.py b/ar_tag_pose/detector_board_main.py index 87b6b51..21f387c 100644 --- a/ar_tag_pose/detector_board_main.py +++ b/ar_tag_pose/detector_board_main.py @@ -154,6 +154,7 @@ class Detector(abc.ABC): frame_num = 0 out = None pose_center = True + rot = np.array([0,0,0]) while cap.isOpened(): if self.src_images: cap.set(cv.CAP_PROP_POS_FRAMES, frame_num) @@ -195,16 +196,19 @@ class Detector(abc.ABC): k = 800 / img.shape[1] y_pos = 30 + alpha_rot = 0.3 for det in self.detector: success, r_vec, t_vec = det.process(img, self.calibration, True, False, False, pose_center) if success: 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) y_pos += 30 else: 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) y_pos += 30