Adding focus area and vision cone features.

1 files changed, 14 insertions, 1 deletions

diff --git a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py
index 14888bb..1ff3835 100644
--- a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py
+++ b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py
@@ -28,7 +28,6 @@ def main():
     parser.add_argument('-u', '--participant_name', metavar='PARTICIPANT_NAME', type=str, default=TobiiController.DEFAULT_PARTICIPANT_NAME, help='participant name')
     parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default='tobii_camera.json', help='json camera calibration filepath')
     parser.add_argument('-y', '--ivy_bus', metavar='IVY_BUS', type=str, default='0.0.0.0:2010', help='Ivy bus ip and port')
-    parser.add_argument('-a', '--aoi_scene', metavar='AOI_SCENE', type=str, default='aoi3D_scene.obj', help='obj aoi scene filepath')
     parser.add_argument('-md', '--marker_dictionary', metavar='DICT', type=str, default='DICT_ARUCO_ORIGINAL', help='aruco marker dictionnary (DICT_4X4_50, DICT_4X4_100, DICT_4X4_250, DICT_4X4_1000, DICT_5X5_50, DICT_5X5_100, DICT_5X5_250, DICT_5X5_1000, DICT_6X6_50, DICT_6X6_100, DICT_6X6_250, DICT_6X6_1000, DICT_7X7_50, DICT_7X7_100, DICT_7X7_250, DICT_7X7_1000, DICT_ARUCO_ORIGINAL,DICT_APRILTAG_16h5, DICT_APRILTAG_25h9, DICT_APRILTAG_36h10, DICT_APRILTAG_36h11)')
     parser.add_argument('-ms', '--marker_size', metavar='MKR', type=float, default=6, help='aruco marker size (cm)')
     parser.add_argument('-mi', '--marker_id_scene', metavar='MARKER_ID_SCENE', type=json.loads, help='{"marker": "aoi scene filepath"} dictionary')
@@ -115,6 +114,9 @@ def main():
             aruco_tracker.track(video_frame.matrix)
             aruco_tracker.draw(video_frame.matrix)
 
+            # Draw focus area
+            cv.circle(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), int(video_frame.width/3), (255, 150, 150), 1) 
+
             # Project 3D scenes related to each aruco markers
             if aruco_tracker.get_markers_number():
 
@@ -128,10 +130,21 @@ def main():
                     
                     if aoi3D_scene == None:
                         continue
+
+                    # Ignore marker out of focus area
+                    marker_x, marker_y = aruco_tracker.get_marker_center(i)
+                    distance_to_center = ( (video_frame.width/2 - marker_x)**2 + (video_frame.height/2 - marker_y)**2 )**0.5
+
+                    if distance_to_center > int(video_frame.width/3):
+                        continue
                     
                     aoi3D_scene.rotation = aruco_tracker.get_marker_rotation(i)
                     aoi3D_scene.translation = aruco_tracker.get_marker_translation(i)
 
+                    # Remove aoi outside vision field 
+                    # The vision cone tip is positionned behind the head
+                    aoi3D_scene = aoi3D_scene.clip(300, 150, cone_tip=[0., 0., -20.])
+
                     # Edit Zero distorsion matrix
                     D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0])