def test_average_quats(): """Test averaging of quaternions.""" sq2 = 1. / np.sqrt(2.) quats = np.array( [[0, sq2, sq2], [0, sq2, sq2], [0, sq2, 0], [0, 0, sq2], [sq2, 0, 0]], float) # In MATLAB: # quats = [[0, sq2, sq2, 0]; [0, sq2, sq2, 0]; # [0, sq2, 0, sq2]; [0, 0, sq2, sq2]; [sq2, 0, 0, sq2]]; expected = [ quats[0], quats[0], [0, 0.788675134594813, 0.577350269189626], [0, 0.657192299694123, 0.657192299694123], [0.100406058540540, 0.616329446922803, 0.616329446922803] ] # Averaging the first two should give the same thing: for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7) quats[1] *= -1 # same quaternion (hidden value is zero here)! rot_0, rot_1 = quat_to_rot(quats[:2]) assert_allclose(rot_0, rot_1, atol=1e-7) for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7) # Assert some symmetry count = 0 extras = [[sq2, sq2, 0]] + list(np.eye(3)) for quat in np.concatenate((quats, expected, extras)): if np.isclose(_quat_real(quat), 0., atol=1e-7): # can flip sign count += 1 angle = _angle_between_quats(quat, -quat) assert_allclose(angle, 0., atol=1e-7) rot_0, rot_1 = quat_to_rot(np.array((quat, -quat))) assert_allclose(rot_0, rot_1, atol=1e-7) assert count == 4 + len(extras)
def test_quaternions(): """Test quaternion calculations.""" rots = [np.eye(3)] for fname in [test_fif_fname, ctf_fname, hp_fif_fname]: rots += [read_info(fname)['dev_head_t']['trans'][:3, :3]] # nasty numerical cases rots += [ np.array([ [-0.99978541, -0.01873462, -0.00898756], [-0.01873462, 0.62565561, 0.77987608], [-0.00898756, 0.77987608, -0.62587152], ]) ] rots += [ np.array([ [0.62565561, -0.01873462, 0.77987608], [-0.01873462, -0.99978541, -0.00898756], [0.77987608, -0.00898756, -0.62587152], ]) ] rots += [ np.array([ [-0.99978541, -0.00898756, -0.01873462], [-0.00898756, -0.62587152, 0.77987608], [-0.01873462, 0.77987608, 0.62565561], ]) ] for rot in rots: assert_allclose(rot, quat_to_rot(rot_to_quat(rot)), rtol=1e-5, atol=1e-5) rot = rot[np.newaxis, np.newaxis, :, :] assert_allclose(rot, quat_to_rot(rot_to_quat(rot)), rtol=1e-5, atol=1e-5) # let's make sure our angle function works in some reasonable way for ii in range(3): for jj in range(3): a = np.zeros(3) b = np.zeros(3) a[ii] = 1. b[jj] = 1. expected = np.pi if ii != jj else 0. assert_allclose(_angle_between_quats(a, b), expected, atol=1e-5) y_180 = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1.]]) assert_allclose(_angle_between_quats(rot_to_quat(y_180), np.zeros(3)), np.pi) h_180_attitude_90 = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1.]]) assert_allclose( _angle_between_quats(rot_to_quat(h_180_attitude_90), np.zeros(3)), np.pi)
def _rand_affine(rng): quat = rng.randn(3) quat /= 5 * np.linalg.norm(quat) affine = np.eye(4) affine[:3, 3] = rng.randn(3) / 5. affine[:3, :3] = quat_to_rot(quat) return affine
def test_euler(quats): """Test euler transformations.""" euler = _quat_to_euler(quats) quats_2 = _euler_to_quat(euler) assert_allclose(quats, quats_2, atol=1e-14) quat_rot = quat_to_rot(quats) euler_rot = np.array([rotation(*e)[:3, :3] for e in euler]) assert_allclose(quat_rot, euler_rot, atol=1e-14)
def test_fit_matched_points(quats, scaling, do_scale): """Test analytical least-squares matched point fitting.""" if scaling != 1 and not do_scale: return # no need to test this, it will not be good rng = np.random.RandomState(0) fro = rng.randn(10, 3) translation = rng.randn(3) for qi, quat in enumerate(quats): to = scaling * np.dot(quat_to_rot(quat), fro.T).T + translation for corrupted in (False, True): # mess up a point if corrupted: to[0, 2] += 100 weights = np.ones(len(to)) weights[0] = 0 else: weights = None est, scale_est = _check_fit_matched_points( fro, to, weights=weights, do_scale=do_scale) assert_allclose(scale_est, scaling, rtol=1e-5) assert_allclose(est[:3], quat, atol=1e-14) assert_allclose(est[3:], translation, atol=1e-14) # if we don't adjust for the corruption above, it should get worse angle = dist = None for weighted in (False, True): if not weighted: weights = None dist_bounds = (5, 20) if scaling == 1: angle_bounds = (5, 95) angtol, dtol, stol = 1, 15, 3 else: angle_bounds = (5, 105) angtol, dtol, stol = 20, 15, 3 else: weights = np.ones(len(to)) weights[0] = 10 # weighted=True here means "make it worse" angle_bounds = (angle, 180) # unweighted values as new min dist_bounds = (dist, 100) if scaling == 1: # XXX this angtol is not great but there is a hard to # identify linalg/angle calculation bug on Travis... angtol, dtol, stol = 180, 70, 3 else: angtol, dtol, stol = 50, 70, 3 est, scale_est = _check_fit_matched_points( fro, to, weights=weights, do_scale=do_scale, angtol=angtol, dtol=dtol, stol=stol) assert not np.allclose(est[:3], quat, atol=1e-5) assert not np.allclose(est[3:], translation, atol=1e-5) angle = np.rad2deg(_angle_between_quats(est[:3], quat)) assert_array_less(angle_bounds[0], angle) assert_array_less(angle, angle_bounds[1]) dist = np.linalg.norm(est[3:] - translation) assert_array_less(dist_bounds[0], dist) assert_array_less(dist, dist_bounds[1])
def test_quaternions(): """Test quaternion calculations """ rots = [np.eye(3)] for fname in [test_fif_fname, ctf_fname, hp_fif_fname]: rots += [read_info(fname)['dev_head_t']['trans'][:3, :3]] # nasty numerical cases rots += [np.array([ [-0.99978541, -0.01873462, -0.00898756], [-0.01873462, 0.62565561, 0.77987608], [-0.00898756, 0.77987608, -0.62587152], ])] rots += [np.array([ [0.62565561, -0.01873462, 0.77987608], [-0.01873462, -0.99978541, -0.00898756], [0.77987608, -0.00898756, -0.62587152], ])] rots += [np.array([ [-0.99978541, -0.00898756, -0.01873462], [-0.00898756, -0.62587152, 0.77987608], [-0.01873462, 0.77987608, 0.62565561], ])] for rot in rots: assert_allclose(rot, quat_to_rot(rot_to_quat(rot)), rtol=1e-5, atol=1e-5) rot = rot[np.newaxis, np.newaxis, :, :] assert_allclose(rot, quat_to_rot(rot_to_quat(rot)), rtol=1e-5, atol=1e-5) # let's make sure our angle function works in some reasonable way for ii in range(3): for jj in range(3): a = np.zeros(3) b = np.zeros(3) a[ii] = 1. b[jj] = 1. expected = np.pi if ii != jj else 0. assert_allclose(_angle_between_quats(a, b), expected, atol=1e-5)
def test_average_quats(): """Test averaging of quaternions.""" sq2 = 1. / np.sqrt(2.) quats = np.array( [[0, sq2, sq2], [0, sq2, sq2], [0, sq2, 0], [0, 0, sq2], [sq2, 0, 0]], float) # In MATLAB: # quats = [[0, sq2, sq2, 0]; [0, sq2, sq2, 0]; # [0, sq2, 0, sq2]; [0, 0, sq2, sq2]; [sq2, 0, 0, sq2]]; expected = [ quats[0], quats[0], [0, 0.788675134594813, 0.577350269189626], [0, 0.657192299694123, 0.657192299694123], [0.100406058540540, 0.616329446922803, 0.616329446922803] ] # Averaging the first two should give the same thing: for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7) quats[1] *= -1 # same quaternion (hidden value is zero here)! rot_0, rot_1 = quat_to_rot(quats[:2]) assert_allclose(rot_0, rot_1, atol=1e-7) for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7)
def test_get_chpi(): """Test CHPI position computation """ with warnings.catch_warnings(record=True): # deprecation trans0, rot0, _, quat0 = get_chpi_positions(hp_fname, return_quat=True) assert_allclose(rot0[0], quat_to_rot(quat0[0])) trans0, rot0 = trans0[:-1], rot0[:-1] raw = Raw(hp_fif_fname) with warnings.catch_warnings(record=True): # deprecation out = get_chpi_positions(raw) trans1, rot1, t1 = out trans1, rot1 = trans1[2:], rot1[2:] # these will not be exact because they don't use equiv. time points assert_allclose(trans0, trans1, atol=1e-5, rtol=1e-1) assert_allclose(rot0, rot1, atol=1e-6, rtol=1e-1) # run through input checking raw_no_chpi = Raw(test_fif_fname) with warnings.catch_warnings(record=True): # deprecation assert_raises(TypeError, get_chpi_positions, 1) assert_raises(ValueError, get_chpi_positions, hp_fname, [1]) assert_raises(RuntimeError, get_chpi_positions, raw_no_chpi) assert_raises(ValueError, get_chpi_positions, raw, t_step='foo') assert_raises(IOError, get_chpi_positions, 'foo')
def test_average_quats(): """Test averaging of quaternions.""" sq2 = 1. / np.sqrt(2.) quats = np.array([[0, sq2, sq2], [0, sq2, sq2], [0, sq2, 0], [0, 0, sq2], [sq2, 0, 0]], float) # In MATLAB: # quats = [[0, sq2, sq2, 0]; [0, sq2, sq2, 0]; # [0, sq2, 0, sq2]; [0, 0, sq2, sq2]; [sq2, 0, 0, sq2]]; expected = [quats[0], quats[0], [0, 0.788675134594813, 0.577350269189626], [0, 0.657192299694123, 0.657192299694123], [0.100406058540540, 0.616329446922803, 0.616329446922803]] # Averaging the first two should give the same thing: for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7) quats[1] *= -1 # same quaternion (hidden value is zero here)! rot_0, rot_1 = quat_to_rot(quats[:2]) assert_allclose(rot_0, rot_1, atol=1e-7) for lim, ex in enumerate(expected): assert_allclose(_average_quats(quats[:lim + 1]), ex, atol=1e-7)
ind_ok = np.where(g > GOF_LIMIT)[0] times_ok = times_all[ind_ok] """ quats give device -> head transformation: y = Rx + T where y is in head coords and x in device coords. to get pos of head origin in device coords: y=0 -> Rx = -T -> x = -R.T * T rotation of head coord system: y = Rx + T -> x = R.T * y -R.T * T -> rotation given by R.T result agrees with maxfilter (note that maxfilter plots coords of its sss expansion origin, not of the head origin!) """ # rotation quaternions -> rot matrices Rq, _ = raw[picks_chpi_r, :] Rq = Rq[:, ind_ok].T R = quat_to_rot(Rq) # translation vectors T, _ = raw[picks_chpi_t, :] T = T[:, ind_ok].T # head origin (see above) nrot = R.shape[0] Y = np.zeros((nrot, 3)) A = np.zeros((nrot, 3)) for k in np.arange(nrot): Y[k, :] = -np.dot(R[k].T, T[k]) # get rot. angle changes directly from quaternion data #Rq = lowpass(Rq, raw.info['sfreq'], args.lpcorner, axis=0) dA = _angle_between_quats(Rq[1:, :], Rq[:-1, :]) / np.pi * 180
times_ok = times_all[ind_ok] """ quats give device -> head transformation: y = Rx + T where y is in head coords and x in device coords. to get pos of head origin in device coords: y=0 -> Rx = -T -> x = -R.T * T rotation of head coord system: y = Rx + T -> x = R.T * y -R.T * T -> rotation given by R.T result agrees with maxfilter (note that maxfilter plots coords of its sss expansion origin, not of the head origin!) """ # rotation quaternions -> rot matrices Rq, _ = raw[picks_chpi_r, :] Rq = Rq[:, ind_ok].T R = quat_to_rot(Rq) # translation vectors T, _ = raw[picks_chpi_t, :] T = T[:, ind_ok].T # head origin (see above) nrot = R.shape[0] Y = np.zeros((nrot, 3)) A = np.zeros((nrot, 3)) for k in np.arange(nrot): Y[k, :] = -np.dot(R[k].T, T[k]) # get rot. angle changes directly from quaternion data #Rq = lowpass(Rq, raw.info['sfreq'], args.lpcorner, axis=0) dA = _angle_between_quats(Rq[1:, :], Rq[:-1, :]) / np.pi * 180
def contAvg_headpos(condition, method='median', folder=[], summary=False): """ Calculate average transformation from dewar to head coordinates, based on the continous head position estimated from MaxFilter Parameters ---------- condition : str String containing part of common filename, e.g. "task" for files task-1.fif, task-2.fif, etc. Consistent naiming of files is mandatory! method : str How to calculate "average, "mean" or "median" (default = "median") folder : str Path to input files. Default = current dir. Returns ------- MNE-Python transform object 4x4 transformation matrix """ # Check that the method works method = method.lower() if method not in ['median', 'mean']: raise RuntimeError( 'Wrong method. Must be either \"mean\" or "median"!') if not condition: raise RuntimeError('You must provide a conditon!') # Get and set folders if not folder: rawdir = getcwd() # [!] Match up with bash script ! else: rawdir = folder print(rawdir) quatdir = op.join(rawdir, 'quat_files') mean_trans_folder = op.join(rawdir, 'trans_files') if not op.exists(mean_trans_folder): # Make sure output folder exists mkdir(mean_trans_folder) mean_trans_file = op.join(mean_trans_folder, condition + '-trans.fif') if op.isfile(mean_trans_file): warnings.warn( 'N"%s\" already exists is %s. Delete if you want to rerun' % (mean_trans_file, mean_trans_folder), RuntimeWarning) return # Change to subject dir files2combine = find_condition_files(quatdir, condition) files2combine.sort() if not files2combine: raise RuntimeError('No files called \"%s\" found in %s' % (condition, quatdir)) allfiles = [] for ff in files2combine: fl = ff.split('_')[0] tmplist = [f for f in listdir(quatdir) if fl in f and '_quat' in f] #Fix order if len(tmplist) > 1: tmplist.sort() if any("-" in f for f in tmplist): firstfile = tmplist[ -1] # The file without a number will always be last! tmpfs = sorted(tmplist[:-1], key=lambda a: int(re.split('-|.fif', a)[-2]) ) # Assuming consistent naming!!! tmplist[0] = firstfile tmplist[1:] = tmpfs allfiles = allfiles + tmplist if len(allfiles) > 1: print('Files used for average head pos:') for ib in range(len(allfiles)): print('{:d}: {:s}'.format(ib + 1, allfiles[ib])) else: print('Will find average head pos in %s' % files2combine) # LOAD DATA # raw = read_raw_fif(op.join(quatdir,firstfile), preload=True, allow_maxshield=True, verbose=False).pick_types(meg=False, chpi=True) # Use files2combine instead of allfiles as MNE will find split files automatically. for idx, ffs in enumerate(files2combine): if idx == 0: raw = read_raw_fif(op.join(quatdir, ffs), preload=True, allow_maxshield=True).pick_types(meg=False, chpi=True) else: raw.append( read_raw_fif(op.join(quatdir, ffs), preload=True, allow_maxshield=True).pick_types(meg=False, chpi=True)) quat, times = raw.get_data(return_times=True) gof = quat[6, ] # Godness of fit channel # fs = raw.info['sfreq'] # In case "record raw" started before "cHPI" if np.any(gof < 0.98): begsam = np.argmax(gof > 0.98) raw.crop(tmin=raw.times[begsam]) quat = quat[:, begsam:].copy() times = times[begsam:].copy() # Make summaries if summary: plot_movement(quat, times, dirname=rawdir, identifier=condition) total_dist_moved(quat, times, write=True, dirname=rawdir, identifier=condition) # Get continous transformation print('Reading transformation. This will take a while...') H = np.empty([4, 4, len(times)]) # Initiate transforms init_rot_angles = np.empty([len(times), 3]) for i, t in enumerate(times): Hi = np.eye(4, 4) Hi[0:3, 3] = quat[3:6, i].copy() Hi[:3, :3] = quat_to_rot(quat[0:3, i]) init_rot_angles[i, :] = rotation_angles(Hi[:3, :3]) assert (np.sum(Hi[-1]) == 1.0) # sanity check result H[:, :, i] = Hi.copy() if method in ["mean"]: H_mean = np.mean(H, axis=2) # stack, then average over new dim mean_rot_xfm = rotation3d(*tuple( np.mean(init_rot_angles, axis=0))) # stack, then average, then make new xfm elif method in ["median"]: H_mean = np.median(H, axis=2) # stack, then average over new dim mean_rot_xfm = rotation3d(*tuple( np.median(init_rot_angles, axis=0))) # stack, then average, then make new xfm H_mean[:3, :3] = mean_rot_xfm assert (np.sum(H_mean[-1]) == 1.0) # sanity check result # Create the mean structure and save as .fif mean_trans = raw.info['dev_head_t'] # use the last info as a template mean_trans['trans'] = H_mean.copy() # Write file write_trans(mean_trans_file, mean_trans) print("Wrote " + mean_trans_file) return mean_trans
def contAvg_headpos(condition, method='median', folder=[]): """ Calculate average transformation from dewar to head coordinates, based on the continous head position estimated from MaxFilter Parameters ---------- condition : str String containing part of common filename, e.g. "task" for files task-1.fif, task-2.fif, etc. Consistent naiming of files is mandatory! method : str How to calculate "average, "mean" or "median" (default = "median") folder : str Path to input files. Default = current dir. Returns ------- MNE-Python transform object 4x4 transformation matrix """ # Check that the method works if method not in ['median', 'mean']: raise RuntimeError( 'Wrong method. Must be either \"mean\" or "median"!') if not condition: raise RuntimeError('You must provide a conditon!') # Get and set folders if not folder: rawdir = getcwd() # [!] Match up with bash script ! else: rawdir = folder print(rawdir) quatdir = op.join(rawdir, 'quat_files') mean_trans_folder = op.join(rawdir, 'trans_files') if not op.exists(mean_trans_folder): # Make sure output folder exists mkdir(mean_trans_folder) mean_trans_file = op.join(mean_trans_folder, condition + '-trans.fif') if op.isfile(mean_trans_file): raise RuntimeError( 'N"%s\" already exists is %s. Delete aif you want to rerun' % (mean_trans_file, mean_trans_folder)) # Change to subject dir # files2combine = glob.glob('%s*' % condition) files2combine = [ f for f in listdir(quatdir) if condition in f and '_quat' in f ] if not files2combine: raise RuntimeError('No files called \"%s\" found in %s' % (condition, quatdir)) elif len(files2combine) > 1: print('Files used for average head pos:') for ib in range(len(files2combine)): print('{:d}: {:s}'.format(ib + 1, files2combine[ib])) else: print('Will find average head pos in %s' % files2combine) # LOAD DATA for idx, ffs in enumerate(files2combine): # print op.join(quatdir,ffs) if idx == 0: raw = read_raw_fif(op.join(quatdir, ffs), preload=True, allow_maxshield=True).pick_types(meg=False, chpi=True) else: raw.append( read_raw_fif(op.join(quatdir, ffs), preload=True, allow_maxshield=True).pick_types(meg=False, chpi=True)) quat, times = raw.get_data(return_times=True) gof = quat[6, ] # Godness of fit channel fs = raw.info['sfreq'] # In case "record raw" started before "cHPI" if np.any(gof < 0.98): begsam = np.argmax(gof > 0.98) raw.crop(tmin=raw.times[begsam]) quat = quat[:, begsam:].copy() times = times[begsam:].copy() # Get continous transformation print('Reading transformation. This will take a while...') H = np.empty([4, 4, len(times)]) # Initiate transforms init_rot_angles = np.empty([len(times), 3]) for i, t in enumerate(times): Hi = np.eye(4, 4) Hi[0:3, 3] = quat[3:6, i].copy() Hi[:3, :3] = quat_to_rot(quat[0:3, i]) init_rot_angles[i, :] = rotation_angles(Hi[:3, :3]) assert (np.sum(Hi[-1]) == 1.0) # sanity check result H[:, :, i] = Hi.copy() if method in ["mean"]: H_mean = np.mean(H, axis=2) # stack, then average over new dim mean_rot_xfm = rotation3d(*tuple( np.mean(init_rot_angles, axis=0))) # stack, then average, then make new xfm elif method in ["median"]: H_mean = np.median(H, axis=2) # stack, then average over new dim mean_rot_xfm = rotation3d(*tuple( np.median(init_rot_angles, axis=0))) # stack, then average, then make new xfm H_mean[:3, :3] = mean_rot_xfm assert (np.sum(H_mean[-1]) == 1.0) # sanity check result # Create the mean structure and save as .fif mean_trans = raw.info['dev_head_t'] # use the last info as a template mean_trans['trans'] = H_mean.copy() # plot_alignment(raw.info,subject='0406',subjects_dir='/home/mikkel/PD_motor/fs_subjects_dir/',dig=True, meg='helmet') # Write file write_trans(mean_trans_file, mean_trans) print("Wrote " + mean_trans_file) return mean_trans