Defining global variables to change referential from camera to imu.

1 files changed, 26 insertions, 26 deletions

diff --git a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
index a3c2bbd..12cc2c6 100644
--- a/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
+++ b/src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py
@@ -11,9 +11,16 @@ import numpy
 from scipy.optimize import curve_fit
 import cv2 as cv
 
-EARTH_GRAVITY = -9.81 # m/s2
+# Earth gravity force (m/s2)
+EARTH_GRAVITY = -9.81
 EARTH_GRAVITY_VECTOR = [0, EARTH_GRAVITY, 0]
 
+# Translation vector from camera referential to imu referential (cm)
+CAMERA_TO_IMU_TRANSLATION_VECTOR = [8, -1, -5]
+
+# Rotation vector from camera referential to imu referential (euler, degree)
+CAMERA_TO_IMU_ROTATION_VECTOR = [18, 0, 180]
+
 class TobiiInertialMeasureUnit():
     """Ease Tobbi IMU data handling."""
 
@@ -80,13 +87,12 @@ class TobiiInertialMeasureUnit():
 
         return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset)
 
-    def reset_rotation(self, ts = 0, gyroscope = numpy.zeros(3), rotation = numpy.zeros(3)):
+    def reset_rotation(self):
         """Reset rotation value before to start integration process."""
 
-        self.__last_gyroscope_ts = ts
-        self.__last_gyroscope = gyroscope
+        self.__last_gyroscope_ts = None
 
-        self.__rotation = rotation
+        self.__rotation = numpy.zeros(3)
 
     def update_rotation(self, gyroscope_data_ts, gyroscope_data_object):
         """Integrate timestamped gyroscope values to update rotation."""
@@ -95,15 +101,16 @@ class TobiiInertialMeasureUnit():
         current_gyroscope = gyroscope_data_object.value * 1e-3
 
         # Init gyroscope integration
-        if self.__last_gyroscope_ts == 0 and self.__last_gyroscope.all() == 0:
+        if self.__last_gyroscope_ts == None:
 
-            self.reset_rotation(gyroscope_data_ts, current_gyroscope)
+            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.__last_gyroscope * delta_time
+        self.__rotation = self.__rotation + (self.__last_gyroscope * delta_time)
 
         # Store current as last
         self.__last_gyroscope_ts = gyroscope_data_ts
@@ -156,22 +163,13 @@ class TobiiInertialMeasureUnit():
 
         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)):
+    def reset_translation(self, translation_speed = 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.__last_accelerometer_ts = None
 
         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')
+        self.__translation = numpy.zeros(3)
 
     def update_translation(self, accelerometer_data_ts, accelerometer_data_object):
         """Integrate timestamped accelerometer values to update translation."""
@@ -184,9 +182,11 @@ class TobiiInertialMeasureUnit():
         print('\tcurrent_accelerometer norm=', numpy.linalg.norm(current_accelerometer))
 
         # Init accelerometer integration
-        if self.__last_accelerometer_ts == 0 and self.__last_accelerometer.all() == 0:
+        if self.__last_accelerometer_ts == None:
 
-            self.reset_translation(ts = accelerometer_data_ts, accelerometer = current_accelerometer)
+            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
@@ -194,10 +194,10 @@ class TobiiInertialMeasureUnit():
         print('\tdelta_time=', delta_time)
 
         # Integrate accelerometer
-        self.__translation_speed += self.__last_accelerometer * delta_time
-        self.__translation += self.__last_translation_speed * delta_time
+        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/s)=', self.__translation_speed)
+        print('\tself.__translation_speed(cm/ms)=', self.__translation_speed)
         print('\tself.__translation(cm)=', self.__translation)
 
         # Store current as last
@@ -220,7 +220,7 @@ class TobiiInertialMeasureUnit():
         """Rotate imu plumb to remove gravity effect in accelerometer data."""
 
         C, _ = cv.Rodrigues(rvec)
-        self.__plumb = -C.dot(EARTH_GRAVITY_VECTOR)
+        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))