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
|
#!/usr/bin/env python
import argparse
import os
from argaze.ArUcoMarkers import ArUcoTracker, ArUcoCamera
from argaze.RegionOfInterest import *
from argaze.TobiiGlassesPro2 import *
import cv2 as cv
import pandas
import matplotlib.pyplot as mpyplot
import matplotlib.patches as mpatches
def main():
"""
Track any ArUco marker into Tobii Glasses Pro 2 camera video stream.
From a loaded ROI scene .obj file, position the scene virtually like the detected ArUco markers and project the scene into camera frame.
Then, detect if Tobii gaze point is inside any ROI.
Export all collected datas into an export folder for further analysis.
"""
# manage arguments
parser = argparse.ArgumentParser(description=main.__doc__.split('-')[0])
parser.add_argument('-t', '--tobii_ip', metavar='TOBII_IP', type=str, default='192.168.1.10', help='tobii glasses ip')
parser.add_argument('-c', '--camera_calibration', metavar='CAM_CALIB', type=str, default='tobii_camera.json', help='json camera calibration filepath')
parser.add_argument('-s', '--roi_scene', metavar='ROI_SCENE', type=str, default='roi3D_scene.obj', help='obj roi scene filepath')
parser.add_argument('-o', '--output', metavar='OUT', type=str, default='.', help='destination path')
parser.add_argument('-d', '--dictionary', metavar='DICT', type=str, default='DICT_4X4_50', help='aruco marker dictionnary')
parser.add_argument('-m', '--marker_size', metavar='MKR', type=int, default=6, help='aruco marker size (cm)')
args = parser.parse_args()
# create tobii controller
tobii_controller = TobiiController.TobiiController(args.tobii_ip, 'ArGaze', 1)
# create tobii data thread
tobii_data_thread = TobiiData.TobiiDataThread(tobii_controller)
tobii_data_thread.start()
# create tobii video thread
tobii_video_thread = TobiiVideo.TobiiVideoThread(tobii_controller)
tobii_video_thread.start()
# create aruco camera
aruco_camera = ArUcoCamera.ArUcoCamera()
aruco_camera.load_calibration_file(args.camera_calibration)
# create aruco tracker
aruco_tracker = ArUcoTracker.ArUcoTracker(args.dictionary, 6, aruco_camera) # aruco dictionaries, marker length (cm), camera
# create ROIs 3D scene
roi3D_scene = ROI3DScene.ROI3DScene()
roi3D_scene.load(args.roi_scene)
# start tobii glasses streaming
tobii_controller.start_streaming()
# process video frames
last_frame_time = 0
roi2D_buffer = []
marker_buffer = []
while True:
frame, frame_width, frame_height, frame_time, pts = tobii_video_thread.read()
# draw tobii gaze
# TODO : sync gaze data according frame pts
gp_data = tobii_data_thread.read_gaze_data(pts)
if 'TIMESTAMP' in gp_data:
pointer = (int(gp_data['X'] * frame_width), int(gp_data['Y'] * frame_height))
cv.circle(frame, pointer, 4, (0, 255, 255), -1)
else:
pointer = (0, 0)
# track markers with pose estimation and draw them
aruco_tracker.track(frame)
aruco_tracker.draw(frame)
# project 3D scenes related to each aruco markers
if aruco_tracker.get_markers_number():
for (i, marker_id) in enumerate(aruco_tracker.get_markers_ids()):
# TODO : select different 3D scenes depending on aruco id
marker_rotation = aruco_tracker.get_marker_rotation(i)
marker_translation = aruco_tracker.get_marker_translation(i)
roi3D_scene.set_rotation(marker_rotation)
roi3D_scene.set_translation(marker_translation)
# zero distorsion matrix
D0 = numpy.asarray([0.0, 0.0, 0.0, 0.0, 0.0])
# 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 ROI, where the distorsion is low, it is acceptable.
roi2D_scene = roi3D_scene.project(aruco_camera.getK(), D0)
# check if gaze is inside 2D rois
roi2D_scene.inside(pointer)
# draw 2D rois
roi2D_scene.draw(frame)
# store roi2D into buffer
for roi2D in roi2D_scene:
roi2D['TIME'] = frame_time
del roi2D['VERTICES']
roi2D_buffer.append(roi2D)
# store marker into buffer
marker = {
'TIME': frame_time,
'ID': i,
'X': marker_translation[0][0],
'Y': marker_translation[0][1],
'Z': marker_translation[0][2]
}
marker_buffer.append(marker)
cv.imshow(f'Live Scene', frame)
# quit on 'Esc' command
key = cv.waitKey(1)
if key == 27:
cv.destroyAllWindows()
last_frame_time = frame_time
break
# stop tobii objects
tobii_video_thread.stop()
tobii_data_thread.stop()
tobii_controller.stop_streaming()
tobii_controller.close()
# create a pandas DataFrame for each buffer
ac_dataframe = pandas.DataFrame(tobii_data_thread.read_accelerometer_buffer(), columns=['TIMESTAMP', 'TIME', 'X', 'Y', 'Z'])
gy_dataframe = pandas.DataFrame(tobii_data_thread.read_gyroscope_buffer(), columns=['TIMESTAMP', 'TIME', 'X', 'Y', 'Z'])
gp_dataframe = pandas.DataFrame(tobii_data_thread.read_gaze_buffer(), columns=['TIMESTAMP', 'TIME', 'X', 'Y'])
data_pts_dataframe = pandas.DataFrame(tobii_data_thread.read_pts_buffer(), columns=['TIMESTAMP', 'TIME', 'PTS'])
video_pts_dataframe = pandas.DataFrame(tobii_video_thread.read_pts_buffer(), columns=['TIME', 'PTS'])
roi2D_dataframe = pandas.DataFrame(roi2D_buffer, columns=['TIME', 'NAME', 'POINTER_INSIDE'])
marker_dataframe = pandas.DataFrame(marker_buffer, columns=['TIME', 'ID', 'X', 'Y', 'Z'])
# manage export folder
if not os.path.exists(args.output):
os.makedirs(args.output)
print(f'{args.output} folder created')
# export all data frames
ac_dataframe.to_csv(f'{args.output}/accelerometer.csv', index=False)
gy_dataframe.to_csv(f'{args.output}/gyroscope.csv', index=False)
gp_dataframe.to_csv(f'{args.output}/gaze.csv', index=False)
data_pts_dataframe.to_csv(f'{args.output}/data_pts.csv', index=False)
video_pts_dataframe.to_csv(f'{args.output}/video_pts.csv', index=False)
roi2D_dataframe.to_csv(f'{args.output}/rois.csv', index=False)
marker_dataframe.to_csv(f'{args.output}/markers.csv', index=False)
# edit figure
figure = mpyplot.figure(figsize=(int(last_frame_time), 5))
# plot gaze data
subplot = figure.add_subplot(211)
subplot.set_title('Gaze')
subplot = gp_dataframe.plot(x='TIME', y='X', xlim=(0, last_frame_time), ax=subplot, color='#276FB6', xlabel='Time (s)', ylabel='X (normalized)', legend=False)
subplot = gp_dataframe.plot(x='TIME', y='Y', xlim=(0, last_frame_time), ax=subplot.twinx(), color='#9427B6', xlabel='Time (s)', ylabel='Y (normalized)', legend=False)
x_patch = mpatches.Patch(color='#276FB6', label='X')
y_speed_patch = mpatches.Patch(color='#9427B6', label='Y')
subplot.legend(handles=[x_patch, y_speed_patch], loc='upper left')
# plot maker position data
subplot = figure.add_subplot(212)
subplot.set_title('Marker')
subplot = marker_dataframe.plot(x='TIME', y='X', xlim=(0, last_frame_time), ax=subplot, color='#276FB6', xlabel='Time (s)', ylabel='X (cm)', legend=False)
subplot = marker_dataframe.plot(x='TIME', y='Y', xlim=(0, last_frame_time), ax=subplot.twinx(), color='#9427B6', xlabel='Time (s)', ylabel='Y (cm)', legend=False)
x_patch = mpatches.Patch(color='#276FB6', label='X')
y_speed_patch = mpatches.Patch(color='#9427B6', label='Y')
subplot.legend(handles=[x_patch, y_speed_patch], loc='upper left')
# export figure
mpyplot.tight_layout()
mpyplot.savefig(f'{args.output}/visualisation.svg')
mpyplot.close('all')
if __name__ == '__main__':
main()
|
