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
|
#!/usr/bin/env python
import argparse
import os, json
from argaze import *
from argaze.TobiiGlassesPro2 import *
from argaze.ArUcoMarkers import *
from argaze.AreaOfInterest import *
from argaze.utils import MiscFeatures
import cv2 as cv
import numpy
def main():
"""
Track ArUcoScene into Tobii Glasses Pro 2 camera video stream.
"""
# Manage arguments
parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default=None, help='tobii glasses ip')
parser.add_argument('-p', '--project_path', metavar='ARGAZE_PROJECT', type=str, default=None, help='json argaze project filepath')
parser.add_argument('-b', '--borders', metavar='BORDERS', type=float, default=16.666, help='define left and right borders mask (%) to not track aruco out of these borders')
parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
args = parser.parse_args()
# Create tobii controller (with auto discovery network process if no ip argument is provided)
print("Looking for a Tobii Glasses Pro 2 device ...")
try:
tobii_controller = TobiiController.TobiiController(args.tobii_ip)
print(f'Tobii Glasses Pro 2 device found at {tobii_controller.address} address.')
except ConnectionError as e:
print(e)
exit()
# Setup camera at 25 fps to work on Full HD video stream
tobii_controller.set_scene_camera_freq_25()
# Print current confirugration
print(f'Tobii Glasses Pro 2 configuration:')
for key, value in tobii_controller.get_configuration().items():
print(f'\t{key}: {value}')
# Enable tobii data stream
tobii_data_stream = tobii_controller.enable_data_stream()
# Enable tobii video stream
tobii_video_stream = tobii_controller.enable_video_stream()
# Load ar scene
ar_scene = ArScene.ArScene.from_json(args.project_path)
print('\n')
print(ar_scene)
# Start streaming
tobii_controller.start_streaming()
# Live video stream capture loop
try:
# Assess loop performance
loop_chrono = MiscFeatures.TimeProbe()
fps = 0
while tobii_video_stream.is_alive():
# Read video stream
video_ts, video_frame = tobii_video_stream.read()
# Copy video frame to edit visualisation on it without disrupting aruco tracking
visu_frame = video_frame.copy()
# Process video and data frame
try:
# 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*args.borders/100), int(video_frame.height)), (0, 0, 0), -1)
cv.rectangle(video_frame.matrix, (int(video_frame.width*(1 - args.borders/100)), 0), (int(video_frame.width), int(video_frame.height)), (0, 0, 0), -1)
# Estimate scene pose from ArUco markers into frame.
tvec, rmat, _ = ar_scene.estimate_pose(video_frame.matrix)
# Project AOI scene into frame according estimated pose
aoi_scene_projection = ar_scene.project(tvec, rmat, visual_hfov=TobiiSpecifications.VISUAL_HFOV)
# Draw scene axis
ar_scene.draw_axis(visu_frame.matrix)
# Draw AOI
aoi_scene_projection.draw(visu_frame.matrix, (0, 0), color=(0, 255, 255))
# Draw tracked markers
ar_scene.aruco_tracker.draw_tracked_markers(visu_frame.matrix)
# Catch exceptions raised by estimate_pose and project methods
except (ArScene.PoseEstimationFailed, ArScene.SceneProjectionFailed) as e:
# Draw tracked markers
ar_scene.aruco_tracker.draw_tracked_markers(visu_frame.matrix)
cv.rectangle(visu_frame.matrix, (0, 50), (550, 100), (127, 127, 127), -1)
cv.putText(visu_frame.matrix, str(e), (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
# Assess loop performance
lap_time, lap_counter, elapsed_time = loop_chrono.lap()
# Update fps each 10 loops
if lap_counter >= 10:
fps = 1e3 * lap_counter / elapsed_time
loop_chrono.restart()
# Write stream timing
cv.rectangle(visu_frame.matrix, (0, 0), (700, 50), (63, 63, 63), -1)
cv.putText(visu_frame.matrix, f'Video stream time: {int(video_ts*1e-3)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.putText(visu_frame.matrix, f'Fps: {int(fps)}', (550, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.imshow(f'Stream ArUco AOI', visu_frame.matrix)
# Close window using 'Esc' key
if cv.waitKey(1) == 27:
break
# Exit on 'ctrl+C' interruption
except KeyboardInterrupt:
pass
# Stop frame display
cv.destroyAllWindows()
# Stop streaming
tobii_controller.stop_streaming()
if __name__ == '__main__':
main()
|