1 files changed, 139 insertions, 0 deletions
diff --git a/src/argaze/RegionOfInterest/ROI3DScene.py b/src/argaze/RegionOfInterest/ROI3DScene.py
new file mode 100644
index 0000000..d292d2a
--- /dev/null
+++ b/src/argaze/RegionOfInterest/ROI3DScene.py
@@ -0,0 +1,139 @@
+import math
+import re
+
+from argaze.RegionOfInterest import ROI2DScene
+
+import numpy
+import cv2 as cv
+import matplotlib.path as mpath
+
+class ROI3DScene(list):
+
+	# subclass list
+	def __new__(cls):
+		return super(ROI3DScene, cls).__new__(cls)
+
+	# initialisation
+	def __init__(self):
+
+		# define rotation and translation matrix
+		self.__rotation = [0, 0, 0]
+		self.__translation = [0, 0, 0]
+
+		# define a zero distorsion matrix
+		self.__D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0])
+
+	# destruction
+	def __del__(self):
+		pass
+
+	# load scen from .obj file
+	def load(self, obj_filepath):
+
+		# regex rules for .obj file parsing
+		OBJ_RX_DICT = {
+			'comment': re.compile(r'#(.*)\n'),
+			'name': re.compile(r'o (\w+)(.*)\n'),
+			'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'),
+			'face': re.compile(r'f (.*)\n')
+		}
+
+        # regex .obj line parser
+		def __parse_obj_line(line):
+
+			for key, rx in OBJ_RX_DICT.items():
+				match = rx.search(line)
+				if match:
+					return key, match
+
+			# if there are no matches
+			return None, None
+        
+        # start parsing
+		try:
+
+			roi3D = {}
+			vertices = []
+			faces = []
+
+			# open the file and read through it line by line
+			with open(obj_filepath, 'r') as file:
+
+				line = file.readline()
+
+				while line:
+
+					# at each line check for a match with a regex
+					key, match = __parse_obj_line(line)
+
+					# extract comment
+					if key == 'comment':
+						pass
+
+					# extract roi3D name
+					elif key == 'name':
+
+						roi3D['NAME'] = str(match.group(1))
+
+					# fill vertices array
+					elif key == 'vertice':
+
+						vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))]))
+
+					# extract roi3D vertice id
+					elif key == 'face':
+
+						roi3D['FACE'] = [int(i) for i in match.group(1).split()]
+
+						# store roi3D dict into scene array
+						self.append(roi3D)
+
+						# clear roi3D dict
+						roi3D = {}
+
+					# go to next line
+					line = file.readline()
+
+				file.close()
+
+				# retreive all roi3D vertices
+				for roi3D in self:
+					roi3D['VERTICES'] = [ vertices[i-1] for i in roi3D['FACE'] ]
+					roi3D.pop('FACE', None)
+
+				# print scene
+				for roi3D in self:
+					name = roi3D['NAME']
+					vertices = roi3D['VERTICES']
+
+		except IOError:
+			raise IOError(f'File not found: {obj_filepath}')
+
+	def set_rotation(self, rvec):
+
+		self.__rotation = rvec
+
+	def set_translation(self, tvec):
+
+		self.__translation = tvec
+
+	# project 3D scene onto 2D scene through a camera
+	def project(self, frame, camera, apply_distorsion = True):
+
+		roi2D_scene = ROI2DScene.ROI2DScene()
+
+		for roi3D in self:
+
+			vertices_3D = numpy.array(roi3D['VERTICES']).astype('float32')
+
+			vertices_2D, J = cv.projectPoints(vertices_3D, self.__rotation, self.__translation, camera.get_K(), camera.get_D() if apply_distorsion else self.__D0)
+			vertices_2D = vertices_2D.astype('int').reshape((len(vertices_2D), 2))
+
+			roi2D = {
+				'NAME': roi3D['NAME'],
+				'VERTICES': vertices_2D
+			}
+
+			roi2D_scene.append(roi2D)
+
+		return roi2D_scene