diff options
Diffstat (limited to 'src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py')
| -rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 253 |
1 files changed, 0 insertions, 253 deletions
diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py deleted file mode 100644 index 35bb035..0000000 --- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py +++ /dev/null @@ -1,253 +0,0 @@ -#!/usr/bin/env python - -from typing import Tuple -import json -import time -import math - -from argaze.TobiiGlassesPro2 import TobiiData - -import numpy -from scipy.optimize import curve_fit -import cv2 as cv - - -EARTH_GRAVITY = -9.81 -"""Earth gravity force (m/s2).""" - -EARTH_GRAVITY_VECTOR = [0, EARTH_GRAVITY, 0] -"""Earth gravity force vector.""" - -CAMERA_TO_IMU_TRANSLATION_VECTOR = [8, -1, -5] -"""Translation vector from camera referential to imu referential (cm).""" - -CAMERA_TO_IMU_ROTATION_VECTOR = [18, 0, 180] -"""Rotation vector from camera referential to imu referential (euler, degree).""" - -class TobiiInertialMeasureUnit(): - """Ease Tobbi [Inertial Measure Unit](https://connect.tobii.com/s/article/How-are-the-MEMS-data-reported-for-Tobii-Pro-Glasses-2?language=en_US) data handling""" - - def __init__(self): - """Define IMU calibration data.""" - - self.__gyroscope_offset = numpy.zeros(3) - self.__accelerometer_coefficients = numpy.array([[1., 0.], [1., 0.], [1., 0.]]) - - self.__plumb = numpy.array(EARTH_GRAVITY_VECTOR) - - self.reset_rotation() - self.reset_translation() - - 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 - calibration_data = json.load(calibration_file) - - # Load calibration data - 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': 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 calibrate_gyroscope_offset(self, gyroscope_ts_buffer) -> numpy.array: - """Calibrate gyroscope offset from a timestamped gyroscope buffer. - **Returns:** numpy.array""" - - # Consider gyroscope values without timestamps - gyroscope_values = [] - for ts, data_object in gyroscope_ts_buffer.items(): - gyroscope_values.append(data_object.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 - self.__gyroscope_offset = numpy.array([gx_offset, gy_offset, gz_offset]) - - return self.__gyroscope_offset - - @property - def gyroscope_offset(self) -> numpy.array: - """Get gyroscope offset.""" - - return self.__gyroscope_offset - - def apply_gyroscope_offset(self, gyroscope_data_object: TobiiData.Gyroscope) -> "TobiiData.Gyroscope": - """Remove gyroscope offset to given gyroscope data.""" - - return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset) - - def reset_rotation(self): - """Reset rotation value before to start integration process.""" - - self.__last_gyroscope_ts = None - - self.__rotation = numpy.zeros(3) - - def update_rotation(self, gyroscope_data_ts, gyroscope_data_object): - """Integrate timestamped gyroscope values to update rotation.""" - - # Convert deg/s into deg/ms - current_gyroscope = gyroscope_data_object.value * 1e-3 - - # Init gyroscope integration - if self.__last_gyroscope_ts == None: - - self.__last_gyroscope_ts = gyroscope_data_ts - self.__last_gyroscope = current_gyroscope - - # Calculate elapsed time in ms - delta_time = (gyroscope_data_ts - self.__last_gyroscope_ts) / 1e3 - - # Integrate gyroscope - self.__rotation = self.__rotation + (self.__last_gyroscope * delta_time) - - # Store current as last - self.__last_gyroscope_ts = gyroscope_data_ts - self.__last_gyroscope = current_gyroscope - - @property - def rotation(self) -> numpy.array: - """Return current rotation value (euler angles in degree).""" - - return self.__rotation - - def _accelerometer_linear_fit(self, x, a, b): - """Linear function for accelerometer axis correction.""" - return a * x + b - - 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.""" - - # Consider accelerometer axis values without timestamps - accelerometer_values = [] - expected_values = [] - - 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(+EARTH_GRAVITY) - - accelerometer_values.append(downward_data_object.value[axis]) - expected_values.append(-EARTH_GRAVITY) - - accelerometer_values.append(perpendicular_data_object.value[axis]) - expected_values.append(0.0) - - # 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) - - # Store results for the given axis - self.__accelerometer_coefficients[axis] = numpy.array(optimal_coefficients) - - @property - def accelerometer_coefficients(self) -> numpy.array: - """Return accelerometer coefficients.""" - - return self.__accelerometer_coefficients - - def apply_accelerometer_coefficients(self, accelerometer_data_object: TobiiData.Accelerometer) -> "TobiiData.Accelerometer": - """Add accelerometer offset to given accelerometer data.""" - - 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]) - - return TobiiData.Accelerometer(numpy.array([x, y , z])) - - def reset_translation(self, translation_speed = numpy.zeros(3)): - """Reset translation value before to start integration process.""" - - self.__last_accelerometer_ts = None - - self.__translation_speed = translation_speed - self.__translation = numpy.zeros(3) - - def update_translation(self, accelerometer_data_ts, accelerometer_data_object): - """Integrate timestamped accelerometer values to update translation.""" - - print('> update_translation: accelerometer_data_ts=', accelerometer_data_ts) - - # Convert m/s2 into cm/ms2 - current_accelerometer = accelerometer_data_object.value * 1e-4 - - print('\tcurrent_accelerometer(cm/ms2)=', current_accelerometer) - print('\tcurrent_accelerometer norm=', numpy.linalg.norm(current_accelerometer)) - - # Init accelerometer integration - if self.__last_accelerometer_ts == None: - - self.__last_accelerometer_ts = accelerometer_data_ts - self.__last_accelerometer = current_accelerometer - self.__last_translation_speed = numpy.zeros(3) - - # Calculate elapsed time in ms - delta_time = (accelerometer_data_ts - self.__last_accelerometer_ts) / 1e3 - - print('\tdelta_time=', delta_time) - - # Integrate accelerometer - self.__translation_speed = self.__translation_speed + (self.__last_accelerometer * delta_time) - self.__translation = self.__translation + (self.__last_translation_speed * delta_time) - - print('\tself.__translation_speed(cm/ms)=', self.__translation_speed) - print('\tself.__translation(cm)=', self.__translation) - - # Store current as last - self.__last_accelerometer = current_accelerometer - self.__last_accelerometer_ts = accelerometer_data_ts - self.__last_translation_speed = self.__translation_speed - - print('< update_translation') - - #else: - - # print('no valid head plumb') - - @property - def translation(self) -> numpy.array: - """Return current translation vector.""" - - return self.__translation - - @property - def translation_speed(self) -> numpy.array: - """Return current translation speed vector.""" - - return self.__translation_speed - - def rotate_plumb(self, rvec): - """Rotate imu plumb to remove gravity effect in accelerometer data.""" - - C, _ = cv.Rodrigues(rvec) - self.__plumb = C.dot(EARTH_GRAVITY_VECTOR) - - # Check plumb length - assert(math.isclose(numpy.linalg.norm(self.__plumb), math.fabs(EARTH_GRAVITY), abs_tol=1e-3)) - - @property - def plumb(self) -> numpy.array: - """Return plumb vector.""" - - return self.__plumb - - def apply_plumb(self, accelerometer_data_object: TobiiData.Accelerometer) -> "TobiiData.Accelerometer": - """Remove gravity along plumb vector to given accelerometer data.""" - - return TobiiData.Accelerometer(accelerometer_data_object.value - self.__plumb) |