aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py113
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)