Working on accelerometer sensor data acquisition.

1 files 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))