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
|
#!/usr/bin/env python
import argparse
import os
import time
from argaze import DataStructures
from argaze.TobiiGlassesPro2 import TobiiController, TobiiInertialMeasureUnit
from argaze.utils import MiscFeatures
import numpy
import matplotlib.pyplot as mpyplot
import matplotlib.patches as mpatches
def main():
"""
Calibrate Tobbi gyroscope and accelerometer sensors and finally outputs camera calibration data into a .json file.
### Reference:
- [Inertial Measure Unit calibration tutorial](https://makersportal.com/blog/calibration-of-an-inertial-measurement-unit-imu-with-raspberry-pi-part-ii)
"""
# 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')
parser.add_argument('-n', '--sample_number', metavar='BUFFER_SIZE', type=int, default=500, help='number of samples to store into calibration buffer')
parser.add_argument('-o', '--output', metavar='OUT', type=str, default='imu.json', help='destination 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()
# 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)
# Enable tobii data stream
tobii_data_stream = tobii_controller.enable_data_stream()
# Menu loop
try:
while True:
print('-' * 52)
menu_input = input('Tobii Inertial Measure Unit sensor calibration menu:\n\t\'a\' for accelerometer calibration.\n\t\'A\' for accelerometer visualisation.\n\t\'g\' for gyroscope calibration.\n\t\'G\' for gyroscope visualisation.\n\t\'p\' print current calibration.\n\t\'s\' save calibration.\n\t\'q\' quit calibration without saving.\n>')
match menu_input:
case 'a':
axis = ['X', 'Y', 'Z']
directions = ['upward', 'downward', 'perpendicular']
for i, axis in enumerate(axis):
print(f'\nACCELEROMETER {axis} AXIS CALIBRATION')
axis_buffers = {}
for j, direction in enumerate(directions):
input(f'\nKeep Tobii Glasses accelerometer {axis} axis {direction} then press \'Enter\' to start data acquisition.\n')
# Initialise progress bar
MiscFeatures.printProgressBar(0, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Capture accelerometer data stream
data_ts_buffer = DataStructures.TimeStampedBuffer()
for progress in tobii_data_stream.capture(data_ts_buffer, 'Accelerometer', args.sample_number):
# Update progress Bar
MiscFeatures.printProgressBar(progress, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
axis_buffers[direction] = data_ts_buffer
tobii_imu.calibrate_accelerometer_axis_coefficients(i, axis_buffers['upward'], axis_buffers['downward'], axis_buffers['perpendicular'])
accelerometer_coefficients = tobii_imu.get_accelerometer_coefficients()
print(f'\n\nAccelerometer optimal linear fit coefficients over {progress} values for each axis:')
print('\tX coefficients: ', accelerometer_coefficients[0])
print('\tY coefficients: ', accelerometer_coefficients[1])
print('\tZ coefficients: ', accelerometer_coefficients[2])
case 'A':
print('\nCAPTURE AND PLOT ACCELEROMETER STREAM')
# Initialise progress bar
MiscFeatures.printProgressBar(0, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Capture accelerometer data stream
data_ts_buffer = DataStructures.TimeStampedBuffer()
for progress in tobii_data_stream.capture(data_ts_buffer, 'Accelerometer', args.sample_number):
# Update progress Bar
MiscFeatures.printProgressBar(progress, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Edit figure
figure_width = min(args.sample_number/10, 56) # maximal width to display: 56 inches at 144 dpi < 2^16 pixels
data_sample = 8064 # 56 inches * 144 dpi = 8064 data can be displayed at max
figure = mpyplot.figure(figsize=(figure_width, 5), dpi=144)
# Plot data
subplot = figure.add_subplot(111)
subplot.set_title('Accelerometer', loc='left')
patches = data_ts_buffer.plot(names=['x','y','z'], colors=['#276FB6','#9427B6','#888888'], split={'value':['x','y','z']}, samples=data_sample)
subplot.legend(handles=patches, loc='upper left')
# Display figure
mpyplot.show()
figure.clear()
case 'g':
print('\nGYROSCOPE CALIBRATION')
input('Keep Tobii Glasses steady then press \'Enter\' to start data acquisition.\n')
# Initialise progress bar
MiscFeatures.printProgressBar(0, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Capture gyroscope data stream
data_ts_buffer = DataStructures.TimeStampedBuffer()
for progress in tobii_data_stream.capture(data_ts_buffer, 'Gyroscope', args.sample_number):
# Update progress Bar
MiscFeatures.printProgressBar(progress, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
gyroscope_offset = tobii_imu.calibrate_gyroscope_offset(data_ts_buffer)
print(f'\n\nGyroscope average over {progress} values for each axis:')
print('\tX offset: ', gyroscope_offset[0])
print('\tY offset: ', gyroscope_offset[1])
print('\tZ offset: ', gyroscope_offset[2])
case 'G':
print('\nCAPTURE AND PLOT GYROSCOPE STREAM')
# Initialise progress bar
MiscFeatures.printProgressBar(0, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Capture accelerometer data stream
data_ts_buffer = DataStructures.TimeStampedBuffer()
for progress in tobii_data_stream.capture(data_ts_buffer, 'Gyroscope', args.sample_number):
# Update progress Bar
MiscFeatures.printProgressBar(progress, args.sample_number, prefix = 'Data acquisition:', suffix = 'Complete', length = 100)
# Edit figure
figure_width = min(args.sample_number/10, 56) # maximal width to display: 56 inches at 144 dpi < 2^16 pixels
data_sample = 8064 # 56 inches * 144 dpi = 8064 data can be displayed at max
figure = mpyplot.figure(figsize=(figure_width, 5), dpi=144)
# Plot data
subplot = figure.add_subplot(111)
subplot.set_title('Gyroscope', loc='left')
patches = data_ts_buffer.plot(names=['x','y','z'], colors=['#276FB6','#9427B6','#888888'], split={'value':['x','y','z']}, samples=data_sample)
subplot.legend(handles=patches, loc='upper left')
# Display figure
mpyplot.show()
figure.clear()
case 'p':
gyroscope_offset = tobii_imu.get_gyroscope_offset()
print(f'\nGyroscope offset for each axis:')
print('\tX offset: ', gyroscope_offset[0])
print('\tY offset: ', gyroscope_offset[1])
print('\tZ offset: ', gyroscope_offset[2])
accelerometer_coefficients = tobii_imu.get_accelerometer_coefficients()
print(f'\nAccelerometer optimal linear fit coefficients for each axis:')
print('\tX coefficients: ', accelerometer_coefficients[0])
print('\tY coefficients: ', accelerometer_coefficients[1])
print('\tZ coefficients: ', accelerometer_coefficients[2])
case 's':
tobii_imu.save_calibration_file(args.output)
print(f'\nCalibration data exported into {args.output} file')
break
case 'q':
break
# exit on 'ctrl+C' interruption
except KeyboardInterrupt:
pass
if __name__ == '__main__':
main()
|
