From 74b120ac8b7dc48690538880bb4a3f5fbcc8e49b Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 2 Nov 2022 18:44:59 +0100 Subject: Adding acceleration calibration. --- .../TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 135 +++++++++------------ 1 file 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 -- cgit v1.1