From 6a192314a5b38bb51e008ff36e732204926368a9 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Thu, 20 Oct 2022 01:30:38 +0200
Subject: Testing success and validity. First work around head pose tracking to
 offset cube pose.

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

diff --git a/src/argaze/utils/tobii_stream_arcube_display.py b/src/argaze/utils/tobii_stream_arcube_display.py
index 255961a..c77d6e9 100644
--- a/src/argaze/utils/tobii_stream_arcube_display.py
+++ b/src/argaze/utils/tobii_stream_arcube_display.py
@@ -12,6 +12,7 @@ from argaze.utils import MiscFeatures
 
 import cv2 as cv
 import numpy
+#import imufusion
 
 def main():
     """
@@ -72,11 +73,14 @@ def main():
 
         print(f'\nArUcoTracker configuration for markers detection:')
         aruco_tracker.print_configuration()
-
-    # Init head pose tracking
-    head_translation = numpy.array((0, 0, 0))
-    head_rotation = numpy.array((0, 0, 0))
-
+    
+    # Init head pose estimation
+    accelerometer = numpy.array([])
+    gyroscope = numpy.array([])
+    head_translation = numpy.zeros(3)
+    head_rotation = numpy.zeros(3)
+    gravity = -9.81
+    
     # Init data timestamped in millisecond
     data_ts_ms = 0
     
@@ -86,6 +90,8 @@ def main():
 
     def data_stream_callback(data_ts, data_object, data_object_type):
 
+        nonlocal gyroscope
+        nonlocal accelerometer
         nonlocal head_translation
         nonlocal head_rotation
         nonlocal data_ts_ms
@@ -96,16 +102,27 @@ def main():
 
             case 'Accelerometer':
                 
-                # Integrate head translation over time
-                #head_translation += numpy.array(data_object.value)
-                pass
+                accelerometer = numpy.array(data_object.value) - gravity
+                return
 
             case 'Gyroscope':
-                
-                # Integrate head rotation over time
-                #head_rotation += numpy.array(data_object.value)
-                pass
 
+                gyroscope = numpy.array(data_object.value)
+                return
+        
+        # Update head pose estimation when all inertial sensors are refreshed
+        if accelerometer.size == 3 and gyroscope.size == 3:
+            '''
+            print('accelerometer:', accelerometer)
+            print('gyroscope:', gyroscope)
+
+            print('head translation:', head_translation)
+            print('head rotation:', head_rotation)
+            '''
+            # Forget old inertial sensors data
+            accelerometer = numpy.array([])
+            gyroscope = numpy.array([])
+        
     tobii_data_stream.reading_callback = data_stream_callback
 
     # Start streaming
@@ -139,24 +156,24 @@ def main():
                     raise UserWarning('No marker detected')
 
                 # Estimate cube pose from tracked markers
-                valid = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers())
+                tvec, rvec, success, validity = aruco_cube.estimate_pose(aruco_tracker.get_tracked_markers())
 
+                # When cube pose estimation fails, use inertial sensors to offset cube pose
+                if not success:
+
+                    #aruco_cube.offset_pose(tvec=[0, 3, 0], rvec=[0, 0, 0])
+                    raise UserWarning('Cube pose estimation failed.')
+                    
                 # Draw markers pose estimation
-                aruco_tracker.draw_tracked_markers(visu_frame.matrix)
+                #aruco_tracker.draw_tracked_markers(visu_frame.matrix)
 
                 # Draw cube pose estimation
                 aruco_cube.draw(visu_frame.matrix, aruco_camera.get_K(), aruco_camera.get_D())
 
                 # Warn about cube pose validity
-                if not valid:
-
-                    raise UserWarning('Cube pose is not validated.')
+                if not validity:
 
-            # Pose estimation error
-            except ValueError as e:
-
-                cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1)
-                cv.putText(visu_frame.matrix, str(e), (20, 140), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA)
+                    raise UserWarning('Cube pose estimation is not validated.')
 
             # Write warning
             except UserWarning as w:
-- 
cgit v1.1