def err_func(parameters):
ypr = parameters[0:3]
T_leds = parameters[3:6]
R_leds = freehead.from_yawpitchroll(ypr, in_degrees=in_degrees)
# apply inverse transform
calculated_positions = np.einsum('ij,tj->ti', np.linalg.pinv(R_leds),
probe_tips - T_leds)
position_error = np.sum((calculated_positions - probed_leds)**2)
return position_error
def err_func_nonlinear(ypr_aa_bb_cc):
ypr = ypr_aa_bb_cc[0:3]
R = fh.from_yawpitchroll(ypr)
gaze_rotated = (R @ gaze_normals.T).T
azim_elev = np.rad2deg(
np.arctan2(gaze_rotated[:, [0, 2]], gaze_rotated[:, 1, None]))
aa = ypr_aa_bb_cc[None, 3:5]
bb = ypr_aa_bb_cc[None, 5:7]
cc = ypr_aa_bb_cc[None, 7:9]
azim_elev_nonlinear = aa * (azim_elev**2) + bb * azim_elev + cc
return ((angles_from_eye - azim_elev_nonlinear)**2).sum()
def err_func(ypr):
R_eye_head = fh.from_yawpitchroll(ypr)
T_eye_world = np.einsum('tij,j->ti', R_head_world,
T_eye_head) + T_head_world
eye_to_target = fh.to_unit(T_target_world - T_eye_world)
gaze_normals_world = np.einsum(
'tij,tj->ti', R_head_world,
np.einsum('ij,tj->ti', R_eye_head, gaze_normals))
angles = np.rad2deg(
np.arccos(np.einsum('ti,ti->t', gaze_normals_world,
eye_to_target)))
return np.sum(angles)
def err_func_nonlinear_direct(ypr_aa_bb_cc):
ypr = ypr_aa_bb_cc[0:3]
aa = ypr_aa_bb_cc[None, 3:5]
bb = ypr_aa_bb_cc[None, 5:7]
cc = ypr_aa_bb_cc[None, 7:9]
R = fh.from_yawpitchroll(ypr)
gaze_rotated = (R @ gaze_normals.T).T
gaze_transformed = gaze_rotated.copy()
gaze_transformed[:,
[0, 2]] = aa * (gaze_transformed[:, [0, 2]]**
2) + bb * gaze_transformed[:, [0, 2]] + cc
gaze_transformed = fh.to_unit(gaze_transformed)
azim_elev_nonlinear = np.rad2deg(
np.arctan2(gaze_transformed[:, [0, 2]], gaze_transformed[:, 1, None]))
return ((angles_from_eye - azim_elev_nonlinear)**2).sum()
def err_func(params):
T_eye_head = ini_T_eye_head
R_eye_head = fh.from_yawpitchroll(params[0:3])
aa = params[3:5]
bb = params[5:7]
cc = params[7:9]
T_eye_world = np.einsum('tij,j->ti', R_head_world,
T_eye_head) + T_head_world
eye_to_target = fh.to_unit(T_target_world - T_eye_world)
gaze_normals_head = np.einsum('ij,tj->ti', R_eye_head,
gaze_normals)
gaze_normals_head_transformed = fh.normals_nonlinear_angular_transform(
gaze_normals_head, aa, bb, cc)
gaze_normals_world = np.einsum('tij,tj->ti', R_head_world,
gaze_normals_head_transformed)
angles = np.rad2deg(
np.arccos(
np.einsum('ti,ti->t', gaze_normals_world, eye_to_target)))
return angles.mean()
def calibrate(self):
calibration_point = 127
self.athread.write_uint8(calibration_point, 128, 0, 0)
print('Press space to reset eye calibration.')
fh.wait_for_keypress(pygame.K_SPACE)
self.pthread.reset_3d_eye_model()
self.athread.write_uint8(calibration_point, 0, 0, 128)
time.sleep(self.after_reset_wait)
while True:
self.athread.write_uint8(calibration_point, 128, 0, 0)
print('Calibration pending. Press space to start.')
fh.wait_for_keypress(pygame.K_SPACE)
self.athread.write_uint8(calibration_point, 255, 0, 0)
self.othread.reset_data_buffer()
self.pthread.reset_data_buffer()
print('\nCalibration starting.\n')
start_time = time.monotonic()
while time.monotonic() - start_time < self.calib_duration:
time.sleep(0.1)
self.athread.write_uint8(255, 0, 0, 0)
print('\nCalibration over. Optimizing parameters...\n')
odata = self.othread.get_shortened_data().copy()
pdata = self.pthread.get_shortened_data().copy()
gaze_normals = pdata[:, NORMALS]
f_interpolate = interp1d(odata[:, OTIME],
odata[:, HELMET],
axis=0,
bounds_error=False,
fill_value=np.nan)
odata_interpolated = f_interpolate(pdata[:, PTIME]).reshape(
(-1, 4, 3))
R_head_world, ref_points = self.helmet.solve(odata_interpolated)
T_head_world = ref_points[:, I_BARY, :]
confidence_enough = pdata[:, CONFIDENCE] > 0.3
rotations_valid = ~np.any(np.isnan(R_head_world).reshape((-1, 9)),
axis=1)
chosen_mask = confidence_enough & rotations_valid
T_target_world = np.tile(self.rig_leds[calibration_point, :],
(chosen_mask.sum(), 1))
ini_T_eye_head = self.helmet.ref_points[
I_EYE, :] - self.helmet.ref_points[I_BARY, :]
calibration_result = fh.calibrate_pupil_nonlinear(
T_head_world[chosen_mask, ...],
R_head_world[chosen_mask, ...],
gaze_normals[chosen_mask, ...],
T_target_world,
ini_T_eye_head=ini_T_eye_head,
leave_T_eye_head=True)
print('Optimization done.\n')
print('Error: ', calibration_result.fun, '\n')
print('Parameters: ', calibration_result.x, '\n')
# signal quality of calibration via leds
if calibration_result.fun <= 0.7:
self.athread.write_uint8(127, 0, 255, 0) # green
elif 0.7 < calibration_result.fun <= 1:
self.athread.write_uint8(127, 255, 128, 0) # yellow
else:
self.athread.write_uint8(127, 255, 0, 0) # red
print('Accept calibration? Yes: Space, No: Escape')
key = fh.wait_for_keypress(pygame.K_SPACE, pygame.K_ESCAPE)
if key == pygame.K_SPACE:
self.R_eye_head = fh.from_yawpitchroll(
calibration_result.x[0:3])
self.nonlinear_parameters = calibration_result.x[3:9]
# self.helmet.ref_points[5, :] = self.helmet.ref_points[0, :] + calibration_result.x[9:12]
break
elif key == pygame.K_ESCAPE:
continue
self.athread.write_uint8(255, 0, 0, 0) # leds off
print('Press space to record led with index', led_rig_index)
while True:
fh.wait_for_keypress(pygame.K_SPACE)
current_sample = othread.current_sample.copy()
rotation, tip = probe.solve(extract_probe(current_sample))
if np.any(np.isnan(tip)):
print('Probe not visible, try again.')
continue
probe_tips[i, :] = tip
break
arduino.write(struct.pack('>B', 255))
led_rig_transform_result = fh.get_rig_transform(probe_tips,
led_rig_indices)
R_rig = fh.from_yawpitchroll(led_rig_transform_result.x[0:3])
T_rig = led_rig_transform_result.x[3:6]
rig_led_positions = np.einsum('ij,tj->ti', R_rig, fh.LED_POSITIONS) + T_rig
else:
print(
'rig_led_positions calibration already present. Delete if you want to recalibrate.'
)
head_measurement_points = [
'head straight', 'nasion', 'inion', 'right ear', 'left ear'
]
for i, measurement_point in enumerate(head_measurement_points):
print('Press space to measure: ' + measurement_point)
while True:
def __init__(self, exp_df_path, trial_df_path, rig_leds_path):
exp_df = pd.read_pickle(exp_df_path)
trial_df = pd.read_pickle(trial_df_path)
self.df = exp_df.join(trial_df.drop('block', axis=1),
on='trial_number')
self.rig_leds = np.load(rig_leds_path)
self.precalculate_data()
verts, faces, normals, nothin = vispy.io.read_mesh(
os.path.join(fh.PACKAGE_DIR, '../datafiles', 'head.obj'))
verts = np.einsum('ni,ji->nj', (verts - verts.mean(axis=0)),
fh.from_yawpitchroll(180, 90, 0))
#verts = verts - verts.mean(axis=0)
# add SceneCanvas first to create QApplication object before widget
self.vispy_canvas = vispy.scene.SceneCanvas(create_native=True,
vsync=True,
show=True,
bgcolor=(0.2, 0.2, 0.2, 0))
super(ExperimentVisualization, self).__init__()
#self.setAttribute(Qt.WA_TranslucentBackground)
self.timer = vispy.app.Timer(1 / 30,
start=False,
connect=self.advance_frame)
self.n_trials = len(self.df)
self.current_trial = 0
self.i_frame = 0
self.current_row = self.df.iloc[self.current_trial]
self.current_R_helmet = None
self.current_gaze_normals = None
self.current_ref_points = None
self.vispy_view = self.vispy_canvas.central_widget.add_view()
self.vispy_view.camera = 'turntable'
self.vispy_view.camera.center = self.rig_leds[127, :] + (
self.rig_leds[0, :] - self.rig_leds[127, :]) + (
self.rig_leds[254, :] - self.rig_leds[127, :])
self.vispy_view.camera.fov = 40
self.vispy_view.camera.distance = 1500
self.main_layout = QtWidgets.QVBoxLayout()
self.setLayout(self.main_layout)
self.frame_slider = QtWidgets.QSlider(Qt.Horizontal)
self.frame_slider.setMinimum(0)
self.frame_slider.valueChanged.connect(self.on_slider_change)
self.trial_picker = QtWidgets.QSpinBox()
self.trial_picker.setMaximum(self.n_trials)
self.trial_picker.valueChanged.connect(self.on_picker_change)
self.trial_changed.connect(self.load_trial)
self.frame_changed.connect(self.load_frame)
self.picker_slider_layout = QtWidgets.QHBoxLayout()
self.main_layout.addWidget(self.vispy_canvas.native)
self.main_layout.addLayout(self.picker_slider_layout)
self.animation_button = QtWidgets.QPushButton('Start Animation')
self.picker_slider_layout.addWidget(self.animation_button)
self.animation_button.clicked.connect(self.toggle_animation)
self.frame_label = QtWidgets.QLabel('Frame')
self.picker_slider_layout.addWidget(self.frame_label)
self.picker_slider_layout.addWidget(self.frame_slider)
self.picker_slider_layout.addWidget(QtWidgets.QLabel('Trial'))
self.picker_slider_layout.addWidget(self.trial_picker)
self.rig_vis = visuals.Markers()
self.rig_vis.set_gl_state(depth_test=False)
self.rig_vis.antialias = 0
self.vispy_view.add(self.rig_vis)
self.helmet_vis = visuals.Markers()
self.vispy_view.add(self.helmet_vis)
self.gaze_vis = visuals.Line()
self.vispy_view.add(self.gaze_vis)
self.head_mesh = visuals.Mesh(vertices=verts,
shading='smooth',
faces=faces,
mode='triangles',
color=(0.5, 0.55, 0.7))
self.head_mesh.shininess = 0
self.head_mesh.light_dir = [0, 1, 1]
# self.head_mesh.light_color = np.array((1, 1, 0.95)) * 0.8
self.head_mesh.ambient_light_color = np.array((0.98, 0.98, 1)) * 0.2
self.head_mesh.attach(Alpha(0.3))
self.head_mesh.set_gl_state(depth_test=True, cull_face=True)
self.head_mesh_transform = MatrixTransform()
self.head_mesh.transform = self.head_mesh_transform
self.vispy_view.add(self.head_mesh)
self.trial_changed.emit(0)
self.frame_changed.emit(0)
self.show()
vispy.app.run()
print('Press space to record led with index', led_rig_index)
while True:
fh.wait_for_keypress(pygame.K_SPACE)
current_sample = othread.current_sample.copy()
rotation, tip = probe.solve(extract_probe(current_sample))
if np.any(np.isnan(tip)):
print('Probe not visible, try again.')
continue
probe_tips[i, :] = tip
break
athread.write_uint8(255, 0, 0, 0)
led_rig_transform_result = fh.get_rig_transform(probe_tips,
led_rig_indices)
R_rig = fh.from_yawpitchroll(led_rig_transform_result.x[0:3])
T_rig = led_rig_transform_result.x[3:6]
rig_led_positions = np.einsum('ij,tj->ti', R_rig, fh.LED_POSITIONS) + T_rig
else:
print(
'rig_led_positions calibration already present. Delete if you want to recalibrate.'
)
head_measurement_points = [
'head straight', 'nasion', 'inion', 'right ear', 'left ear', 'right eye'
]
for i, measurement_point in enumerate(head_measurement_points):
print('Press space to measure: ' + measurement_point)
while True:
T_head_world[i, :] = ref_points[0, :]
gaze_normals[i, :] = gaze_normal
athread.write_uint8(255, 0, 0, 0)
#%%
calibration_result = fh.calibrate_pupil(
T_head_world,
R_head_world,
gaze_normals,
T_target_world,
ini_T_eye_head=(helmet.ref_points[5, :] - helmet.ref_points[0, :]) +
15 * fh.to_unit(helmet.ref_points[2, :] - helmet.ref_points[1, :]
), # add 15 mm of nasion to inion to measured eye position
bounds_mm=50)
R_eye_head = fh.from_yawpitchroll(calibration_result.x[0:3])
T_eye_head = calibration_result.x[3:6]
#%%
helmet.ref_points[5, :] = T_eye_head + helmet.ref_points[0, :]
# %%
settings = {
'trials_per_shift': 2,
'shift_magnitudes': np.arange(-5, 6) * 4,
'default_fixation_led_index': 50,
'default_target_led_index': 200,
'max_random_led_offset': 10,
'before_fixation_color': (1, 0, 0),
'during_fixation_color': (0, 1, 0),
def err_func(ypr):
R = fh.from_yawpitchroll(ypr)
gaze_rotated = (R @ gaze_normals.T).T
azim_elev = np.rad2deg(
np.arctan2(gaze_rotated[:, [0, 2]], gaze_rotated[:, 1, None]))
return ((angles_from_eye - azim_elev)**2).sum()
#%%
gaze_normals = np.vstack([p[3:6] for p in pupil_samples])
# find rotation matrix
def err_func(ypr):
R = fh.from_yawpitchroll(ypr)
gaze_rotated = (R @ gaze_normals.T).T
azim_elev = np.rad2deg(
np.arctan2(gaze_rotated[:, [0, 2]], gaze_rotated[:, 1, None]))
return ((angles_from_eye - azim_elev)**2).sum()
optim_result = minimize(err_func, np.zeros(3))
R_eye = fh.from_yawpitchroll(optim_result.x)
gaze_corrected = (R_eye @ gaze_normals.T).T
#%%
azim_elev = np.rad2deg(
np.arctan2(gaze_corrected[:, [0, 2]], gaze_corrected[:, 1, None]))
#%%
def err_func_nonlinear(ypr_aa_bb_cc):
ypr = ypr_aa_bb_cc[0:3]
R = fh.from_yawpitchroll(ypr)
gaze_rotated = (R @ gaze_normals.T).T
azim_elev = np.rad2deg(
