gazesim/code/pupil/calibrate.py

256 lines
9.5 KiB
Python
Raw Normal View History

2016-03-09 19:52:35 +01:00
'''
(*)~----------------------------------------------------------------------------------
Pupil - eye tracking platform
Copyright (C) 2012-2015 Pupil Labs
Distributed under the terms of the CC BY-NC-SA License.
License details are in the file license.txt, distributed as part of this software.
----------------------------------------------------------------------------------~(*)
'''
import numpy as np
#logging
import logging
logger = logging.getLogger(__name__)
def get_map_from_cloud(cal_pt_cloud,screen_size=(2,2),threshold = 35,return_inlier_map=False,return_params=False):
"""
we do a simple two pass fitting to a pair of bi-variate polynomials
return the function to map vector
"""
# fit once using all avaiable data
model_n = 7
cx,cy,err_x,err_y = fit_poly_surface(cal_pt_cloud,model_n)
err_dist,err_mean,err_rms = fit_error_screen(err_x,err_y,screen_size)
if cal_pt_cloud[err_dist<=threshold].shape[0]: #did not disregard all points..
# fit again disregarding extreme outliers
cx,cy,new_err_x,new_err_y = fit_poly_surface(cal_pt_cloud[err_dist<=threshold],model_n)
map_fn = make_map_function(cx,cy,model_n)
new_err_dist,new_err_mean,new_err_rms = fit_error_screen(new_err_x,new_err_y,screen_size)
logger.info('first iteration. root-mean-square residuals: %s, in pixel' %err_rms)
logger.info('second iteration: ignoring outliers. root-mean-square residuals: %s in pixel',new_err_rms)
logger.info('used %i data points out of the full dataset %i: subset is %i percent' \
%(cal_pt_cloud[err_dist<=threshold].shape[0], cal_pt_cloud.shape[0], \
100*float(cal_pt_cloud[err_dist<=threshold].shape[0])/cal_pt_cloud.shape[0]))
if return_inlier_map and return_params:
return map_fn,err_dist<=threshold,(cx,cy,model_n)
if return_inlier_map and not return_params:
return map_fn,err_dist<=threshold
if return_params and not return_inlier_map:
return map_fn,(cx,cy,model_n)
return map_fn
else: # did disregard all points. The data cannot be represented by the model in a meaningful way:
map_fn = make_map_function(cx,cy,model_n)
logger.info('First iteration. root-mean-square residuals: %s in pixel, this is bad!'%err_rms)
logger.warning('The data cannot be represented by the model in a meaningfull way.')
if return_inlier_map and return_params:
return map_fn,err_dist<=threshold,(cx,cy,model_n)
if return_inlier_map and not return_params:
return map_fn,err_dist<=threshold
if return_params and not return_inlier_map:
return map_fn,(cx,cy,model_n)
return map_fn
def fit_poly_surface(cal_pt_cloud,n=7):
M = make_model(cal_pt_cloud,n)
U,w,Vt = np.linalg.svd(M[:,:n],full_matrices=0)
V = Vt.transpose()
Ut = U.transpose()
pseudINV = np.dot(V, np.dot(np.diag(1/w), Ut))
cx = np.dot(pseudINV, M[:,n])
cy = np.dot(pseudINV, M[:,n+1])
# compute model error in world screen units if screen_res specified
err_x=(np.dot(M[:,:n],cx)-M[:,n])
err_y=(np.dot(M[:,:n],cy)-M[:,n+1])
return cx,cy,err_x,err_y
def fit_error_screen(err_x,err_y,(screen_x,screen_y)):
err_x *= screen_x/2.
err_y *= screen_y/2.
err_dist=np.sqrt(err_x*err_x + err_y*err_y)
err_mean=np.sum(err_dist)/len(err_dist)
err_rms=np.sqrt(np.sum(err_dist*err_dist)/len(err_dist))
return err_dist,err_mean,err_rms
def make_model(cal_pt_cloud,n=7):
n_points = cal_pt_cloud.shape[0]
if n==3:
X=cal_pt_cloud[:,0]
Y=cal_pt_cloud[:,1]
Ones=np.ones(n_points)
ZX=cal_pt_cloud[:,2]
ZY=cal_pt_cloud[:,3]
M=np.array([X,Y,Ones,ZX,ZY]).transpose()
elif n==7:
X=cal_pt_cloud[:,0]
Y=cal_pt_cloud[:,1]
XX=X*X
YY=Y*Y
XY=X*Y
XXYY=XX*YY
Ones=np.ones(n_points)
ZX=cal_pt_cloud[:,2]
ZY=cal_pt_cloud[:,3]
M=np.array([X,Y,XX,YY,XY,XXYY,Ones,ZX,ZY]).transpose()
elif n==9:
X=cal_pt_cloud[:,0]
Y=cal_pt_cloud[:,1]
XX=X*X
YY=Y*Y
XY=X*Y
XXYY=XX*YY
XXY=XX*Y
YYX=YY*X
Ones=np.ones(n_points)
ZX=cal_pt_cloud[:,2]
ZY=cal_pt_cloud[:,3]
M=np.array([X,Y,XX,YY,XY,XXYY,XXY,YYX,Ones,ZX,ZY]).transpose()
else:
raise Exception("ERROR: Model n needs to be 3, 7 or 9")
return M
def make_map_function(cx,cy,n):
if n==3:
def fn((X,Y)):
x2 = cx[0]*X + cx[1]*Y +cx[2]
y2 = cy[0]*X + cy[1]*Y +cy[2]
return x2,y2
elif n==7:
def fn((X,Y)):
x2 = cx[0]*X + cx[1]*Y + cx[2]*X*X + cx[3]*Y*Y + cx[4]*X*Y + cx[5]*Y*Y*X*X +cx[6]
y2 = cy[0]*X + cy[1]*Y + cy[2]*X*X + cy[3]*Y*Y + cy[4]*X*Y + cy[5]*Y*Y*X*X +cy[6]
return x2,y2
elif n==9:
def fn((X,Y)):
# X Y XX YY XY XXYY XXY YYX Ones
x2 = cx[0]*X + cx[1]*Y + cx[2]*X*X + cx[3]*Y*Y + cx[4]*X*Y + cx[5]*Y*Y*X*X + cx[6]*Y*X*X + cx[7]*Y*Y*X + cx[8]
y2 = cy[0]*X + cy[1]*Y + cy[2]*X*X + cy[3]*Y*Y + cy[4]*X*Y + cy[5]*Y*Y*X*X + cy[6]*Y*X*X + cy[7]*Y*Y*X + cy[8]
return x2,y2
else:
raise Exception("ERROR: Model n needs to be 3, 7 or 9")
return fn
def preprocess_data(pupil_pts,ref_pts):
'''small utility function to deal with timestamped but uncorrelated data
input must be lists that contain dicts with at least "timestamp" and "norm_pos"
'''
cal_data = []
if len(ref_pts)<=2:
return cal_data
cur_ref_pt = ref_pts.pop(0)
next_ref_pt = ref_pts.pop(0)
while True:
matched = []
while pupil_pts:
#select all points past the half-way point between current and next ref data sample
if pupil_pts[0]['timestamp'] <=(cur_ref_pt['timestamp']+next_ref_pt['timestamp'])/2.:
matched.append(pupil_pts.pop(0))
else:
for p_pt in matched:
#only use close points
if abs(p_pt['timestamp']-cur_ref_pt['timestamp']) <= 1/15.: #assuming 30fps + slack
data_pt = p_pt["norm_pos"][0], p_pt["norm_pos"][1],cur_ref_pt['norm_pos'][0],cur_ref_pt['norm_pos'][1]
cal_data.append(data_pt)
break
if ref_pts:
cur_ref_pt = next_ref_pt
next_ref_pt = ref_pts.pop(0)
else:
break
return cal_data
# if __name__ == '__main__':
# import matplotlib.pyplot as plt
# from matplotlib import cm
# from mpl_toolkits.mplot3d import Axes3D
# cal_pt_cloud = np.load('cal_pt_cloud.npy')
# # plot input data
# # Z = cal_pt_cloud
# # ax.scatter(Z[:,0],Z[:,1],Z[:,2], c= "r")
# # ax.scatter(Z[:,0],Z[:,1],Z[:,3], c= "b")
# # fit once
# model_n = 7
# cx,cy,err_x,err_y = fit_poly_surface(cal_pt_cloud,model_n)
# map_fn = make_map_function(cx,cy,model_n)
# err_dist,err_mean,err_rms = fit_error_screen(err_x,err_y,(1280,720))
# print err_rms,"in pixel"
# threshold =15 # err_rms*2
# # fit again disregarding crass outlines
# cx,cy,new_err_x,new_err_y = fit_poly_surface(cal_pt_cloud[err_dist<=threshold],model_n)
# map_fn = make_map_function(cx,cy,model_n)
# new_err_dist,new_err_mean,new_err_rms = fit_error_screen(new_err_x,new_err_y,(1280,720))
# print new_err_rms,"in pixel"
# print "using %i datapoints out of the full dataset %i: subset is %i percent" \
# %(cal_pt_cloud[err_dist<=threshold].shape[0], cal_pt_cloud.shape[0], \
# 100*float(cal_pt_cloud[err_dist<=threshold].shape[0])/cal_pt_cloud.shape[0])
# # plot residuals
# fig_error = plt.figure()
# plt.scatter(err_x,err_y,c="y")
# plt.scatter(new_err_x,new_err_y)
# plt.title("fitting residuals full data set (y) and better subset (b)")
# # plot projection of eye and world vs observed data
# X,Y,ZX,ZY = cal_pt_cloud.transpose().copy()
# X,Y = map_fn((X,Y))
# X *= 1280/2.
# Y *= 720/2.
# ZX *= 1280/2.
# ZY *= 720/2.
# fig_projection = plt.figure()
# plt.scatter(X,Y)
# plt.scatter(ZX,ZY,c='y')
# plt.title("world space projection in pixes, mapped and observed (y)")
# # plot the fitting functions 3D plot
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# outliers =cal_pt_cloud[err_dist>threshold]
# inliers = cal_pt_cloud[err_dist<=threshold]
# ax.scatter(outliers[:,0],outliers[:,1],outliers[:,2], c= "y")
# ax.scatter(outliers[:,0],outliers[:,1],outliers[:,3], c= "y")
# ax.scatter(inliers[:,0],inliers[:,1],inliers[:,2], c= "r")
# ax.scatter(inliers[:,0],inliers[:,1],inliers[:,3], c= "b")
# Z = cal_pt_cloud
# X = np.linspace(min(Z[:,0])-.2,max(Z[:,0])+.2,num=30,endpoint=True)
# Y = np.linspace(min(Z[:,1])-.2,max(Z[:,1]+.2),num=30,endpoint=True)
# X, Y = np.meshgrid(X,Y)
# ZX,ZY = map_fn((X,Y))
# ax.plot_surface(X, Y, ZX, rstride=1, cstride=1, linewidth=.1, antialiased=True,alpha=0.4,color='r')
# ax.plot_surface(X, Y, ZY, rstride=1, cstride=1, linewidth=.1, antialiased=True,alpha=0.4,color='b')
# plt.xlabel("Pupil x in Eye-Space")
# plt.ylabel("Pupil y Eye-Space")
# plt.title("Z: Gaze x (blue) Gaze y (red) World-Space, yellow=outliers")
# # X,Y,_,_ = cal_pt_cloud.transpose()
# # pts= map_fn((X,Y))
# # import cv2
# # pts = np.array(pts,dtype=np.float32).transpose()
# # print cv2.convexHull(pts)[:,0]
# plt.show()