1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
|
#!/usr/bin/env python
from dataclasses import dataclass, field
import math
import re
from argaze import DataStructures
from argaze.AreaOfInterest import AOIFeatures, AOI2DScene
import numpy
import cv2 as cv
# Define a zero distorsion matrix
D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0])
@dataclass
class AOI3DScene(AOIFeatures.AOIScene):
"""Define AOI 3D scene."""
rotation: list = field(init=False, default=numpy.asarray([0., 0., 0.]))
translation: list = field(init=False, default=numpy.asarray([0., 0., 0.]))
def __post_init__(self, **aois):
# set dimension member
self.dimension = 3
def load(self, obj_filepath: str):
"""Load AOI3D scene from .obj file."""
# 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:
name = None
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 aoi3D name
elif key == 'name':
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 aoi3D vertice id
elif key == 'face':
faces[name] = [int(i) for i in match.group(1).split()]
# go to next line
line = file.readline()
file.close()
# retreive all aoi3D vertices
for name, face in faces.items():
aoi3D = numpy.array([ vertices[i-1] for i in face ]).astype(numpy.float32).view(AOIFeatures.AreaOfInterest)
self[name] = aoi3D
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"""
aoi2D_scene = AOI2DScene.AOI2DScene()
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
return aoi2D_scene
|
