Updating imu uses.

2 files changed, 74 insertions, 26 deletions

diff --git a/src/argaze/utils/tobii_imu_calibrate.py b/src/argaze/utils/tobii_imu_calibrate.py
index 1422e78..369134e 100644
--- a/src/argaze/utils/tobii_imu_calibrate.py
+++ b/src/argaze/utils/tobii_imu_calibrate.py
@@ -21,6 +21,7 @@ def main():
     # manage arguments
     parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
     parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip')
+    parser.add_argument('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath')
     parser.add_argument('-b', '--buffer_size', metavar='BUFFER_SIZE', type=int, default=500, help='number of samples to store into calibration buffer')
     parser.add_argument('-o', '--output', metavar='OUT', type=str, default='imu.json', help='destination filepath')
     args = parser.parse_args()
@@ -41,6 +42,11 @@ def main():
     # Create tobii imu handler
     tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
 
+    # Load optional imu calibration file
+    if args.imu_calibration != None:
+
+        tobii_imu.load_calibration_file(args.imu_calibration)
+
     # Enable tobii data stream 
     tobii_data_stream = tobii_controller.enable_data_stream()
 
@@ -50,28 +56,44 @@ def main():
         while True:
 
             print('-' * 52)
-            menu_input = input('Tobii Inertial Measure Unit sensor calibration menu:\n\t\'a\' for accelerometer calibration.\n\t\'g\' for gyroscope calibration.\n\t\'s\' save calibration.\n\t\'q\' quit calibration without saving.\n>')
+            menu_input = input('Tobii Inertial Measure Unit sensor calibration menu:\n\t\'a\' for accelerometer calibration.\n\t\'g\' for gyroscope calibration.\n\t\'p\' print current calibration.\n\t\'s\' save calibration.\n\t\'q\' quit calibration without saving.\n>')
 
             match menu_input:
 
                 case 'a':
 
-                    print('\nACCELEROMETER CALIBRATION')
-                    input('Press \'Enter\' to start data acquisition.\n')
+                    axis = ['X', 'Y', 'Z']
+                    directions = ['upward', 'downward', 'perpendicular']
 
