aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorThéo de la Hogue2022-11-07 23:58:11 +0100
committerThéo de la Hogue2022-11-07 23:58:11 +0100
commit29a05decec40216aa016e73f26e5a4f9eccc92d2 (patch)
treebceb29428bec4654ef68404db83a26bada5e9377
parent9570c84dc2cc8469b685d01d87577102acc4156f (diff)
downloadargaze-29a05decec40216aa016e73f26e5a4f9eccc92d2.zip
argaze-29a05decec40216aa016e73f26e5a4f9eccc92d2.tar.gz
argaze-29a05decec40216aa016e73f26e5a4f9eccc92d2.tar.bz2
argaze-29a05decec40216aa016e73f26e5a4f9eccc92d2.tar.xz
Defining global variables to change referential from camera to imu.
-rw-r--r--src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py52
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))