From 269ff445067bffa53725a24cdf55d1519c2b3520 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 2 Nov 2022 09:40:21 +0100 Subject: Adding new class to use and calibrate Tobii's Inertial Measure Unit. --- .../TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 134 +++++++++++++++++++++ src/argaze/TobiiGlassesPro2/__init__.py | 2 +- 2 files changed, 135 insertions(+), 1 deletion(-) create mode 100644 src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py 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 -- cgit v1.1