Adding acceleration calibration.

1 files changed, 57 insertions, 78 deletions

diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
index e115de7..6bc89f7 100644
--- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
+++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
@@ -7,6 +7,7 @@ from argaze import DataStructures
 from argaze.TobiiGlassesPro2 import TobiiData
 
 import numpy
+from scipy.optimize import curve_fit
 
 class TobiiInertialMeasureUnit():
     """Ease Tobbi IMU data handling."""
@@ -14,13 +15,8 @@ class TobiiInertialMeasureUnit():
     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
+        self.__gyroscope_offset = numpy.array([0., 0., 0.])
+        self.__accelerometer_coefficients = numpy.array([[1., 0.], [1., 0.], [1., 0.]])
 
     def load_calibration_file(self, calibration_filepath):
         """Load IMU calibration from a .json  file."""
@@ -29,106 +25,89 @@ class TobiiInertialMeasureUnit():
 
             # Deserialize .json
             # TODO find a better way
-            configuration = json.load(configuration_file)
+            calibration_data = json.load(calibration_file)
 
             # Load calibration data
-            self.__gyroscope_offset = TobiiData.Gyroscope(configuration['gyroscope_offset'])
-            self.__accelerometer_offset = TobiiData.Accelerometer(configuration['accelerometer_offset'])
+            self.__gyroscope_offset = numpy.array(calibration_data['gyroscope_offset'])
+            self.__accelerometer_coefficients = numpy.array(calibration_data['accelerometer_coefficients'])
 
     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}
+        calibration_data = {
+            'gyroscope_offset': list(self.__gyroscope_offset), 
+            'accelerometer_coefficients': [list(self.__accelerometer_coefficients[0]), list(self.__accelerometer_coefficients[1]), list(self.__accelerometer_coefficients[2])]
+            }
+
         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"""
+    def calibrate_gyroscope_offset(self, gyroscope_ts_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):
+        # Consider gyroscope values without timestamps
+        gyroscope_values = []
+        for ts, data_object in gyroscope_ts_buffer.items():
+            gyroscope_values.append(data_object.value)
 
-        # Prepare for gyroscope data acquisition
-        self.__data_stream_selector = 'Gyroscope'
-        self.__data_ts_buffer = DataStructures.TimeStampedBuffer()
-        self.__data_ts_buffer_size = buffer_size
+        # 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])
 
-        # Subscribe to tobii data stream
-        tobii_data_stream.reading_callback = self.__data_stream_callback
-        tobii_data_stream.open()
+        # Store result
+        self.__gyroscope_offset = numpy.array([gx_offset, gy_offset, gz_offset])
 
-        # Share data acquisition progress
-        data_size = 0
-        while data_size < buffer_size:
+        return self.__gyroscope_offset
 
-            time.sleep(0.1)
+    def get_gyroscope_offset(self):
+        """Get gyroscope offset."""
 
-            data_size = len(self.__data_ts_buffer.keys())
+        return self.__gyroscope_offset
 
-            yield data_size
+    def apply_gyroscope_offset(self, gyroscope_data_object):
+        """Remove gyroscope offset to given gyroscope data."""
 
-        # Unsubscribe to tobii data stream
-        tobii_data_stream.close()
-        tobii_data_stream.reading_callback = None
+        return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset)
 
-        # Consider gyroscope values without timestamps
-        gyroscope_values = []
-        for ts, value in self.__data_ts_buffer.items():
-            gyroscope_values.append(value)
+    def _accelerometer_linear_fit(self, x, a, b):
 
-        # 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])
+        return a * x + b
 
-        # Store result as gyroscope value
-        self.__gyroscope_offset = TobiiData.Gyroscope((gx_offset, gy_offset, gz_offset))
+    def calibrate_accelerometer_axis_coefficients(self, axis, upward_ts_buffer, downward_ts_buffer, perpendicular_ts_buffer):
+        """Calibrate one accelerometer axis using three data set (upward/+1g, downward/-1g, perpendicular/0g) for linear fit."""
 
-    def calibrate_accelerometer(self, tobii_data_stream = TobiiData.TobiiDataStream, buffer_size = 500):
+        # Consider accelerometer axis values without timestamps
+        accelerometer_values = []
+        expected_values = []
 
-        # Prepare for accelerometer data acquisition
-        self.__data_stream_selector = 'Accelerometer'
-        self.__data_ts_buffer = DataStructures.TimeStampedBuffer()
-        self.__data_ts_buffer_size = buffer_size
+        for (upward_ts, upward_data_object), (downward_ts, downward_data_object), (perpendicular_ts, perpendicular_data_object) in zip(upward_ts_buffer.items(), downward_ts_buffer.items(), perpendicular_ts_buffer.items()):
+            
+            accelerometer_values.append(upward_data_object.value[axis])
+            expected_values.append(+1.0)
 
-        # Subscribe to tobii data stream
-        tobii_data_stream.reading_callback = self.__data_stream_callback
-        tobii_data_stream.open()
+            accelerometer_values.append(downward_data_object.value[axis])
+            expected_values.append(-1.0)
 
-        # Share data acquisition progress
-        data_size = 0
-        while data_size < buffer_size:
+            accelerometer_values.append(perpendicular_data_object.value[axis])
+            expected_values.append(0.0)
 
-            time.sleep(0.1)
+        # Find optimal coefficients according linear fit between accelerometer values and expected values
+        optimal_coefficients, _ = curve_fit(self._accelerometer_linear_fit, accelerometer_values, expected_values, maxfev = 10000)
 
-            data_size = len(self.__data_ts_buffer.keys())
+        # Store results for the given axis
+        self.__accelerometer_coefficients[axis] = numpy.array(optimal_coefficients)
 
-            yield data_size
+    def get_accelerometer_coefficients(self):
+        """Get accelerometer coefficients."""
 
-        # Unsubscribe to tobii data stream
-        tobii_data_stream.close()
-        tobii_data_stream.reading_callback = None
+        return self.__accelerometer_coefficients
 
-        # Consider accelerometer values without timestamps
-        accelerometer_values = []
-        for ts, value in self.__data_ts_buffer.items():
-            accelerometer_values.append(value)
+    def apply_accelerometer_coefficients(self, accelerometer_data_object):
+        """Add accelerometer offset to given accelerometer data."""
 
-        # Calculate ?
+        x = self._accelerometer_linear_fit(accelerometer_data_object.value[0], *self.__accelerometer_coefficients[0])
+        y = self._accelerometer_linear_fit(accelerometer_data_object.value[1], *self.__accelerometer_coefficients[1])
+        z = self._accelerometer_linear_fit(accelerometer_data_object.value[2], *self.__accelerometer_coefficients[2])
 
-        # Store result as ?
-        #self.__accelerometer_offset = ?
\ No newline at end of file
+        return TobiiData.Accelerometer([x, y , z])
\ No newline at end of file