Improving gyroscope data processing.

1 files changed, 79 insertions, 53 deletions

diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 09d35f5..bed3ba6 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('-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('-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)
     args = parser.parse_args()
 
@@ -70,6 +71,14 @@ def main():
     aruco_cube = ArUcoCube.ArUcoCube(args.aruco_cube)
     aruco_cube.print_cache()
 
+    # Load AOI 3D scene centered onto aruco cube
+    aoi3D_scene = AOI3DScene.AOI3DScene()
+    aoi3D_scene.load(args.aoi_scene)
+
+    print(f'\nAOI in {os.path.basename(args.aoi_scene)} scene related to ArCube:')
+    for aoi in aoi3D_scene.keys():
+        print(f'\t{aoi}')
+
     # Create aruco camera
     aruco_camera = ArUcoCamera.ArUcoCamera()
     
@@ -94,15 +103,17 @@ def main():
         aruco_tracker.print_configuration()
 
     # Init head movment estimation
-    last_accelerometer = numpy.array([])
+    '''
+    last_accelerometer = numpy.zeros(3)
     last_accelerometer_ts_ms = 0
-    last_gyroscope = numpy.array([])
-    last_gyroscope_ts_ms = 0
-    gyroscope_drift = numpy.zeros(3)
     head_translation_speed = numpy.zeros(3)
-    head_rotation_speed = numpy.zeros(3)
     gravity = -9.81
-    
+    '''
+    last_gyroscope = numpy.zeros(3)
+    last_gyroscope_ts_ms = 0
+    gyroscope_drift = numpy.zeros(3)
+    head_rotation = numpy.zeros(3)
+
     # Init data timestamped in millisecond
     data_ts_ms = 0
     
@@ -112,20 +123,51 @@ def main():
 
     def data_stream_callback(data_ts, data_object, data_object_type):
 
-        nonlocal last_accelerometer
-        nonlocal last_accelerometer_ts_ms
-        nonlocal last_gyroscope
-        nonlocal last_gyroscope_ts_ms
-        nonlocal gyroscope_drift
-        nonlocal head_translation_speed
-        nonlocal head_rotation_speed
         nonlocal data_ts_ms
 
         data_ts_ms = data_ts / 1e3
 
         match data_object_type:
 
-            case 'Accelerometer':
+            case 'Gyroscope':
+
+                nonlocal last_gyroscope
+                nonlocal last_gyroscope_ts_ms
+                nonlocal gyroscope_drift
+                nonlocal head_rotation
+
+                # Convert deg/s into deg/ms
+                current_gyroscope = numpy.array(data_object.value) * 1e-3
+
+                # Init gyroscope derivation
+                if last_gyroscope_ts_ms == 0:
+
+                    last_gyroscope = current_gyroscope
+                    last_gyroscope_ts_ms = data_ts_ms
+
+                # Derivate gyroscope
+                delta_time = data_ts_ms - last_gyroscope_ts_ms
+                gyroscope_derivation = (current_gyroscope - last_gyroscope) / delta_time if delta_time > 0 else numpy.zeros(3)
+
+                # Update gyroscope drift smoothly and reset head rotation when gyroscope is stable
+                if numpy.linalg.norm(gyroscope_derivation) < 1e-5:
+                
+                    gyroscope_drift = current_gyroscope * 0.1 + gyroscope_drift * 0.9
+                    head_rotation = numpy.zeros(3)
+                    print(f'> gyroscope_drift={gyroscope_drift}')
+
+                # Integrate gyroscope with drift compensation
+                head_rotation += (last_gyroscope - gyroscope_drift) * delta_time
+
+                # Store current as last
+                last_gyroscope = current_gyroscope
+                last_gyroscope_ts_ms = data_ts_ms
+
+            #case 'Accelerometer':
+                '''
+                nonlocal last_accelerometer
+                nonlocal last_accelerometer_ts_ms
+                nonlocal head_translation_speed
 
                 # Convert m/s2 into cm/ms2
                 current_accelerometer = numpy.array(data_object.value) * 1e-4
@@ -142,32 +184,7 @@ def main():
                     head_translation_speed +=  (last_accelerometer + current_accelerometer) * (data_ts_ms - last_accelerometer_ts_ms) / 2
 
                    # print(head_translation_speed)
-
-            case 'Gyroscope':
-
-                # convert deg/s into deg/ms
-                current_gyroscope = numpy.array(data_object.value) * 1e-3
-
-                # Init gyroscope derivation
-                if last_gyroscope_ts_ms == 0:
-
-                    last_gyroscope = current_gyroscope
-                    last_gyroscope_ts_ms = data_ts_ms
-                    gyroscope_derivation = 0
-
-                # Derivate gyroscope
-                else:
-                    gyroscope_derivation = (current_gyroscope - last_gyroscope) / (data_ts_ms - last_gyroscope_ts_ms)
-
-                # Update gyroscope drift when is not moving
-                if numpy.linalg.norm(gyroscope_derivation) < 1e-7:
-
-                     gyroscope_drift = current_gyroscope
-                     
-                 # Drift compensation
-                head_rotation_speed = current_gyroscope - gyroscope_drift
-
-                #print(head_rotation_speed)
+                '''
         
     tobii_data_stream.reading_callback = data_stream_callback
 
