diff options
| -rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 113 |
1 files changed, 103 insertions, 10 deletions
diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py index 1328389..a3c2bbd 100644 --- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py +++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py @@ -2,12 +2,17 @@ import json import time +import math from argaze import DataStructures from argaze.TobiiGlassesPro2 import TobiiData import numpy from scipy.optimize import curve_fit +import cv2 as cv + +EARTH_GRAVITY = -9.81 # m/s2 +EARTH_GRAVITY_VECTOR = [0, EARTH_GRAVITY, 0] class TobiiInertialMeasureUnit(): """Ease Tobbi IMU data handling.""" @@ -15,9 +20,14 @@ class TobiiInertialMeasureUnit(): def __init__(self): """Define IMU calibration data.""" - self.__gyroscope_offset = numpy.array([0., 0., 0.]) + 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.""" @@ -70,19 +80,22 @@ class TobiiInertialMeasureUnit(): return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset) - def reset_rotation(self, ts = 0, value = [0, 0, 0]): + def reset_rotation(self, ts = 0, gyroscope = numpy.zeros(3), rotation = numpy.zeros(3)): + """Reset rotation value before to start integration process.""" self.__last_gyroscope_ts = ts - self.__last_gyroscope = value - self.__rotation = value + self.__last_gyroscope = gyroscope + + self.__rotation = rotation 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 == 0: + if self.__last_gyroscope_ts == 0 and self.__last_gyroscope.all() == 0: self.reset_rotation(gyroscope_data_ts, current_gyroscope) @@ -97,11 +110,12 @@ class TobiiInertialMeasureUnit(): self.__last_gyroscope = current_gyroscope def get_rotation(self): + """Return current rotation value.""" 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): @@ -114,10 +128,10 @@ class TobiiInertialMeasureUnit(): 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) + expected_values.append(+EARTH_GRAVITY) accelerometer_values.append(downward_data_object.value[axis]) - expected_values.append(-1.0) + expected_values.append(-EARTH_GRAVITY) accelerometer_values.append(perpendicular_data_object.value[axis]) expected_values.append(0.0) @@ -129,7 +143,7 @@ class TobiiInertialMeasureUnit(): self.__accelerometer_coefficients[axis] = numpy.array(optimal_coefficients) def get_accelerometer_coefficients(self): - """Get accelerometer coefficients.""" + """Return accelerometer coefficients.""" return self.__accelerometer_coefficients @@ -140,4 +154,83 @@ class TobiiInertialMeasureUnit(): 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([x, y , z])
\ No newline at end of file + return TobiiData.Accelerometer(numpy.array([x, y , z])) + + def reset_translation(self, ts = 0, accelerometer = numpy.zeros(3), translation_speed = numpy.zeros(3), translation = numpy.zeros(3)): + """Reset translation value before to start integration process.""" + + print(f'> reset_translation: translation_speed={translation_speed}, translation={translation}') + + self.__last_accelerometer_ts = ts + self.__last_accelerometer = accelerometer + self.__last_translation_speed = translation_speed + + self.__translation_speed = translation_speed + self.__translation = translation + + print('\tself.__translation_speed(cm/s)=', self.__translation_speed) + print('\tself.__translation(cm)=', self.__translation) + + print('< reset_translation') + + 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 == 0 and self.__last_accelerometer.all() == 0: + + self.reset_translation(ts = accelerometer_data_ts, accelerometer = current_accelerometer) + + # 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.__last_accelerometer * delta_time + self.__translation += self.__last_translation_speed * delta_time + + print('\tself.__translation_speed(cm/s)=', 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') + + def get_translation(self): + """Return current translation speed and translation values.""" + + return self.__translation_speed, self.__translation + + 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)) + + def get_plumb(self): + """Return plumb vector.""" + + return self.__plumb + + def apply_plumb(self, accelerometer_data_object): + """Remove gravity along plumb vector to given accelerometer data.""" + + return TobiiData.Accelerometer(accelerometer_data_object.value - self.__plumb) |