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
|
#!/usr/bin/env python
import argparse
import os, json
from argaze import DataStructures
from argaze import GazeFeatures
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 ArUcoCube 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('-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('-ac', '--aruco_cube', metavar='ARUCO_CUBE', type=str, help='json arcube description filepath')
parser.add_argument('-to', '--tolerance', metavar='TOLERANCE', type=float, default=1, help='arcube face pose estimation tolerance')
parser.add_argument('-w', '--window', metavar='DISPLAY', type=bool, default=True, help='enable window display', action=argparse.BooleanOptionalAction)
args = parser.parse_args()
# Load ArUcoCube
arcube = ArUcoCube.ArUcoCube(args.aruco_cube)
arcube.print_cache()
# Create tobii controller (with auto discovery network process if no ip argument is provided)
print('\nLooking 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()
# 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 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(arcube.dictionary, arcube.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 markers detection:')
aruco_tracker.print_configuration()
# Init head pose tracking
head_translation = numpy.array((0, 0, 0))
head_rotation = numpy.array((0, 0, 0))
# Init data timestamped in millisecond
data_ts_ms = 0
# Assess temporal performance
loop_chrono = MiscFeatures.TimeProbe()
loop_ps = 0
def data_stream_callback(data_ts, data_object, data_object_type):
nonlocal head_translation
nonlocal head_rotation
nonlocal data_ts_ms
data_ts_ms = data_ts / 1e3
match data_object_type:
case 'Accelerometer':
# Integrate head translation over time
#head_translation += numpy.array(data_object.value)
pass
case 'Gyroscope':
# Integrate head rotation over time
#head_rotation += numpy.array(data_object.value)
pass
tobii_data_stream.reading_callback = data_stream_callback
# 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()
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:
# Track markers with pose estimation
aruco_tracker.track(video_frame.matrix)
# Draw markers pose estimation
aruco_tracker.draw_tracked_markers(visu_frame.matrix)
# Warn user that no markers have been detected
if aruco_tracker.get_tracked_markers_number() == 0:
raise UserWarning('No marker detected')
# Estimate cube pose from tracked markers
arcube.estimate_pose(aruco_tracker.get_tracked_markers())
# Draw cube axis
arcube.draw(visu_frame.matrix, aruco_camera.get_K())
# Write warning
except UserWarning as w:
cv.rectangle(visu_frame.matrix, (0, 100), (500, 150), (127, 127, 127), -1)
cv.putText(visu_frame.matrix, str(w), (20, 140), 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:
loop_ps = 1e3 * lap_counter / elapsed_time
loop_chrono.restart()
# 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 stream timing
cv.rectangle(visu_frame.matrix, (0, 0), (1100, 50), (63, 63, 63), -1)
cv.putText(visu_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(visu_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(visu_frame.matrix, f'Fps: {int(loop_ps)}', (950, 40), cv.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 1, cv.LINE_AA)
cv.imshow(f'Stream ArUcoCube', 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()
|
