Working on acceleration integration.

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)