Adding new class to use and calibrate Tobii's Inertial Measure Unit.

2 files changed, 135 insertions, 1 deletions

diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
new file mode 100644
index 0000000..e115de7
--- /dev/null
+++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
@@ -0,0 +1,134 @@
+#!/usr/bin/env python
+
+import json
+import time
+
+from argaze import DataStructures
+from argaze.TobiiGlassesPro2 import TobiiData
+
+import numpy
+
+class TobiiInertialMeasureUnit():
+    """Ease Tobbi IMU data handling."""
+
+    def __init__(self):
+        """Define IMU calibration data."""
+
+        self.__gyroscope_offset = TobiiData.Gyroscope((0., 0., 0.))
+        self.__accelerometer_offset = TobiiData.Accelerometer((0., 0., 0.))
+
+        # Define data stream acquisition variables
+        self.__data_stream_selector = ''
+        self.__data_ts_buffer = DataStructures.TimeStampedBuffer()
+        self.__data_ts_buffer_size = 0
+
+    def load_calibration_file(self, calibration_filepath):
+        """Load IMU calibration from a .json  file."""
+
+        with open(calibration_filepath) as calibration_file:
+
+            # Deserialize .json
+            # TODO find a better way
+            configuration = json.load(configuration_file)
+
+            # Load calibration data
+            self.__gyroscope_offset = TobiiData.Gyroscope(configuration['gyroscope_offset'])
+            self.__accelerometer_offset = TobiiData.Accelerometer(configuration['accelerometer_offset'])
+
+    def save_calibration_file(self, calibration_filepath):
+        """Save IMU calibration into .json file."""
+
+        calibration_data = {'gyroscope_offset': self.__gyroscope_offset.value, 'accelerometer_offset': self.__accelerometer_offset.value}
+        with open(calibration_filepath, 'w', encoding='utf-8') as calibration_file:
+
+            json.dump(calibration_data, calibration_file, ensure_ascii=False, indent=4)
+
+    def get_gyroscope_offset(self):
+        """Get gyroscope offset."""
+        return self.__gyroscope_offset
+
+    def get_accelerometer_offset(self):
+        """Get accelerometer offset."""
+        return self.__accelerometer_offset
+
+    def __data_stream_callback(self, data_ts, data_object, data_object_type):
+        """Store incoming data to calibrate into timestamped buffer"""
+
+        if data_object_type == self.__data_stream_selector:
+
+            if len(self.__data_ts_buffer.keys()) < self.__data_ts_buffer_size:
+
+                self.__data_ts_buffer[data_ts / 1e3] = numpy.array(data_object.value)
+
+    def calibrate_gyroscope(self, tobii_data_stream = TobiiData.TobiiDataStream, buffer_size = 500):
+
+        # Prepare for gyroscope data acquisition
+        self.__data_stream_selector = 'Gyroscope'
+        self.__data_ts_buffer = DataStructures.TimeStampedBuffer()
+        self.__data_ts_buffer_size = buffer_size
+
+        # Subscribe to tobii data stream
+        tobii_data_stream.reading_callback = self.__data_stream_callback
+        tobii_data_stream.open()
+
+        # Share data acquisition progress
+        data_size = 0
+        while data_size < buffer_size:
+
+            time.sleep(0.1)
+
+            data_size = len(self.__data_ts_buffer.keys())
+
+            yield data_size
+
+        # Unsubscribe to tobii data stream
+        tobii_data_stream.close()
+        tobii_data_stream.reading_callback = None
+
+        # Consider gyroscope values without timestamps
+        gyroscope_values = []
+        for ts, value in self.__data_ts_buffer.items():
+            gyroscope_values.append(value)
+
+        # Calculate average value for each axis
+        gx_offset = numpy.mean(numpy.array(gyroscope_values)[:, 0])
+        gy_offset = numpy.mean(numpy.array(gyroscope_values)[:, 1])
+        gz_offset = numpy.mean(numpy.array(gyroscope_values)[:, 2])
+
+        # Store result as gyroscope value
+        self.__gyroscope_offset = TobiiData.Gyroscope((gx_offset, gy_offset, gz_offset))
+
+    def calibrate_accelerometer(self, tobii_data_stream = TobiiData.TobiiDataStream, buffer_size = 500):
+
+        # Prepare for accelerometer data acquisition
+        self.__data_stream_selector = 'Accelerometer'
+        self.__data_ts_buffer = DataStructures.TimeStampedBuffer()
+        self.__data_ts_buffer_size = buffer_size
+
+        # Subscribe to tobii data stream
+        tobii_data_stream.reading_callback = self.__data_stream_callback
+        tobii_data_stream.open()
+
+        # Share data acquisition progress
+        data_size = 0
+        while data_size < buffer_size:
+
+            time.sleep(0.1)
+
+            data_size = len(self.__data_ts_buffer.keys())
+
+            yield data_size
+
+        # Unsubscribe to tobii data stream
+        tobii_data_stream.close()
+        tobii_data_stream.reading_callback = None
+
+        # Consider accelerometer values without timestamps
+        accelerometer_values = []
+        for ts, value in self.__data_ts_buffer.items():
+            accelerometer_values.append(value)
+
+        # Calculate ?
+
+        # Store result as ?
+        #self.__accelerometer_offset = ?
\ No newline at end of file
diff --git a/src/argaze/TobiiGlassesPro2/__init__.py b/src/argaze/TobiiGlassesPro2/__init__.py
index 4fc65bb..754e5a1 100644
--- a/src/argaze/TobiiGlassesPro2/__init__.py
+++ b/src/argaze/TobiiGlassesPro2/__init__.py
@@ -2,4 +2,4 @@
 .. include:: README.md
 """
 __docformat__ = "restructuredtext"
-__all__ = ['TobiiEntities', 'TobiiNetworkInterface', 'TobiiController', 'TobiiData', 'TobiiVideo', 'TobiiSpecifications']
\ No newline at end of file
+__all__ = ['TobiiEntities', 'TobiiNetworkInterface', 'TobiiController', 'TobiiData', 'TobiiVideo', 'TobiiSpecifications', 'TobiiInertialMeasureUnit']
\ No newline at end of file