From f7e2875ef14d96164f7b795360365c2fccbafa74 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Wed, 2 Nov 2022 09:41:13 +0100
Subject: Adding a new utils script to calibrate Tobii IMU.

---
 src/argaze/utils/tobii_imu_calibrate.py | 112 ++++++++++++++++++++++++++++++++
 1 file changed, 112 insertions(+)
 create mode 100644 src/argaze/utils/tobii_imu_calibrate.py

diff --git a/src/argaze/utils/tobii_imu_calibrate.py b/src/argaze/utils/tobii_imu_calibrate.py
new file mode 100644
index 0000000..1422e78
--- /dev/null
+++ b/src/argaze/utils/tobii_imu_calibrate.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+import argparse
+import os
+import time
+
+from argaze import DataStructures
+from argaze.TobiiGlassesPro2 import TobiiController, TobiiInertialMeasureUnit
+from argaze.utils import MiscFeatures
+
+import numpy
+
+def main():
+    """
+    Calibrate Tobbi gyroscope and accelerometer sensors and finally outputs camera calibration data into a .json file.
+
+    ### Reference:
+    - [Inertial Measure Unit calibration tutorial](https://makersportal.com/blog/calibration-of-an-inertial-measurement-unit-imu-with-raspberry-pi-part-ii)
+    """
+
+    # 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('-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()
+
+    # Create tobii controller (with auto discovery network process if no ip argument is provided)
+    print("Looking for a Tobii Glasses Pro 2 device ...")
+
+    try:
+
+        tobii_controller = TobiiController.TobiiController(args.tobii_ip)
+        print(f'Tobii Glasses Pro 2 device found at {tobii_controller.address} address.')
+
+    except ConnectionError as e:
+
+        print(e)
+        exit()
+
+    # Create tobii imu handler
+    tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
+
+    # Enable tobii data stream 
+    tobii_data_stream = tobii_controller.enable_data_stream()
+
+    # Menu loop
+    try:
+
+        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>')
+
+            match menu_input:
+
+                case 'a':
+
+                    print('\nACCELEROMETER CALIBRATION')
+                    input('Press \'Enter\' to start data acquisition.\n')
+
+                    # Initialise progress bar
+                    MiscFeatures.printProgressBar(0, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+
+                    # Enable accelerometer calibration process listening to incoming Tobii data stream
+                    for progress in tobii_imu.calibrate_gyroscope(tobii_data_stream, args.buffer_size):
+
+                        # Update progress Bar
+                        MiscFeatures.printProgressBar(progress, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+
+                    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])
+
+                case 'g':
+
+                    print('\nGYROSCOPE CALIBRATION')
+                    input('Keep Tobii Glasses steady then press \'Enter\' to start data acquisition.\n')
+
+                    # 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):
+
+                        # Update progress Bar
+                        MiscFeatures.printProgressBar(progress, args.buffer_size, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
+
+                    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])
+
+                case 's':
+
+                    tobii_imu.save_calibration_file(args.output)
+                    print(f'\nCalibration data exported into {args.output} file')
+
+                    break
+
+                case 'q':
+
+                    break
+
+    # exit on 'ctrl+C' interruption
+    except KeyboardInterrupt:
+        pass
+
+if __name__ == '__main__':
+
+    main()
-- 
cgit v1.1