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
|
#!/usr/bin/env python
import argparse
from argaze import DataStructures, GazeFeatures
from argaze.TobiiGlassesPro2 import *
from argaze.utils import MiscFeatures
import cv2 as cv
import numpy
def main():
"""
Capture video camera and gaze data streams and synchronise them.
"""
# 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('-i', '--imu_calibration', metavar='IMU_CALIB', type=str, default=None, help='json imu calibration filepath')
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_video_freq_25()
# Enable tobii data stream
tobii_data_stream = tobii_controller.enable_data_stream()
# Enable tobii video stream
tobii_video_stream = tobii_controller.enable_video_stream()
# Create tobii imu handler
tobii_imu = TobiiInertialMeasureUnit.TobiiInertialMeasureUnit()
# Load optional imu calibration file
if args.imu_calibration != None:
tobii_imu.load_calibration_file(args.imu_calibration)
# Init head rotation speed
head_rotation_speed = numpy.zeros(3).astype(int)
# Init gaze position
gaze_position_px = GazeFeatures.GazePosition((0, 0))
# Init data timestamped in millisecond
data_ts_ms = 0
# Assess temporal performance
loop_chrono = MiscFeatures.TimeProbe()
gyroscope_chrono = MiscFeatures.TimeProbe()
gaze_chrono = MiscFeatures.TimeProbe()
loop_ps = 0
gyroscope_ps = 0
gaze_ps = 0
def data_stream_callback(data_ts, data_object, data_object_type):
nonlocal head_rotation_speed
nonlocal gaze_position_px
nonlocal data_ts_ms
nonlocal gyroscope_chrono
nonlocal gaze_chrono
data_ts_ms = data_ts / 1e3
match data_object_type:
case 'Gyroscope':
# Assess gyroscope stream performance
gyroscope_chrono.lap()
# Apply imu calibration
head_rotation_speed = tobii_imu.apply_gyroscope_offset(data_object).value.astype(int) * 5
case 'GazePosition':
# Assess gaze position stream performance
gaze_chrono.lap()
# Ignore frame when gaze position is not valid
if data_object.validity == 0:
gaze_position_px = GazeFeatures.GazePosition( (int(data_object.value[0] * video_frame.width), int(data_object.value[1] * video_frame.height)) )
case 'GazePosition3D':
# Ignore frame when gaze position 3D is not valid
if data_object.validity == 0:
gaze_accuracy_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.ACCURACY)) * data_object.value[2]
tobii_camera_hfov_mm = numpy.tan(numpy.deg2rad(TobiiSpecifications.CAMERA_HFOV / 2)) * data_object.value[2]
gaze_position_px.accuracy = round(video_frame.width * float(gaze_accuracy_mm) / float(tobii_camera_hfov_mm))
tobii_data_stream.reading_callback = data_stream_callback
# Start streaming
tobii_controller.start_streaming()
# Live video stream capture loop
try:
while tobii_video_stream.is_alive():
# Read video stream
video_ts, video_frame = tobii_video_stream.read()
video_ts_ms = video_ts / 1e3
# Assess loop performance
lap_time, lap_counter, elapsed_time = loop_chrono.lap()
# Update fps each 10 loops
if lap_counter >= 10:
loop_ps = 1e3 * lap_counter / elapsed_time
loop_chrono.restart()
# Assess gyroscope streaming performance
elapsed_time, lap_counter = gyroscope_chrono.end()
gyroscope_ps = 1e3 * lap_counter / elapsed_time
gyroscope_chrono.restart()
# Assess gaze streaming performance
elapsed_time, lap_counter = gaze_chrono.end()
gaze_ps = 1e3 * lap_counter / elapsed_time
gaze_chrono.restart()
# Draw head rotation speed considering only yaw and pitch values
cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2)), (int(video_frame.width/2) + head_rotation_speed[1], int(video_frame.height/2) - head_rotation_speed[0]), (150, 150, 150), 3)
# Draw gaze
gaze_position_px.draw(video_frame.matrix)
# Draw center
cv.line(video_frame.matrix, (int(video_frame.width/2) - 50, int(video_frame.height/2)), (int(video_frame.width/2) + 50, int(video_frame.height/2)), (255, 150, 150), 1)
cv.line(video_frame.matrix, (int(video_frame.width/2), int(video_frame.height/2) - 50), (int(video_frame.width/2), int(video_frame.height/2) + 50), (255, 150, 150), 1)
# Write stream timing
cv.rectangle(video_frame.matrix, (0, 0), (1100, 50), (63, 63, 63), -1)
cv.putText(video_frame.matrix, f'Data stream time: {int(data_ts_ms)} ms', (20, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.putText(video_frame.matrix, f'Video delay: {int(data_ts_ms - video_ts_ms)} ms', (550, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.putText(video_frame.matrix, f'Fps: {int(loop_ps)}', (950, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.rectangle(video_frame.matrix, (0, 50), (580, 100), (127, 127, 127), -1)
cv.putText(video_frame.matrix, f'Gyroscope fps: {int(gyroscope_ps)}', (20, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
cv.putText(video_frame.matrix, f'Gaze fps: {int(gaze_ps)}', (350, 80), cv.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 1, cv.LINE_AA)
cv.imshow(f'Video and data stream', video_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()
|