-                    # Initialise progress bar
-                    MiscFeatures.printProgressBar(0, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+                    for i, axis in enumerate(axis):
 
-                    # Enable accelerometer calibration process listening to incoming Tobii data stream
-                    for progress in tobii_imu.calibrate_gyroscope(tobii_data_stream, args.buffer_size):
+                        print(f'\nACCELEROMETER {axis} AXIS CALIBRATION')
 
-                        # Update progress Bar
-                        MiscFeatures.printProgressBar(progress, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+                        axis_buffers = {}
 
-                    print(f'\n\nGyroscope average over {progress} values for each axis:')
-                    print('\tX offset: ', tobii_imu.get_gyroscope_offset().value[0])
-                    print('\tY offset: ', tobii_imu.get_gyroscope_offset().value[1])
-                    print('\tZ offset: ', tobii_imu.get_gyroscope_offset().value[2])
+                        for j, direction in enumerate(directions):
+
+                            input(f'\nKeep Tobii Glasses accelerometer {axis} axis {direction} then press \'Enter\' to start data acquisition.\n')
+
+                            # Initialise progress bar
+                            MiscFeatures.printProgressBar(0, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+
+                            # Capture accelerometer data stream
+                            for progress, data_ts_buffer in tobii_data_stream.capture('Accelerometer', args.buffer_size):
+
+                                # Update progress Bar
+                                MiscFeatures.printProgressBar(progress, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+
+                            axis_buffers[direction] = data_ts_buffer
+
+                        tobii_imu.calibrate_accelerometer_axis_coefficients(i, axis_buffers['upward'], axis_buffers['downward'], axis_buffers['perpendicular'])
+
+                    accelerometer_coefficients = tobii_imu.get_accelerometer_coefficients()
+
+                    print(f'\n\nAccelerometer optimal linear fit coefficients over {progress} values for each axis:')
+                    print('\tX coefficients: ', accelerometer_coefficients[0])
+                    print('\tY coefficients: ', accelerometer_coefficients[1])
+                    print('\tZ coefficients: ', accelerometer_coefficients[2])
 
                 case 'g':
 
@@ -81,16 +103,34 @@ def main():
                     # Initialise progress bar
                     MiscFeatures.printProgressBar(0, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
 
-                    # Enable gyroscope calibration process listening to incoming Tobii data stream
-                    for progress in tobii_imu.calibrate_gyroscope(tobii_data_stream, args.buffer_size):
+                    # Capture gyroscope data stream
+                    for progress, buffer in tobii_data_stream.capture('Gyroscope', args.buffer_size):
 
                         # Update progress Bar
                         MiscFeatures.printProgressBar(progress, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
 
+                    gyroscope_offset = tobii_imu.calibrate_gyroscope_offset(buffer)
+
                     print(f'\n\nGyroscope average over {progress} values for each axis:')
-                    print('\tX offset: ', tobii_imu.get_gyroscope_offset().value[0])
-                    print('\tY offset: ', tobii_imu.get_gyroscope_offset().value[1])
-                    print('\tZ offset: ', tobii_imu.get_gyroscope_offset().value[2])
+                    print('\tX offset: ', gyroscope_offset[0])
+                    print('\tY offset: ', gyroscope_offset[1])
+                    print('\tZ offset: ', gyroscope_offset[2])
+
+                case 'p':
+
+                    gyroscope_offset = tobii_imu.get_gyroscope_offset()
+
+                    print(f'\nGyroscope offset for each axis:')
+                    print('\tX offset: ', gyroscope_offset[0])
+                    print('\tY offset: ', gyroscope_offset[1])
+                    print('\tZ offset: ', gyroscope_offset[2])
+
+                    accelerometer_coefficients = tobii_imu.get_accelerometer_coefficients()
+
+                    print(f'\nAccelerometer optimal linear fit coefficients for each axis:')
+                    print('\tX coefficients: ', accelerometer_coefficients[0])
+                    print('\tY coefficients: ', accelerometer_coefficients[1])
+                    print('\tZ coefficients: ', accelerometer_coefficients[2])
 
                 case 's':
 
diff --git a/src/argaze/utils/tobii_stream_display.py b/src/argaze/utils/tobii_stream_display.py
index 09360ef..f437de9 100644
--- a/src/argaze/utils/tobii_stream_display.py
+++ b/src/argaze/utils/tobii_stream_display.py
@@ -17,6 +17,7 @@ def main():
     # Manage arguments
     parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
     parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip')
+    parser.add_argument('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath')
 
     args = parser.parse_args()
 
@@ -39,8 +40,16 @@ def main():
     # Enable tobii video stream
     tobii_video_stream = tobii_controller.enable_video_stream()
 
-    # Init head movement
-    head_movement_px = numpy.array((0, 0))
+    # Create tobii imu handler
+    tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
+    
+    # Load optional imu calibration file
+    if args.imu_calibration != None:
+
+        tobii_imu.load_calibration_file(args.imu_calibration)
+
+    # Init head rotation speed
+    head_rotation_speed = numpy.array((0, 0, 0))
 
     # Init gaze position
     gaze_position_px = GazeFeatures.GazePosition((0, 0))
@@ -59,7 +68,7 @@ def main():
     
     def data_stream_callback(data_ts, data_object, data_object_type):
 
-        nonlocal head_movement_px
+        nonlocal head_rotation_speed
         nonlocal gaze_position_px
         nonlocal data_ts_ms
         nonlocal gyroscope_chrono
@@ -74,9 +83,8 @@ def main():
                 # Assess gyroscope stream performance
                 gyroscope_chrono.lap()
 
-                # Calculate head movement considering only head yaw and pitch
-                head_movement = numpy.array(data_object.value)
-                head_movement_px = head_movement.astype(int)
+                # Apply imu calibration
+                head_rotation_speed = tobii_imu.apply_gyroscope_offset(data_object).value.astype(int) * 5
 
             case 'GazePosition':
                 
@@ -131,8 +139,8 @@ def main():
                 gaze_ps = 1e3 * lap_counter / elapsed_time
                 gaze_chrono.restart()
 
-            # Draw head movement
-            cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), (int(video_frame.width/2) + head_movement_px[1], int(video_frame.height/2) - head_movement_px[0]), (150, 150, 150), 3)
+            # Draw head rotation speed considering only yaw and pitch values
+            cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), (int(video_frame.width/2) + head_rotation_speed[1], int(video_frame.height/2) - head_rotation_speed[0]), (150, 150, 150), 3)
             
             # Draw gaze
             gaze_position_px.draw(video_frame.matrix)