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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
|
#!/usr/bin/env python
import argparse
import os
import json
from argaze import DataStructures
from argaze import GazeFeatures
from argaze.TobiiGlassesPro2 import TobiiEntities, TobiiData, TobiiVideo, TobiiSpecifications
from argaze.ArUcoMarkers import *
from argaze.AreaOfInterest import *
from argaze.utils import MiscFeatures
import numpy
import cv2 as cv
def main():
"""
Track ArUco markers into Tobii Glasses Pro 2 segment video file.
For each loaded AOI scene .obj file, position the scene virtually relatively to each detected ArUco markers and project the scene into camera frame.
Export AOIs video and data as a aruco_aoi.csv, aruco_aoi.mp4 files
"""
# Manage arguments
parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
parser.add_argument('-s', '--segment_path', metavar='SEGMENT_PATH', type=str, default=None, help='segment path')
parser.add_argument('-r', '--time_range', metavar=('START_TIME', 'END_TIME'), nargs=2, type=float, default=(0., None), help='start and end time (in second)')
parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default=None, help='json camera calibration filepath')
parser.add_argument('-p', '--aruco_tracker_configuration', metavar='TRACK_CONFIG', type=str, default=None, help='json aruco tracker configuration filepath')
parser.add_argument('-md', '--marker_dictionary', metavar='MARKER_DICT', type=ArUcoMarkersDictionary.ArUcoMarkersDictionary, 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='MARKER_SIZE', 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')
parser.add_argument('-o', '--output', metavar='OUT', type=str, default=None, help='destination folder path (segment folder by default)')
parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
args = parser.parse_args()
if args.segment_path != None:
# Manage markers id to track
if args.marker_id_scene == None:
print(f'Track any Aruco markers from the {args.marker_dictionary.name} dictionary')
else:
print(f'Track Aruco markers {list(args.marker_id_scene.keys())} from the {args.marker_dictionary.name} dictionary')
# Manage destination path
destination_path = '.'
if args.output != None:
if not os.path.exists(os.path.dirname(args.output)):
os.makedirs(os.path.dirname(args.output))
print(f'{os.path.dirname(args.output)} folder created')
destination_path = args.output
else:
destination_path = args.segment_path
# Export into a dedicated time range folder
if args.time_range[1] != None:
timerange_path = f'[{int(args.time_range[0])}s - {int(args.time_range[1])}s]'
else:
timerange_path = f'[all]'
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}/aoi.csv'
vs_video_filepath = f'{destination_path}/aoi.mp4'
# Load a tobii segment
tobii_segment = TobiiEntities.TobiiSegment(args.segment_path, int(args.time_range[0] * 1e6), int(args.time_range[1] * 1e6) if args.time_range[1] != None else None)
# Load a tobii segment video
tobii_segment_video = tobii_segment.load_video()
print(f'Video properties:\n\tduration: {tobii_segment_video.duration/1e6} s\n\twidth: {tobii_segment_video.width} px\n\theight: {tobii_segment_video.height} px')
# Load a tobii segment data
tobii_segment_data = tobii_segment.load_data()
print(f'Loaded data count:')
for name in tobii_segment_data.keys():
print(f'\t{name}: {len(tobii_segment_data[name])} data')
# Access to video timestamp data buffer
tobii_ts_vts = tobii_segment_data['VideoTimeStamp']
# Access to timestamped head rotations data buffer
tobii_ts_head_rotations = tobii_segment_data['Gyroscope']
# Prepare video exportation at the same format than segment video
output_video = TobiiVideo.TobiiVideoOutput(vs_video_filepath, tobii_segment_video.stream)
# Create aruco camera
aruco_camera = ArUcoCamera.ArUcoCamera()
# Load calibration file
if args.camera_calibration != None:
aruco_camera.load_calibration_file(args.camera_calibration)
else:
raise UserWarning('.json camera calibration filepath required. Use -c option.')
# Create aruco tracker
aruco_tracker = ArUcoTracker.ArUcoTracker(args.marker_dictionary, args.marker_size, aruco_camera)
# Load specific configuration file
if args.aruco_tracker_configuration != None:
aruco_tracker.load_configuration_file(args.aruco_tracker_configuration)
print(f'ArUcoTracker configuration for {args.marker_dictionary.format} markers detection:')
aruco_tracker.print_configuration()
# Load AOI 3D scene for each marker and create a AOI 2D scene and frame when a 'Visualisation_Plan' AOI exist
aoi3D_scenes = {}
aoi2D_visu_scenes = {}
all_aois_names = []
for marker_id, aoi_scene_filepath in args.marker_id_scene.items():
marker_id = int(marker_id)
aoi3D_scenes[marker_id] = AOI3DScene.AOI3DScene()
aoi3D_scenes[marker_id].load(aoi_scene_filepath)
print(f'AOI in {os.path.basename(aoi_scene_filepath)} scene related to marker #{marker_id}:')
for aoi in aoi3D_scenes[marker_id].keys():
print(f'\t{aoi}')
# Store aoi name once
if aoi not in all_aois_names:
all_aois_names.append(aoi)
def aoi3D_scene_selector(marker_id):
return aoi3D_scenes.get(marker_id, None)
# Create timestamped buffer to store AOIs and primary time stamp offset
ts_offset_aois = DataStructures.TimeStampedBuffer()
# Video and data replay loop
try:
# Initialise progress bar
MiscFeatures.printProgressBar(0, tobii_segment_video.duration/1e3, prefix = 'Progress:', suffix = 'Complete', length = 100)
head_moving = False
head_movement_last = 0.
# Iterate on video frames
for video_ts, video_frame in tobii_segment_video.frames():
video_ts_ms = video_ts / 1e3
# Copy video frame to edit visualisation on it without disrupting aruco tracking
visu_frame = video_frame.copy()
# Process video and data frame
try:
# Get nearest video timestamp
_, nearest_vts = tobii_ts_vts.get_last_before(video_ts)
# Edit dictionary to store 2D aoi with primary timestamp offset and warning
all_aoi2D = {
'offset': nearest_vts.offset,
'warning': None
}
# Get nearest head rotation before video timestamp and remove all head rotations before
_, nearest_head_rotation = tobii_ts_head_rotations.pop_first_until(video_ts)
# Calculate head movement considering only head yaw and pitch
head_movement = numpy.array(nearest_head_rotation.value)
head_movement_px = head_movement.astype(int)
head_movement_norm = numpy.linalg.norm(head_movement[0:2])
# Draw movement vector
cv.line(visu_frame.matrix, (int(visu_frame.width/2), int(visu_frame.height/2)), (int(visu_frame.width/2) + head_movement_px[1], int(visu_frame.height/2) - head_movement_px[0]), (150, 150, 150), 3)
# Head movement detection hysteresis
# TODO : pass the threshold value as argument
if not head_moving and head_movement_norm > 50:
head_moving = True
if head_moving and head_movement_norm < 10:
head_moving = False
# When head is moving, ArUco tracking could return bad pose estimation and so bad AOI scene projection
if head_moving:
all_aoi2D['warning'] = 'Head is moving'
ts_offset_aois[video_ts] = all_aoi2D
raise UserWarning('Head is moving')
# Hide frame left and right borders before tracking to ignore markers outside focus area
cv.rectangle(video_frame.matrix, (0, 0), (int(video_frame.width/6), int(video_frame.height)), (0, 0, 0), -1)
cv.rectangle(video_frame.matrix, (int(video_frame.width*(1 - 1/6)), 0), (int(video_frame.width), int(video_frame.height)), (0, 0, 0), -1)
# Track markers with pose estimation and draw them
aruco_tracker.track(video_frame.matrix)
aruco_tracker.draw_tracked_markers(visu_frame.matrix)
# When no marker is detected, no AOI scene projection can't be done
if len(aruco_tracker.tracked_markers) == 0:
all_aoi2D['warning'] = 'No marker detected'
ts_offset_aois[video_ts] = all_aoi2D
raise UserWarning('No marker detected')
# Store aoi 2D video for further scene merging
aoi2D_dict = {}
# Project 3D scene on each video frame and the visualisation frame
for (marker_id, marker) in aruco_tracker.tracked_markers.items():
# Copy 3D scene related to detected marker
aoi3D_scene = aoi3D_scene_selector(marker_id)
if aoi3D_scene == None:
continue
# Transform scene into camera referential
aoi3D_camera = aoi3D_scene.transform(marker.translation, marker.rotation)
# Get aoi inside vision cone field
cone_vision_height_cm = 200 # cm
cone_vision_radius_cm = numpy.tan(numpy.deg2rad(TobiiSpecifications.VISUAL_HFOV / 2)) * cone_vision_height_cm
aoi3D_inside, aoi3D_outside = aoi3D_camera.vision_cone(cone_vision_radius_cm, cone_vision_height_cm)
# Keep only aoi inside vision cone field
aoi3D_scene = aoi3D_scene.copy(exclude=aoi3D_outside.keys())
# 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(marker.translation, marker.rotation, aruco_camera.K)
# Store each 2D aoi for further scene merging
for name, aoi in aoi2D_video_scene.items():
if name not in aoi2D_dict.keys():
aoi2D_dict[name] = []
aoi2D_dict[name].append(aoi.clockwise())
# Merge all 2D aoi into a single 2D scene
aoi2D_merged_scene = AOI2DScene.AOI2DScene()
for name, aoi_array in aoi2D_dict.items():
aoi2D_merged_scene[name] = numpy.sum(aoi_array, axis=0) / len(aoi_array)
aoi2D_merged_scene.draw(visu_frame.matrix, (0, 0))
# Store all 2D aoi
for aoi_name in aoi2D_merged_scene.keys():
all_aoi2D[aoi_name] = numpy.rint(aoi2D_merged_scene[aoi_name]).astype(int)
ts_offset_aois[video_ts] = all_aoi2D
# Warn user when the merged scene is empty
if len(aoi2D_merged_scene.keys()) == 0:
raise UserWarning('Scene is empty')
# Write warning
except UserWarning as w:
cv.rectangle(visu_frame.matrix, (0, 50), (550, 100), (127, 127, 127), -1)
cv.putText(visu_frame.matrix, str(w), (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
# Raised when timestamped buffer is empty
except KeyError:
pass
# Draw focus area
cv.rectangle(visu_frame.matrix, (int(video_frame.width/6), 0), (int(visu_frame.width*(1-1/6)), int(visu_frame.height)), (255, 150, 150), 1)
# Draw center
cv.line(visu_frame.matrix, (int(visu_frame.width/2) - 50, int(visu_frame.height/2)), (int(visu_frame.width/2) + 50, int(visu_frame.height/2)), (255, 150, 150), 1)
cv.line(visu_frame.matrix, (int(visu_frame.width/2), int(visu_frame.height/2) - 50), (int(visu_frame.width/2), int(visu_frame.height/2) + 50), (255, 150, 150), 1)
# Write segment timing
cv.rectangle(visu_frame.matrix, (0, 0), (550, 50), (63, 63, 63), -1)
cv.putText(visu_frame.matrix, f'Segment time: {int(video_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
if args.window:
# Close window using 'Esc' key
if cv.waitKey(1) == 27:
break
# Display visualisation
cv.imshow(f'Segment {tobii_segment.id} ArUco AOI', visu_frame.matrix)
# Write video
output_video.write(visu_frame.matrix)
# Update Progress Bar
progress = video_ts_ms - int(args.time_range[0] * 1e3)
MiscFeatures.printProgressBar(progress, tobii_segment_video.duration/1e3, prefix = 'Progress:', suffix = 'Complete', length = 100)
# Exit on 'ctrl+C' interruption
except KeyboardInterrupt:
pass
# Stop frame display
cv.destroyAllWindows()
# End output video file
output_video.close()
# Print aruco tracking metrics
print('\nAruco marker tracking metrics')
try_count, tracked_counts = aruco_tracker.track_metrics
for marker_id, tracked_count in tracked_counts.items():
print(f'Markers {marker_id} has been detected in {tracked_count} / {try_count} frames ({round(100 * tracked_count / try_count, 2)} %)')
# Export aruco aoi data
ts_offset_aois.save_as_csv(vs_data_filepath)
print(f'Aruco AOI data saved into {vs_data_filepath}')
# Notify when the aruco aoi video has been exported
print(f'Aruco AOI video saved into {vs_video_filepath}')
if __name__ == '__main__':
main()
|