@@ -213,7 +230,9 @@ def main():
                     aruco_cube_rvec = rvec
                     aruco_cube_success = success
                     aruco_cube_validity = validity
-                    aruco_cube_ts_ms = video_ts_ms
+
+                    # reset head rotation
+                    head_rotation = numpy.zeros(3)
 
                 # Cube pose estimation fails: use tobii glasses inertial sensors to estimate cube pose from last estimated pose
                 elif aruco_cube_success:
@@ -221,25 +240,32 @@ def main():
                     # Translate cube according head translation speed
                     #aruco_cube_tvec += head_translation_speed * (video_ts_ms - aruco_cube_ts_ms)
 
-                    #print('after:')
-                    #print(aruco_cube_tvec)
-
-                    # Rotate cube around origin according head rotation speed
-                    R = make_rotation_matrix(* (head_rotation_speed * (video_ts_ms - aruco_cube_ts_ms)))
-                    aruco_cube_tvec = aruco_cube_tvec.dot(R.T)
-                    aruco_cube_ts_ms = video_ts_ms
+                    #if numpy.linalg.norm(head_rotation) > 0:
+                    #    print(f'X={head_rotation[0]:3f}, Y={head_rotation[1]:3f}, Z={head_rotation[2]:3f}')
 
-                    #print('berore rotation: ', aruco_cube_tvec)
-                    #print(R)
-                    #print('after rotation: ', aruco_cube_tvec)
+                    # Rotate cube around origin according head rotation
+                    cam_rot = make_rotation_matrix(*head_rotation)
+                    
+                    cube_rot, _ = cv.Rodrigues(aruco_cube_rvec)
+                    cube_rot = cube_rot.dot(cam_rot)
+                    new_rvec, _ = cv.Rodrigues(cube_rot)
 
                     # Set cube pose estimation
-                    aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = aruco_cube_rvec)
+                    aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = new_rvec)
 
                 else:
 
                     raise UserWarning('Cube pose estimation fails.')
 
+                # Project AOI 3 scene onto camera frame
+
+                # DON'T APPLY CAMERA DISTORSION : it projects points which are far from the frame into it
+                # This hack isn't realistic but as the gaze will mainly focus on centered AOI, where the distorsion is low, it is acceptable.
+                aoi2D_scene = aoi3D_scene.project(aruco_cube_tvec, aruco_cube_rvec, aruco_camera.get_K())
+
+                # Draw projected scene
+                aoi2D_scene.draw(visu_frame.matrix)
+
                 # Draw markers pose estimation
                 #aruco_tracker.draw_tracked_markers(visu_frame.matrix)
 
@@ -254,7 +280,7 @@ def main():
             # Write warning
             except UserWarning as w:
 
-                cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1)
+                cv.rectangle(visu_frame.matrix, (0, 100), (600, 150), (127, 127, 127), -1)
                 cv.putText(visu_frame.matrix, str(w), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
 
             # Assess loop performance