Clipping 3D scene to ignore aoi outside the vision field.

2 files changed, 56 insertions, 2 deletions

diff --git a/src/argaze/AreaOfInterest/AOI3DScene.py b/src/argaze/AreaOfInterest/AOI3DScene.py
index 658fa64..ca59024 100644
--- a/src/argaze/AreaOfInterest/AOI3DScene.py
+++ b/src/argaze/AreaOfInterest/AOI3DScene.py
@@ -96,6 +96,45 @@ class AOI3DScene(AOIFeatures.AOIScene):
 		except IOError:
 			raise IOError(f'File not found: {obj_filepath}')
 
+	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.
+		By default, the cone have the tip at origin and the base oriented to positive Z axis.
+		**Returns:** AOI3DScene"""
+
+		# define cone tip and direction as numpy array
+		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
+
+		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:
+
+				distance = numpy.dot(vertices - cone_tip, cone_direction)
+				radius = (distance / cone_height) * cone_radius
+				ortho_distance = numpy.linalg.norm((vertices - cone_tip) - distance * cone_direction)
+
+				if ortho_distance > radius:
+					one_vertice_out = True
+					break
+
+			# if no vertice is outside the cone, select the aoi
+			if not one_vertice_out:
+				aoi3D_scene_clipped[name] = aoi3D
+
+		return aoi3D_scene_clipped
+
 	def project(self, K, D=D0):
 		"""Project 3D scene onto 2D scene according optical parameters.
 		**Returns:** AOI2DScene"""
@@ -105,6 +144,7 @@ class AOI3DScene(AOIFeatures.AOIScene):
 		for name, aoi3D in self.items():
 
 			vertices_2D, J = cv.projectPoints(aoi3D, self.rotation, self.translation, K, D)
+			
 			aoi2D = vertices_2D.reshape((len(vertices_2D), 2)).astype(numpy.float32).view(AOIFeatures.AreaOfInterest)
 
 			aoi2D_scene[name] = aoi2D
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 721f8d7..25babf6 100644
--- a/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
+++ b/src/argaze/utils/export_tobii_segment_aruco_visual_scan.py
@@ -58,6 +58,16 @@ def main():
 
                 destination_path = args.segment_path
 
+                # Export into a dedicated time range folder
+                timerange_path = f'[{int(args.time_range[0])}s - {int(args.time_range[1])}s]'
+
+                destination_path = f'{destination_path}/{timerange_path}'
+
+                if not os.path.exists(destination_path):
+
+                    os.makedirs(destination_path)
+                    print(f'{destination_path} folder created')
+
         vs_data_filepath = f'{destination_path}/visual_scan.csv'
         vs_visu_filepath = f'{destination_path}/visual_scan_marker_%d.jpg'
         vs_video_filepath = f'{destination_path}/visual_scan.mp4'
@@ -206,7 +216,7 @@ def main():
                 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.height/2), (255, 150, 150), 1)
+                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 scene on each video frame and the visualisation frame
                 if aruco_tracker.get_markers_number():
@@ -226,12 +236,16 @@ def main():
                         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.height/2):
+                        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.])
+
                         # 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.project(aruco_camera.get_K())