Using TobiiIMU features.

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}')