From 4e78540aef28f5c60620859b354ec400b74c0d55 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Wed, 2 Nov 2022 20:24:01 +0100
Subject: Using imu gyroscope feature integration.

---
 src/argaze/utils/tobii_stream_arcube_display.py | 56 ++++++++-----------------
 1 file changed, 17 insertions(+), 39 deletions(-)

diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 292c2b4..4ba59af 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -44,6 +44,7 @@ def main():
     parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip')
     parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default=None, help='json camera calibration filepath')
     parser.add_argument('-p', '--aruco_tracker_configuration', metavar='TRACK_CONFIG', type=str, default=None, help='json aruco tracker configuration filepath')
+    parser.add_argument('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath')
     parser.add_argument('-ac', '--aruco_cube', metavar='ARUCO_CUBE', type=str, help='json aruco cube description filepath')
     parser.add_argument('-s', '--aoi_scene', metavar='AOI_SCENE', type=str, help='obj aoi 3D scene description filepath')
     parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
@@ -103,13 +104,17 @@ def main():
         print(f'\nArUcoTracker configuration for markers detection:')
         aruco_tracker.print_configuration()
 
+    # Create tobii imu handler
+    tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
+    
+    # Load optional imu calibration file
+    if args.imu_calibration != None:
+
+        tobii_imu.load_calibration_file(args.imu_calibration)
+
     # Init gyroscope processing to track head rotation changes when arcuco cube pose can't be estimated
     # So, the resulting head rotation is relative to last pose estimation (it's not an absolute orientation)
-    last_gyroscope = numpy.zeros(3)
-    last_gyroscope_ts_ms = 0
-    gyroscope_offset = numpy.zeros(3)
-    gyroscope_offset_smooth = 0.5
-    head_rotation = numpy.zeros(3)
+    tobii_imu.reset_rotation()
     
     # Init accelerotmeter processing to track head translation changes when arcuco cube pose can't be estimated
     # So, the resulting head translation is relative to last pose estimation (it's not an absolute position)
@@ -132,6 +137,7 @@ def main():
 
     def data_stream_callback(data_ts, data_object, data_object_type):
 
+        nonlocal tobii_imu
         nonlocal data_ts_ms
 
         data_ts_ms = data_ts / 1e3
@@ -140,38 +146,9 @@ def main():
 
             case 'Gyroscope':
 
-                nonlocal last_gyroscope
-                nonlocal last_gyroscope_ts_ms
-                nonlocal gyroscope_offset
-                nonlocal head_rotation
-
-                # Convert deg/s into deg/ms
-                current_gyroscope = numpy.array(data_object.value) * 1e-3
-
-                # Init gyroscope derivation and integration
-                if last_gyroscope_ts_ms == 0:
-
-                    last_gyroscope = current_gyroscope
-                    last_gyroscope_ts_ms = data_ts_ms
+                data_object = tobii_imu.apply_gyroscope_offset(data_object)
 
-                # Calculate elapsed time
-                delta_time = data_ts_ms - last_gyroscope_ts_ms
-
-                # Derivate gyroscope
-                gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3)
-
-                # Update gyroscope offset smoothly and reset head rotation when gyroscope is stable
-                if numpy.linalg.norm(gyroscope_derivation) < 1e-5:
-                
-                    gyroscope_offset = gyroscope_offset * gyroscope_offset_smooth + current_gyroscope * (1 - gyroscope_offset_smooth)
-                    head_rotation = numpy.zeros(3)
-
-                # Integrate gyroscope with offset compensation
-                head_rotation += (last_gyroscope - gyroscope_offset) * delta_time
-
-                # Store current as last
-                last_gyroscope = current_gyroscope
-                last_gyroscope_ts_ms = data_ts_ms
+                tobii_imu.update_rotation(data_ts, data_object)
 
             case 'Accelerometer':
                 
@@ -279,7 +256,8 @@ def main():
 
                     # Reset head rotation, translation and translation speed (cm/s)
                     # Note : head translation speed is simultaneously estimated thanks to accelerometer sensor (see upward)
-                    head_rotation = numpy.zeros(3)
+                    tobii_imu.reset_rotation()
+
                     head_translation = numpy.zeros(3)
                     head_translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms)
 
@@ -300,13 +278,13 @@ def main():
                     # Use tobii glasses inertial sensors to estimate cube pose from last estimated pose
 
                     # Translate cube according head translation
-                    new_tvec = aruco_cube_tvec + head_translation
+                    new_tvec = aruco_cube_tvec #+ head_translation
 
                     #if numpy.linalg.norm(head_rotation) > 0:
                     #    print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}')
 
                     # Rotate cube around origin according head rotation
-                    R = make_rotation_matrix(*head_rotation)
+                    R = make_rotation_matrix(*tobii_imu.get_rotation())
 
                     # rotate tvec ???
                     #new_tvec = aruco_cube_tvec.dot(R.T)
-- 
cgit v1.1