''' (*)~---------------------------------------------------------------------------------- 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()