diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/utils/tobii_stream_arcube_display.py | 97 |
1 files changed, 16 insertions, 81 deletions
diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index 4ba59af..784c704 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -104,7 +104,8 @@ def main(): print(f'\nArUcoTracker configuration for markers detection:') aruco_tracker.print_configuration() - # Create tobii imu handler + # Create tobii imu handler to track head translation changes when arcuco cube pose can't be estimated + # So, the resulting head translation is relative to last pose estimation (it's not an absolute position) tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit() # Load optional imu calibration file @@ -112,22 +113,6 @@ def main(): tobii_imu.load_calibration_file(args.imu_calibration) - # Init gyroscope processing to track head rotation changes when arcuco cube pose can't be estimated - # So, the resulting head rotation is relative to last pose estimation (it's not an absolute orientation) - tobii_imu.reset_rotation() - - # Init accelerotmeter processing to track head translation changes when arcuco cube pose can't be estimated - # So, the resulting head translation is relative to last pose estimation (it's not an absolute position) - last_accelerometer = numpy.zeros(3) - last_accelerometer_ts_ms = 0 - accelerometer_offset = numpy.zeros(3) - accelerometer_offset_smooth = 0.2 - earth_gravity = numpy.array([0, -9.81, 0]) - head_plumb = numpy.zeros(3) - head_translation_speed = numpy.zeros(3) - last_head_translation_speed = numpy.zeros(3) - head_translation = numpy.zeros(3) - # Init data timestamped in millisecond data_ts_ms = 0 @@ -151,68 +136,20 @@ def main(): tobii_imu.update_rotation(data_ts, data_object) case 'Accelerometer': - - nonlocal last_accelerometer - nonlocal last_accelerometer_ts_ms - nonlocal accelerometer_offset - nonlocal earth_gravity - nonlocal head_plumb - nonlocal head_translation_speed - nonlocal last_head_translation_speed - nonlocal head_translation - - # Check head plumb as we need to know it to remove earth gravity - - #print('head_plumb=', head_plumb) - #print('numpy.linalg.norm(head_plumb)=', numpy.linalg.norm(head_plumb)) - #print('numpy.linalg.norm(earth_gravity)=', numpy.linalg.norm(earth_gravity)) - - if math.isclose(numpy.linalg.norm(head_plumb), numpy.linalg.norm(earth_gravity), abs_tol=1e-3): - - #print('raw accelerometer=',numpy.array(data_object.value)) - #print('head_plumb=', head_plumb) - # Remove gravity along head plumb to accelerometer - current_accelerometer = numpy.array(data_object.value) - head_plumb - - # Convert m/s2 into cm/ms2 - current_accelerometer = numpy.array(data_object.value) * 1e-4 - - # Init accelerometer integration - if last_accelerometer_ts_ms == 0: - - last_accelerometer = current_accelerometer - last_accelerometer_ts_ms = data_ts_ms - - # Calculate elapsed time - delta_time = data_ts_ms - last_accelerometer_ts_ms - - # Update accelerometer offset smoothly and reset head translation speed when head translation speed is close to zero - # Note : head translation speed is simultaneously estimated thanks to cube pose (see below) - print('numpy.linalg.norm(head_translation_speed)=',numpy.linalg.norm(head_translation_speed)) - - if numpy.linalg.norm(head_translation_speed) < 1e-3: - - accelerometer_offset = accelerometer_offset * accelerometer_offset_smooth + current_accelerometer * (1 - accelerometer_offset_smooth) - print('> accelerometer_offset=',accelerometer_offset) - head_translation_speed = numpy.zeros(3) + print('raw accelerometer=', data_object.value) + + data_object = tobii_imu.apply_accelerometer_coefficients(data_object) - # Integrate accelerometer with offset compensation - head_translation_speed += (last_accelerometer - accelerometer_offset) * delta_time - head_translation += last_head_translation_speed * delta_time + print('corrected accelerometer=', data_object.value) - print('current_accelerometer(cm/ms2)=',current_accelerometer) - print('head_translation_speed(cm/s)=',head_translation_speed) - print('head_translation(cm)=',head_translation) + print('current plumb=', tobii_imu.get_plumb()) - # Store current as last - last_accelerometer = current_accelerometer - last_accelerometer_ts_ms = data_ts_ms - last_head_translation_speed = head_translation_speed + data_object = tobii_imu.apply_plumb(data_object) - #else: + print('corrected accelerometer - gravity=', data_object.value) - # print('no valid head plumb') + tobii_imu.update_translation(data_ts, data_object) tobii_data_stream.reading_callback = data_stream_callback @@ -255,15 +192,12 @@ def main(): if success and validity >= 2: # Reset head rotation, translation and translation speed (cm/s) - # Note : head translation speed is simultaneously estimated thanks to accelerometer sensor (see upward) + # 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)) - head_translation = numpy.zeros(3) - head_translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms) - - # Rotate head plumb orientation - C, _ = cv.Rodrigues(rvec) - head_plumb = -C.dot(earth_gravity) + # Update head plumb orientation + tobii_imu.rotate_plumb(rvec) # Store cube pose aruco_cube_tvec = tvec @@ -278,7 +212,8 @@ def main(): # Use tobii glasses inertial sensors to estimate cube pose from last estimated pose # Translate cube according head translation - new_tvec = aruco_cube_tvec #+ head_translation + head_translation_speed, head_translation = tobii_imu.get_translation() + new_tvec = aruco_cube_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}') |