diff options
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 98 |
1 files changed, 75 insertions, 23 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index a910c41..cc7e70f 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -3,6 +3,7 @@ import argparse import os, json import math +import threading from argaze import DataStructures from argaze import GazeFeatures @@ -113,6 +114,12 @@ def main(): tobii_imu.load_calibration_file(args.imu_calibration) + # Init tobii imu lock + tobii_imu_lock = threading.Lock() + + # TEST : DIFF ACC + last_accl = numpy.zeros(3) + # Init data timestamped in millisecond data_ts_ms = 0 @@ -123,10 +130,21 @@ def main(): def data_stream_callback(data_ts, data_object, data_object_type): nonlocal tobii_imu + nonlocal tobii_imu_lock nonlocal data_ts_ms + #TEST + nonlocal last_accl + data_ts_ms = data_ts / 1e3 + # Don't update imu when it is used + if tobii_imu_lock.locked(): + return + + # Lock tobii imu updates + tobii_imu_lock.acquire() + match data_object_type: case 'Gyroscope': @@ -135,21 +153,35 @@ def main(): tobii_imu.update_rotation(data_ts, data_object) + case 'Accelerometer': + pass + ''' + print('raw accelerometer(m/s2)=', data_object.value) + + # TEST : + diff_accl = last_accl - numpy.array(data_object.value) + last_accl = numpy.array(data_object.value) + print('\tdiff(cm/s2)=', 100 * numpy.linalg.norm(diff_accl)) - print('raw accelerometer=', data_object.value) + # TEST : ignore acceleration double + if numpy.linalg.norm(diff_accl) > 0.: - data_object = tobii_imu.apply_accelerometer_coefficients(data_object) + data_object = tobii_imu.apply_accelerometer_coefficients(data_object) - print('corrected accelerometer=', data_object.value) + print('corrected accelerometer(m/s2)=', data_object.value) - print('current plumb=', tobii_imu.get_plumb()) + print('current plumb=', tobii_imu.get_plumb()) - data_object = tobii_imu.apply_plumb(data_object) + data_object = tobii_imu.apply_plumb(data_object) - print('corrected accelerometer - gravity=', data_object.value) + print('corrected accelerometer - gravity(m/s2)=', data_object.value) + print('\tnorm(cm/s2)=', 100 * numpy.linalg.norm(data_object.value)) - tobii_imu.update_translation(data_ts, data_object) + tobii_imu.update_translation(data_ts, data_object) + ''' + # Unlock tobii imu updates + tobii_imu_lock.release() tobii_data_stream.reading_callback = data_stream_callback @@ -191,13 +223,24 @@ def main(): # Cube pose estimation succeed and is validated by 2 faces at least if success and validity >= 2: + # Lock tobii imu updates + tobii_imu_lock.acquire() + # Reset head rotation, translation and translation speed (cm/s) # Note : head translation speed is also estimated thanks to accelerometer sensor (see upward) tobii_imu.reset_rotation() - tobii_imu.reset_translation(translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms)) + #tobii_imu.reset_translation(translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms)) + + # Create a rotation matrix to transform cube rotation from camera referential to imu referential + F = make_rotation_matrix(*TobiiInertialMeasureUnit.CAMERA_TO_IMU_ROTATION_VECTOR) + R, _ = cv.Rodrigues(rvec) + rvec_flipped, _ = cv.Rodrigues(F.dot(R)) + + # Update head plumb orientation with flipped cube orientation + tobii_imu.rotate_plumb(rvec_flipped) - # Update head plumb orientation - tobii_imu.rotate_plumb(rvec) + # Unlock tobii imu updates + tobii_imu_lock.release() # Store cube pose aruco_cube_tvec = tvec @@ -211,23 +254,29 @@ def main(): # Use tobii glasses inertial sensors to estimate cube pose from last estimated pose + # Translate cube into imu referential + imu_tvec = aruco_cube_tvec + numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) + # Translate cube according head translation head_translation_speed, head_translation = tobii_imu.get_translation() - new_tvec = aruco_cube_tvec + head_translation + imu_tvec = imu_tvec + head_translation - #if numpy.linalg.norm(head_rotation) > 0: - # print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}') + # Rotate cube around imu origin according head rotation + imu_rvec = tobii_imu.get_rotation() * numpy.array([-1., -1., 1.]) + imu_R = make_rotation_matrix(*imu_rvec) + new_imu_tvec = imu_tvec.dot(imu_R) - # Rotate cube around origin according head rotation - R = make_rotation_matrix(*tobii_imu.get_rotation()) + # Translate back cube into camera referential + new_tvec = new_imu_tvec - numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR) - # rotate tvec ??? - #new_tvec = aruco_cube_tvec.dot(R.T) + # Rotate cube orientation (supposing cube top is up in ) + imu_rvec = tobii_imu.get_rotation() * numpy.array([1., -1., 1.]) + imu_R = make_rotation_matrix(*imu_rvec) - # rotate rvec C, _ = cv.Rodrigues(aruco_cube_rvec) - C = C.dot(R) + C = C.dot(imu_R) new_rvec, _ = cv.Rodrigues(C) + #new_rvec = aruco_cube_rvec # Set cube pose estimation aruco_cube.set_pose(tvec = new_tvec, rvec = new_rvec) @@ -236,20 +285,23 @@ def main(): raise UserWarning('Cube pose estimation fails.') + # Get final estimated cube pose + tvec, rvec, _, _ = aruco_cube.get_pose() + # Project AOI 3 scene onto camera frame # DON'T APPLY CAMERA DISTORSION : it projects points which are far from the frame into it # This hack isn't realistic but as the gaze will mainly focus on centered AOI, where the distorsion is low, it is acceptable. - aoi2D_scene = aoi3D_scene.project(aruco_cube_tvec, aruco_cube_rvec, aruco_camera.get_K()) + aoi2D_scene = aoi3D_scene.project(tvec, rvec, aruco_camera.get_K()) # Draw projected scene - aoi2D_scene.draw(visu_frame.matrix) + #aoi2D_scene.draw(visu_frame.matrix) # Draw markers pose estimation - aruco_tracker.draw_tracked_markers(visu_frame.matrix) + #aruco_tracker.draw_tracked_markers(visu_frame.matrix) # Draw cube pose estimation (without camera distorsion) - aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), (0, 0, 0, 0)) + aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), (0, 0, 0, 0), draw_faces=False) # Warn about cube pose validity if not aruco_cube_validity: |