From 73f031a492124980b4cf2173732c20cd67e2a610 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Tue, 25 Oct 2022 16:23:56 +0200 Subject: Working on accelerometer sensor data acquisition. --- src/argaze/utils/tobii_stream_arcube_display.py | 126 ++++++++++++++++++------ 1 file changed, 95 insertions(+), 31 deletions(-) diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py index 9cd89d9..d1dc734 100644 --- a/src/argaze/utils/tobii_stream_arcube_display.py +++ b/src/argaze/utils/tobii_stream_arcube_display.py @@ -2,6 +2,7 @@ import argparse import os, json +import math from argaze import DataStructures from argaze import GazeFeatures @@ -102,18 +103,26 @@ def main(): print(f'\nArUcoTracker configuration for markers detection:') aruco_tracker.print_configuration() - # Init head movment estimation - ''' - last_accelerometer = numpy.zeros(3) - last_accelerometer_ts_ms = 0 - head_translation_speed = numpy.zeros(3) - gravity = -9.81 - ''' + # 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) last_gyroscope = numpy.zeros(3) last_gyroscope_ts_ms = 0 gyroscope_offset = numpy.zeros(3) + gyroscope_offset_smooth = 0.5 head_rotation = numpy.zeros(3) - smooth_factor = 0.5 + + + # 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 @@ -140,20 +149,22 @@ def main(): # Convert deg/s into deg/ms current_gyroscope = numpy.array(data_object.value) * 1e-3 - # Init gyroscope derivation + # Init gyroscope derivation and integration if last_gyroscope_ts_ms == 0: last_gyroscope = current_gyroscope last_gyroscope_ts_ms = data_ts_ms - # Derivate gyroscope + # Calculate elapsed time delta_time = data_ts_ms - last_gyroscope_ts_ms + + # Derivate gyroscope gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3) # Update gyroscope offset smoothly and reset head rotation when gyroscope is stable if numpy.linalg.norm(gyroscope_derivation) < 1e-5: - gyroscope_offset = gyroscope_offset * smooth_factor + current_gyroscope * (1 - smooth_factor) + gyroscope_offset = gyroscope_offset * gyroscope_offset_smooth + current_gyroscope * (1 - gyroscope_offset_smooth) head_rotation = numpy.zeros(3) # Integrate gyroscope with offset compensation @@ -163,28 +174,69 @@ def main(): last_gyroscope = current_gyroscope last_gyroscope_ts_ms = data_ts_ms - #case 'Accelerometer': - ''' + 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) - # Convert m/s2 into cm/ms2 - current_accelerometer = numpy.array(data_object.value) * 1e-4 + # Remove gravity along head plumb to accelerometer + current_accelerometer = numpy.array(data_object.value) - head_plumb - # Init accelerometer time integration - if last_accelerometer_ts_ms == 0: + # 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) + + # Integrate accelerometer with offset compensation + head_translation_speed += (last_accelerometer - accelerometer_offset) * delta_time + head_translation += last_head_translation_speed * delta_time + + print('current_accelerometer(cm/ms2)=',current_accelerometer) + print('head_translation_speed(cm/s)=',head_translation_speed) + print('head_translation(cm)=',head_translation) + + # Store current as last last_accelerometer = current_accelerometer last_accelerometer_ts_ms = data_ts_ms + last_head_translation_speed = head_translation_speed - # Integrate accelerometer values in time to get head translation speed (cm/ms) - else: + #else: - head_translation_speed += (last_accelerometer + current_accelerometer) * (data_ts_ms - last_accelerometer_ts_ms) / 2 - - # print(head_translation_speed) - ''' + # print('no valid head plumb') tobii_data_stream.reading_callback = data_stream_callback @@ -223,21 +275,33 @@ def main(): # Estimate cube pose from tracked markers tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers()) - # Cube estimation succeed and validated by 2 faces at least: store cube pose + # Cube pose estimation succeed and is validated by 2 faces at least 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) + head_rotation = numpy.zeros(3) + 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) + + # Store cube pose aruco_cube_tvec = tvec aruco_cube_rvec = rvec aruco_cube_success = success aruco_cube_validity = validity + aruco_cube_ts_ms = video_ts_ms - # reset head rotation - head_rotation = numpy.zeros(3) - - # Cube pose estimation fails: use tobii glasses inertial sensors to estimate cube pose from last estimated pose + # Cube pose estimation fails elif aruco_cube_success: - # TODO : Translate cube according head translation speed ? + # 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 #if numpy.linalg.norm(head_rotation) > 0: # print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}') @@ -254,7 +318,7 @@ def main(): new_rvec, _ = cv.Rodrigues(C) # Set cube pose estimation - aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = new_rvec) + aruco_cube.set_pose(tvec = new_tvec, rvec = new_rvec) else: @@ -270,7 +334,7 @@ def main(): 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)) -- cgit v1.1