From ec28ba66d454c405cf1cfad1385b0e7eec443741 Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Fri, 6 May 2022 01:33:51 +0200
Subject: Correcting look_at and looked_pixel factor.

---
 src/argaze/AreaOfInterest/AOIFeatures.py           | 14 +++++----
 .../export_tobii_segment_aruco_visual_scan.py      | 34 +++++++++-------------
 2 files changed, 22 insertions(+), 26 deletions(-)

diff --git a/src/argaze/AreaOfInterest/AOIFeatures.py b/src/argaze/AreaOfInterest/AOIFeatures.py
index 4495df5..8b93935 100644
--- a/src/argaze/AreaOfInterest/AOIFeatures.py
+++ b/src/argaze/AreaOfInterest/AOIFeatures.py
@@ -53,10 +53,12 @@ class AreaOfInterest(numpy.ndarray):
         Dst = numpy.array([[0., 0.], [1., 0.], [1., 1.], [0., 1.]]).astype(numpy.float32)
 
         P = cv.getPerspectiveTransform(Src, Dst)
-        G = gaze_pixel - Src_origin
-        Gp = numpy.dot(P, numpy.array([G[0], G[1], 1]))[:-1]
+        X = numpy.append(numpy.array(gaze_pixel - Src_origin), [1.0]).astype(numpy.float32)
+        Y = numpy.dot(P, X)
 
-        return numpy.around(Gp, 4).tolist()
+        La = (Y/Y[2])[:-1]
+
+        return numpy.around(La, 4).tolist()
 
     def looked_pixel(self, look_at):
         """Get which pixel is looked."""
@@ -71,8 +73,10 @@ class AreaOfInterest(numpy.ndarray):
         Dst = (Dst - Dst_origin).reshape((len(Dst)), 2)
 
         P = cv.getPerspectiveTransform(Src, Dst)
-        L = look_at
-        Lp = Dst_origin + numpy.dot(P, numpy.array([L[0], L[1], 1]))[:-1]
+        X = numpy.array([look_at[0], look_at[1], 1.0]).astype(numpy.float32)
+        Y = numpy.dot(P, X)
+
+        Lp = Dst_origin + (Y/Y[2])[:-1]
 
         return numpy.rint(Lp).astype(int).tolist()
 
diff --git a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
index 4f84943..f8092c0 100644
--- a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
+++ b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
@@ -11,20 +11,8 @@ from argaze.AreaOfInterest import *
 from argaze.utils import MiscFeatures
 
 import numpy
-
 import cv2 as cv
 
-aoi_color = {
-    'Scene_Plan': (127, 127, 127),
-    'PFD_Plan': (63, 127, 63),
-    'Attitude_Plan': (0, 255, 0),
-    'Air_Speed_Plan': (255, 0, 255),
-    'Vertical_Speed_Plan': (255, 255, 0),
-    'Localiser_Plan': (0, 0, 255),
-    'ND_Plan': (127, 63, 63),
-    'Marker_Plan': (0, 0, 0)
-}
-
 def main():
     """
     Track any ArUco marker into Tobii Glasses Pro 2 segment video file. 
@@ -119,6 +107,8 @@ def main():
 
         # Project 3D scene on the reference frame
         # TODO : center projection on a reference AOI
+        ref_aoi = 'Scene_Plan'
+
         # TODO: pass the reference AOI in argument
         aoi3D_scene.rotation = numpy.asarray([[-numpy.pi, 0.0, 0.0]])
         aoi3D_scene.translation = numpy.asarray([[25.0, -32.0, 20.0]])
@@ -129,7 +119,9 @@ def main():
         aoi2D_visu_scene = aoi3D_scene.project(K0)
 
         for name, aoi in aoi2D_visu_scene.items():
-            aoi.draw(visu_frame, aoi_color[name])
+            aoi.draw(visu_frame, (0, 0, 0))
+
+        draw_line = False
 
         # Video and data replay loop
         try:
@@ -186,18 +178,18 @@ def main():
                         ts_aois_scenes[round(video_ts/1000)] = aoi2D_video_scene
 
                         # Draw gaze path
-                        for name, aoi in aoi2D_video_scene.items():
+                        look_at = aoi2D_video_scene[ref_aoi].look_at(video_gaze_pixel)
 
-                            if not aoi.looked(video_gaze_pixel):
-                                continue
+                        visu_gaze_pixel = aoi2D_visu_scene[ref_aoi].looked_pixel(look_at)
+                        cv.circle(visu_frame, visu_gaze_pixel, 4, (0, 0, 255), -1)
 
-                            ref_aoi = name #'Scene_Plan'
+                        if draw_line:
 
-                            look_at = aoi2D_video_scene[ref_aoi].look_at(video_gaze_pixel)
+                            # draw line
+                            cv.line(visu_frame, last_visu_gaze_pixel, visu_gaze_pixel, (0, 0, 255), 1)
 
-                            visu_gaze_pixel = aoi2D_visu_scene[ref_aoi].looked_pixel(look_at)
-                            
-                            cv.circle(visu_frame, visu_gaze_pixel, 4, aoi_color[ref_aoi], -1)
+                        last_visu_gaze_pixel = visu_gaze_pixel
+                        draw_line = True
 
                 # Close window using 'Esc' key
                 if cv.waitKey(1) == 27:
-- 
cgit v1.1