Rewriting vision cone clipping.

5 files changed, 32 insertions, 26 deletions

diff --git a/src/argaze/AreaOfInterest/AOI3DScene.py b/src/argaze/AreaOfInterest/AOI3DScene.py
index 44b8e3c..ee07043 100644
--- a/src/argaze/AreaOfInterest/AOI3DScene.py
+++ b/src/argaze/AreaOfInterest/AOI3DScene.py
@@ -133,8 +133,8 @@ class AOI3DScene(AOIFeatures.AOIScene):
 				file.write('s off\n')
 				file.write(vertices_ids + '\n')
 
-	def clip(self, cone_radius, cone_height, cone_tip=[0., 0., 0.], cone_direction=[0., 0., 1.]):
-		"""Select AOI which are inside a given cone field.
+	def vision_cone(self, cone_radius, cone_height, cone_tip=[0., 0., 0.], cone_direction=[0., 0., 1.]):
+		"""Get AOI which are inside and out a given cone field.
 		By default, the cone have the tip at origin and the base oriented to positive Z axis.
 		**Returns:** AOI3DScene"""
 
@@ -142,21 +142,14 @@ class AOI3DScene(AOIFeatures.AOIScene):
 		cone_tip = numpy.array(cone_tip).astype(numpy.float32)
 		cone_direction = numpy.array(cone_direction).astype(numpy.float32)
 
-		# retreive rotation matrix from rotation vector
-		R, _ = cv.Rodrigues(self.rotation)
-
-		# store valid aoi into a new scene with same pose
-		aoi3D_scene_clipped = AOI3DScene()
-		aoi3D_scene_clipped.rotation = self.rotation
-		aoi3D_scene_clipped.translation = self.translation
+		# sort aoi
+		aoi3D_scene_inside = AOI3DScene()
+		aoi3D_scene_outside = AOI3DScene()
 
 		for name, aoi3D in self.items():
 
-			# rotate and translate back aoi to compensate the scene pose
-			aoi3D_comp = aoi3D.dot(R.T) + self.translation
-
 			one_vertice_out = False
-			for vertices in aoi3D_comp:
+			for vertices in aoi3D:
 
 				distance = numpy.dot(vertices - cone_tip, cone_direction)
 				radius = (distance / cone_height) * cone_radius
@@ -166,11 +159,13 @@ class AOI3DScene(AOIFeatures.AOIScene):
 					one_vertice_out = True
 					break
 
-			# if no vertice is outside the cone, select the aoi
+			# if no vertice is outside the cone, aoi is inside
 			if not one_vertice_out:
-				aoi3D_scene_clipped[name] = aoi3D
+				aoi3D_scene_inside[name] = aoi3D
+			else:
+				aoi3D_scene_outside[name] = aoi3D
 
-		return aoi3D_scene_clipped
+		return aoi3D_scene_inside, aoi3D_scene_outside
 
 	def project(self, T=T0, R=R0, K=K0, D=D0):
 		"""Project 3D scene onto 2D scene according translation, rotation and optical parameters.
diff --git a/src/argaze/AreaOfInterest/AOIFeatures.py b/src/argaze/AreaOfInterest/AOIFeatures.py
index 987a25f..01bb484 100644
--- a/src/argaze/AreaOfInterest/AOIFeatures.py
+++ b/src/argaze/AreaOfInterest/AOIFeatures.py
@@ -142,6 +142,10 @@ class AOIScene():
         """Add an aoi to the scene."""
         self.areas[name] = aoi.tolist()
 
+    def __delitem__(self, key):
+        """Remove an aoi from the scene."""
+        del self.areas[key]
+
     def items(self):
         for name, area in self.areas.items():
             yield name, numpy.array(area).astype(numpy.float32).view(AreaOfInterest)
diff --git a/src/argaze/utils/edit_tobii_segment_aruco_pose.py b/src/argaze/utils/edit_tobii_segment_aruco_pose.py
index 05da07d..fd8f45b 100644
--- a/src/argaze/utils/edit_tobii_segment_aruco_pose.py
+++ b/src/argaze/utils/edit_tobii_segment_aruco_pose.py
@@ -297,10 +297,6 @@ def main():
                                 cv.putText(video_frame.matrix, f'{T[1]:.3f}', (40, 400), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 1, cv.LINE_AA)
                                 cv.putText(video_frame.matrix, f'{T[2]:.3f}', (40, 440), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 1, cv.LINE_AA)
                             
-                            # Remove aoi outside vision field 
-                            # The vision cone tip is positionned behind the head
-                            #aoi3D_scene_edited = aoi3D_scene_edited.clip(300, 150, cone_tip=[0., 0., -20.])
-
                             # 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_video_scene = aoi3D_scene_edited.project(aruco_tracker.get_marker_translation(selected_marker_index), aruco_tracker.get_marker_rotation(selected_marker_index), aruco_camera.get_K())
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 5e487c1..e2e52cc 100644
--- a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
+++ b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
@@ -268,9 +268,16 @@ def main():
                         if distance_to_center > int(video_frame.width/3):
                             continue
 
-                        # Remove aoi outside vision field 
+                        # Transform scene into camera referential
+                        aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i))
+
+                        # Get aoi inside vision cone field 
                         # The vision cone tip is positionned behind the head
-                        aoi3D_scene = aoi3D_scene.clip(300, 150, cone_tip=[0., 0., -20.])
+                        aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(300, 150, cone_tip=[0., 0., -20.])
+
+                        # Keep only aoi inside vision cone field
+                        for name, aoi in aoi3D_outside.items():
+                            del aoi3D_scene[name]
 
                         # 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.
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 e1a6e87..51d45f3 100644
--- a/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py
+++ b/src/argaze/utils/live_tobii_aruco_aoi_ivy_controller.py
@@ -164,12 +164,16 @@ def main():
                     if distance_to_center > int(video_frame.width/3):
                         continue
 
-                    # Remove aoi outside vision field 
+                    # Transform scene into camera referential
+                    aoi3D_camera = aoi3D_scene.transform(aruco_tracker.get_marker_translation(i), aruco_tracker.get_marker_rotation(i))
+
+                    # Get aoi inside vision cone field 
                     # The vision cone tip is positionned behind the head
-                    aoi3D_scene = aoi3D_scene.clip(300, 150, cone_tip=[0., 0., -20.])
+                    aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(300, 150, cone_tip=[0., 0., -20.])
 
-                    # Edit Zero distorsion matrix
-                    D0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0])
+                    # Keep only aoi inside vision cone field
+                    for name, aoi in aoi3D_outside.items():
+                        del aoi3D_scene[name]
 
                     # 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.