Adding a thread lock. More work around cube pose estimation with imu.

1 files changed, 75 insertions, 23 deletions

diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index a910c41..cc7e70f 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -3,6 +3,7 @@
 import argparse
 import os, json
 import math
+import threading
 
 from argaze import DataStructures
 from argaze import GazeFeatures
@@ -113,6 +114,12 @@ def main():
 
         tobii_imu.load_calibration_file(args.imu_calibration)
 
+    # Init tobii imu lock
+    tobii_imu_lock = threading.Lock()
+
+    # TEST : DIFF ACC
+    last_accl = numpy.zeros(3)
+
     # Init data timestamped in millisecond
     data_ts_ms = 0
     
@@ -123,10 +130,21 @@ def main():
     def data_stream_callback(data_ts, data_object, data_object_type):
 
         nonlocal tobii_imu
+        nonlocal tobii_imu_lock
         nonlocal data_ts_ms
 
+        #TEST
+        nonlocal last_accl
+
         data_ts_ms = data_ts / 1e3
 
+        # Don't update imu when it is used
+        if tobii_imu_lock.locked():
+            return
+
+        # Lock tobii imu updates
+        tobii_imu_lock.acquire()
+
         match data_object_type:
 
             case 'Gyroscope':
@@ -135,21 +153,35 @@ def main():
 
                 tobii_imu.update_rotation(data_ts, data_object)
 
+            
             case 'Accelerometer':
+                pass
+                '''
+                print('raw accelerometer(m/s2)=', data_object.value)
+
+                # TEST : 
+                diff_accl = last_accl - numpy.array(data_object.value)
+                last_accl = numpy.array(data_object.value)
+                print('\tdiff(cm/s2)=', 100 * numpy.linalg.norm(diff_accl))
 
-                print('raw accelerometer=', data_object.value)
+                # TEST : ignore acceleration double
+                if numpy.linalg.norm(diff_accl) > 0.:
                 
-                data_object = tobii_imu.apply_accelerometer_coefficients(data_object)
+                    data_object = tobii_imu.apply_accelerometer_coefficients(data_object)
 
-                print('corrected accelerometer=', data_object.value)
+                    print('corrected accelerometer(m/s2)=', data_object.value)
 
-                print('current plumb=', tobii_imu.get_plumb())
+                    print('current plumb=', tobii_imu.get_plumb())
 
-                data_object = tobii_imu.apply_plumb(data_object)
+                    data_object = tobii_imu.apply_plumb(data_object)
 
-                print('corrected accelerometer - gravity=', data_object.value)
+                    print('corrected accelerometer - gravity(m/s2)=', data_object.value)
+                    print('\tnorm(cm/s2)=', 100 * numpy.linalg.norm(data_object.value))
 
-                tobii_imu.update_translation(data_ts, data_object)
+                    tobii_imu.update_translation(data_ts, data_object)
+                '''
+        # Unlock tobii imu updates
+        tobii_imu_lock.release()
         
     tobii_data_stream.reading_callback = data_stream_callback
 
@@ -191,13 +223,24 @@ def main():
                 # Cube pose estimation succeed and is validated by 2 faces at least
                 if success and validity >= 2:
 
+                    # Lock tobii imu updates
+                    tobii_imu_lock.acquire()
+
                     # Reset head rotation, translation and translation speed (cm/s)
                     # Note : head translation speed is also estimated thanks to accelerometer sensor (see upward)
                     tobii_imu.reset_rotation()
-                    tobii_imu.reset_translation(translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms))
+                    #tobii_imu.reset_translation(translation_speed = (tvec - aruco_cube_tvec) / (video_ts_ms - aruco_cube_ts_ms))
+
+                    # Create a rotation matrix to transform cube rotation from camera referential to imu referential
+                    F = make_rotation_matrix(*TobiiInertialMeasureUnit.CAMERA_TO_IMU_ROTATION_VECTOR)
+                    R, _ = cv.Rodrigues(rvec)
+                    rvec_flipped, _ = cv.Rodrigues(F.dot(R))
+
+                    # Update head plumb orientation with flipped cube orientation
+                    tobii_imu.rotate_plumb(rvec_flipped)
 
-                    # Update head plumb orientation
-                    tobii_imu.rotate_plumb(rvec)
+                    # Unlock tobii imu updates
+                    tobii_imu_lock.release()
 
                     # Store cube pose
                     aruco_cube_tvec = tvec
@@ -211,23 +254,29 @@ def main():
 
                     # Use tobii glasses inertial sensors to estimate cube pose from last estimated pose
 
+                    # Translate cube into imu referential
+                    imu_tvec = aruco_cube_tvec + numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR)
+
                     # Translate cube according head translation
                     head_translation_speed, head_translation = tobii_imu.get_translation()
-                    new_tvec = aruco_cube_tvec + head_translation
+                    imu_tvec = imu_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 imu origin according head rotation
+                    imu_rvec = tobii_imu.get_rotation() * numpy.array([-1., -1., 1.])
+                    imu_R = make_rotation_matrix(*imu_rvec)
+                    new_imu_tvec = imu_tvec.dot(imu_R)
 
-                    # Rotate cube around origin according head rotation
-                    R = make_rotation_matrix(*tobii_imu.get_rotation())
+                    # Translate back cube into camera referential
+                    new_tvec = new_imu_tvec - numpy.array(TobiiInertialMeasureUnit.CAMERA_TO_IMU_TRANSLATION_VECTOR)
 
-                    # rotate tvec ???
-                    #new_tvec = aruco_cube_tvec.dot(R.T)
+                    # Rotate cube orientation (supposing cube top is up in )
+                    imu_rvec = tobii_imu.get_rotation() * numpy.array([1., -1., 1.])
+                    imu_R = make_rotation_matrix(*imu_rvec)
 
-                    # rotate rvec
                     C, _ = cv.Rodrigues(aruco_cube_rvec)
-                    C = C.dot(R)
+                    C = C.dot(imu_R)
                     new_rvec, _ = cv.Rodrigues(C)
+                    #new_rvec = aruco_cube_rvec
 
                     # Set cube pose estimation
                     aruco_cube.set_pose(tvec = new_tvec, rvec = new_rvec)
@@ -236,20 +285,23 @@ def main():
 
                     raise UserWarning('Cube pose estimation fails.')
 
+                # Get final estimated cube pose
+                tvec, rvec, _, _ = aruco_cube.get_pose()
+
                 # 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())
+                aoi2D_scene = aoi3D_scene.project(tvec, rvec, aruco_camera.get_K())
 
                 # Draw projected scene
-                aoi2D_scene.draw(visu_frame.matrix)
+                #aoi2D_scene.draw(visu_frame.matrix)
 
                 # Draw markers pose estimation
-                aruco_tracker.draw_tracked_markers(visu_frame.matrix)
+                #aruco_tracker.draw_tracked_markers(visu_frame.matrix)
 
                 # Draw cube pose estimation (without camera distorsion)
-                aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), (0, 0, 0, 0))
+                aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), (0, 0, 0, 0), draw_faces=False)
 
                 # Warn about cube pose validity
                 if not aruco_cube_validity: