diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 61 |
1 files changed, 39 insertions, 22 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index 255961a..c77d6e9 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -12,6 +12,7 @@ from argaze.utils import MiscFeatures import cv2 as cv import numpy +#import imufusion def main(): """ @@ -72,11 +73,14 @@ def main(): print(f'\nArUcoTracker configuration for markers detection:') aruco_tracker.print_configuration() - - # Init head pose tracking - head_translation = numpy.array((0, 0, 0)) - head_rotation = numpy.array((0, 0, 0)) - + + # Init head pose estimation + accelerometer = numpy.array([]) + gyroscope = numpy.array([]) + head_translation = numpy.zeros(3) + head_rotation = numpy.zeros(3) + gravity = -9.81 + # Init data timestamped in millisecond data_ts_ms = 0 @@ -86,6 +90,8 @@ def main(): def data_stream_callback(data_ts, data_object, data_object_type): + nonlocal gyroscope + nonlocal accelerometer nonlocal head_translation nonlocal head_rotation nonlocal data_ts_ms @@ -96,16 +102,27 @@ def main(): case 'Accelerometer': - # Integrate head translation over time - #head_translation += numpy.array(data_object.value) - pass + accelerometer = numpy.array(data_object.value) - gravity + return case 'Gyroscope': - - # Integrate head rotation over time - #head_rotation += numpy.array(data_object.value) - pass + gyroscope = numpy.array(data_object.value) + return + + # Update head pose estimation when all inertial sensors are refreshed + if accelerometer.size == 3 and gyroscope.size == 3: + ''' + print('accelerometer:', accelerometer) + print('gyroscope:', gyroscope) + + print('head translation:', head_translation) + print('head rotation:', head_rotation) + ''' + # Forget old inertial sensors data + accelerometer = numpy.array([]) + gyroscope = numpy.array([]) + tobii_data_stream.reading_callback = data_stream_callback # Start streaming @@ -139,24 +156,24 @@ def main(): raise UserWarning('No marker detected') # Estimate cube pose from tracked markers - valid = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers()) + tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers()) + # When cube pose estimation fails, use inertial sensors to offset cube pose + if not success: + + #aruco_cube.offset_pose(tvec=[0, 3, 0], rvec=[0, 0, 0]) + raise UserWarning('Cube pose estimation failed.') + # Draw markers pose estimation - aruco_tracker.draw_tracked_markers(visu_frame.matrix) + #aruco_tracker.draw_tracked_markers(visu_frame.matrix) # Draw cube pose estimation aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), aruco_camera.get_D()) # Warn about cube pose validity - if not valid: - - raise UserWarning('Cube pose is not validated.') + if not validity: - # Pose estimation error - except ValueError as e: - - cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1) - cv.putText(visu_frame.matrix, str(e), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA) + raise UserWarning('Cube pose estimation is not validated.') # Write warning except UserWarning as w: |