diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/argaze/TobiiGlassesPro2/TobiiInertialMeasureUnit.py | 52 |
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)) |