From 6551819225e623cc8246be29908b022bb7b98a98 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Tue, 25 Oct 2022 00:56:05 +0200
Subject: Using a smooth factor, Renaming variables.

---
 src/argaze/utils/tobii_stream_arcube_display.py | 21 +++++++++++++--------
 1 file changed, 13 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index bed3ba6..d307206 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -113,6 +113,7 @@ def main():
     last_gyroscope_ts_ms = 0
     gyroscope_drift = numpy.zeros(3)
     head_rotation = numpy.zeros(3)
+    smooth_factor = 0.5
 
     # Init data timestamped in millisecond
     data_ts_ms = 0
@@ -152,8 +153,9 @@ def main():
                 # 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
+                    gyroscope_drift = gyroscope_drift * smooth_factor + current_gyroscope * (1 - smooth_factor) 
                     head_rotation = numpy.zeros(3)
+
                     print(f'> gyroscope_drift={gyroscope_drift}')
 
                 # Integrate gyroscope with drift compensation
@@ -237,18 +239,21 @@ def main():
                 # Cube pose estimation fails: use tobii glasses inertial sensors to estimate cube pose from last estimated pose
                 elif aruco_cube_success:
 
-                    # Translate cube according head translation speed
-                    #aruco_cube_tvec += head_translation_speed * (video_ts_ms - aruco_cube_ts_ms)
+                    # TODO : Translate cube according head translation speed ?
 
                     #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
-                    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)
+                    R = make_rotation_matrix(*head_rotation)
+
+                    # rotate tvec ???
+                    #new_tvec = aruco_cube_tvec.dot(R.T)
+
+                    # rotate rvec
+                    C, _ = cv.Rodrigues(aruco_cube_rvec)
+                    C = C.dot(R)
+                    new_rvec, _ = cv.Rodrigues(C)
 
                     # Set cube pose estimation
                     aruco_cube.set_pose(tvec = aruco_cube_tvec, rvec = new_rvec)
-- 
cgit v1.1