aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorThéo de la Hogue2022-11-02 18:44:59 +0100
committerThéo de la Hogue2022-11-02 18:44:59 +0100
commit74b120ac8b7dc48690538880bb4a3f5fbcc8e49b (patch)
tree9419cc41a9d03e4ae8f7b3ede6eb152041bb0bb0 /src
parent14bff592690bf86f740ab1cd202ac71983a59de1 (diff)
downloadargaze-74b120ac8b7dc48690538880bb4a3f5fbcc8e49b.zip
argaze-74b120ac8b7dc48690538880bb4a3f5fbcc8e49b.tar.gz
argaze-74b120ac8b7dc48690538880bb4a3f5fbcc8e49b.tar.bz2
argaze-74b120ac8b7dc48690538880bb4a3f5fbcc8e49b.tar.xz
Adding acceleration calibration.
Diffstat (limited to 'src')
-rw-r--r--src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py135
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