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
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
|
#!/usr/bin/env python
import json
import time
import math
from argaze import DataStructures
from argaze.TobiiGlassesPro2 import TobiiData
import numpy
from scipy.optimize import curve_fit
import cv2 as cv
# Earth gravity force (m/s2)
EARTH_GRAVITY = -9.81
EARTH_GRAVITY_VECTOR = [0, EARTH_GRAVITY, 0]
# Translation vector from camera referential to imu referential (cm)
CAMERA_TO_IMU_TRANSLATION_VECTOR = [8, -1, -5]
# Rotation vector from camera referential to imu referential (euler, degree)
CAMERA_TO_IMU_ROTATION_VECTOR = [18, 0, 180]
class TobiiInertialMeasureUnit():
"""Ease Tobbi IMU data handling."""
def __init__(self):
"""Define IMU calibration data."""
self.__gyroscope_offset = numpy.zeros(3)
self.__accelerometer_coefficients = numpy.array([[1., 0.], [1., 0.], [1., 0.]])
self.__plumb = numpy.array(EARTH_GRAVITY_VECTOR)
self.reset_rotation()
self.reset_translation()
def load_calibration_file(self, calibration_filepath):
"""Load IMU calibration from a .json file."""
with open(calibration_filepath) as calibration_file:
# Deserialize .json
# TODO find a better way
calibration_data = json.load(calibration_file)
# Load calibration data
self.__gyroscope_offset = numpy.array(calibration_data['gyroscope_offset'])
self.__accelerometer_coefficients = numpy.array(calibration_data['accelerometer_coefficients'])
def save_calibration_file(self, calibration_filepath):
"""Save IMU calibration into .json file."""
calibration_data = {
'gyroscope_offset': list(self.__gyroscope_offset),
'accelerometer_coefficients': [list(self.__accelerometer_coefficients[0]), list(self.__accelerometer_coefficients[1]), list(self.__accelerometer_coefficients[2])]
}
with open(calibration_filepath, 'w', encoding='utf-8') as calibration_file:
json.dump(calibration_data, calibration_file, ensure_ascii=False, indent=4)
def calibrate_gyroscope_offset(self, gyroscope_ts_buffer):
# Consider gyroscope values without timestamps
gyroscope_values = []
for ts, data_object in gyroscope_ts_buffer.items():
gyroscope_values.append(data_object.value)
# Calculate average value for each axis
gx_offset = numpy.mean(numpy.array(gyroscope_values)[:, 0])
gy_offset = numpy.mean(numpy.array(gyroscope_values)[:, 1])
gz_offset = numpy.mean(numpy.array(gyroscope_values)[:, 2])
# Store result
self.__gyroscope_offset = numpy.array([gx_offset, gy_offset, gz_offset])
return self.__gyroscope_offset
def get_gyroscope_offset(self):
"""Get gyroscope offset."""
return self.__gyroscope_offset
def apply_gyroscope_offset(self, gyroscope_data_object):
"""Remove gyroscope offset to given gyroscope data."""
return TobiiData.Gyroscope(gyroscope_data_object.value - self.__gyroscope_offset)
def reset_rotation(self):
"""Reset rotation value before to start integration process."""
self.__last_gyroscope_ts = None
self.__rotation = numpy.zeros(3)
def update_rotation(self, gyroscope_data_ts, gyroscope_data_object):
"""Integrate timestamped gyroscope values to update rotation."""
# Convert deg/s into deg/ms
current_gyroscope = gyroscope_data_object.value * 1e-3
# Init gyroscope integration
if self.__last_gyroscope_ts == None:
self.__last_gyroscope_ts = gyroscope_data_ts
self.__last_gyroscope = current_gyroscope
# Calculate elapsed time in ms
delta_time = (gyroscope_data_ts - self.__last_gyroscope_ts) / 1e3
# Integrate gyroscope
self.__rotation = self.__rotation + (self.__last_gyroscope * delta_time)
# Store current as last
self.__last_gyroscope_ts = gyroscope_data_ts
self.__last_gyroscope = current_gyroscope
def get_rotation(self):
"""Return current rotation value."""
return self.__rotation
def _accelerometer_linear_fit(self, x, a, b):
"""Linear function for accelerometer axis correction."""
return a * x + b
def calibrate_accelerometer_axis_coefficients(self, axis, upward_ts_buffer, downward_ts_buffer, perpendicular_ts_buffer):
"""Calibrate one accelerometer axis using three data set (upward/+1g, downward/-1g, perpendicular/0g) for linear fit."""
# Consider accelerometer axis values without timestamps
accelerometer_values = []
expected_values = []
for (upward_ts, upward_data_object), (downward_ts, downward_data_object), (perpendicular_ts, perpendicular_data_object) in zip(upward_ts_buffer.items(), downward_ts_buffer.items(), perpendicular_ts_buffer.items()):
accelerometer_values.append(upward_data_object.value[axis])
expected_values.append(+EARTH_GRAVITY)
accelerometer_values.append(downward_data_object.value[axis])
expected_values.append(-EARTH_GRAVITY)
accelerometer_values.append(perpendicular_data_object.value[axis])
expected_values.append(0.0)
# Find optimal coefficients according linear fit between accelerometer values and expected values
optimal_coefficients, _ = curve_fit(self._accelerometer_linear_fit, accelerometer_values, expected_values, maxfev = 10000)
# Store results for the given axis
self.__accelerometer_coefficients[axis] = numpy.array(optimal_coefficients)
def get_accelerometer_coefficients(self):
"""Return accelerometer coefficients."""
return self.__accelerometer_coefficients
def apply_accelerometer_coefficients(self, accelerometer_data_object):
"""Add accelerometer offset to given accelerometer data."""
x = self._accelerometer_linear_fit(accelerometer_data_object.value[0], *self.__accelerometer_coefficients[0])
y = self._accelerometer_linear_fit(accelerometer_data_object.value[1], *self.__accelerometer_coefficients[1])
z = self._accelerometer_linear_fit(accelerometer_data_object.value[2], *self.__accelerometer_coefficients[2])
return TobiiData.Accelerometer(numpy.array([x, y , z]))
def reset_translation(self, translation_speed = numpy.zeros(3)):
"""Reset translation value before to start integration process."""
self.__last_accelerometer_ts = None
self.__translation_speed = translation_speed
self.__translation = numpy.zeros(3)
def update_translation(self, accelerometer_data_ts, accelerometer_data_object):
"""Integrate timestamped accelerometer values to update translation."""
print('> update_translation: accelerometer_data_ts=', accelerometer_data_ts)
# Convert m/s2 into cm/ms2
current_accelerometer = accelerometer_data_object.value * 1e-4
print('\tcurrent_accelerometer(cm/ms2)=', current_accelerometer)
print('\tcurrent_accelerometer norm=', numpy.linalg.norm(current_accelerometer))
# Init accelerometer integration
if self.__last_accelerometer_ts == None:
self.__last_accelerometer_ts = accelerometer_data_ts
self.__last_accelerometer = current_accelerometer
self.__last_translation_speed = numpy.zeros(3)
# Calculate elapsed time in ms
delta_time = (accelerometer_data_ts - self.__last_accelerometer_ts) / 1e3
print('\tdelta_time=', delta_time)
# Integrate accelerometer
self.__translation_speed = self.__translation_speed + (self.__last_accelerometer * delta_time)
self.__translation = self.__translation + (self.__last_translation_speed * delta_time)
print('\tself.__translation_speed(cm/ms)=', self.__translation_speed)
print('\tself.__translation(cm)=', self.__translation)
# Store current as last
self.__last_accelerometer = current_accelerometer
self.__last_accelerometer_ts = accelerometer_data_ts
self.__last_translation_speed = self.__translation_speed
print('< update_translation')
#else:
# print('no valid head plumb')
def get_translation(self):
"""Return current translation speed and translation values."""
return self.__translation_speed, self.__translation
def rotate_plumb(self, rvec):
"""Rotate imu plumb to remove gravity effect in accelerometer data."""
C, _ = cv.Rodrigues(rvec)
self.__plumb = C.dot(EARTH_GRAVITY_VECTOR)
# Check plumb length
assert(math.isclose(numpy.linalg.norm(self.__plumb), math.fabs(EARTH_GRAVITY), abs_tol=1e-3))
def get_plumb(self):
"""Return plumb vector."""
return self.__plumb
def apply_plumb(self, accelerometer_data_object):
"""Remove gravity along plumb vector to given accelerometer data."""
return TobiiData.Accelerometer(accelerometer_data_object.value - self.__plumb)
|
