<s> import numpy as np <EOL> from scipy import linalg , fftpack <EOL> from . io . pick import pick_types , pick_channels <EOL> from . io . base import _BaseRaw <EOL> from . io . constants import FIFF <EOL> from . forward import ( _magnetic_dipole_field_vec , _create_meg_coils , <EOL> _concatenate_coils , _read_coil_defs ) <EOL> from . cov import make_ad_hoc_cov , _get_whitener_data <EOL> from . transforms import ( apply_trans , invert_transform , _angle_between_quats , <EOL> quat_to_rot , rot_to_quat ) <EOL> from . utils import ( verbose , logger , check_version , use_log_level , deprecated , <EOL> _check_fname , warn ) <EOL> from . fixes import partial <EOL> from . externals . six import string_types <EOL> @ deprecated ( '<STR_LIT>' <EOL> '<STR_LIT>' <EOL> '<STR_LIT>' ) <EOL> @ verbose <EOL> def get_chpi_positions ( raw , t_step = None , return_quat = False , verbose = None ) : <EOL> """<STR_LIT>""" <EOL> if isinstance ( raw , _BaseRaw ) : <EOL> if t_step is None : <EOL> t_step = <NUM_LIT:1.0> <EOL> t_step = float ( t_step ) <EOL> picks = pick_types ( raw . info , meg = False , ref_meg = False , <EOL> chpi = True , exclude = [ ] ) <EOL> if len ( picks ) == <NUM_LIT:0> : <EOL> raise RuntimeError ( '<STR_LIT>' ) <EOL> time_idx = raw . time_as_index ( np . arange ( <NUM_LIT:0> , raw . times [ - <NUM_LIT:1> ] , t_step ) ) <EOL> data = [ raw [ picks , ti ] for ti in time_idx ] <EOL> t = np . array ( [ d [ <NUM_LIT:1> ] for d in data ] ) <EOL> data = np . array ( [ d [ <NUM_LIT:0> ] [ : , <NUM_LIT:0> ] for d in data ] ) <EOL> data = np . c_ [ t , data ] <EOL> else : <EOL> if not isinstance ( raw , string_types ) : <EOL> raise TypeError ( '<STR_LIT>' ) <EOL> if t_step is not None : <EOL> raise ValueError ( '<STR_LIT>' ) <EOL> data = read_head_pos ( raw ) <EOL> out = head_pos_to_trans_rot_t ( data ) <EOL> if return_quat : <EOL> out = out + ( data [ : , <NUM_LIT:1> : <NUM_LIT:4> ] , ) <EOL> return out <EOL> def read_head_pos ( fname ) : <EOL> """<STR_LIT>""" <EOL> _check_fname ( fname , must_exist = True , overwrite = True ) <EOL> data = np . loadtxt ( fname , skiprows = <NUM_LIT:1> ) <EOL> data . shape = ( - <NUM_LIT:1> , <NUM_LIT:10> ) <EOL> return data <EOL> def write_head_pos ( fname , pos ) : <EOL> """<STR_LIT>""" <EOL> _check_fname ( fname , overwrite = True ) <EOL> pos = np . array ( pos , np . float64 ) <EOL> if pos . ndim != <NUM_LIT:2> or pos . shape [ <NUM_LIT:1> ] != <NUM_LIT:10> : <EOL> raise ValueError ( '<STR_LIT>' ) <EOL> with open ( fname , '<STR_LIT:wb>' ) as fid : <EOL> fid . write ( '<STR_LIT>' <EOL> '<STR_LIT>' . encode ( '<STR_LIT>' ) ) <EOL> for p in pos : <EOL> fmts = [ '<STR_LIT>' ] + [ '<STR_LIT>' ] * <NUM_LIT:9> <EOL> fid . write ( ( ( '<STR_LIT:U+0020>' + '<STR_LIT:U+0020>' . join ( fmts ) + '<STR_LIT:\n>' ) <EOL> % tuple ( p ) ) . encode ( '<STR_LIT>' ) ) <EOL> def head_pos_to_trans_rot_t ( quats ) : <EOL> """<STR_LIT>""" <EOL> t = quats [ ... , <NUM_LIT:0> ] . copy ( ) <EOL> rotation = quat_to_rot ( quats [ ... , <NUM_LIT:1> : <NUM_LIT:4> ] ) <EOL> translation = quats [ ... , <NUM_LIT:4> : <NUM_LIT:7> ] . copy ( ) <EOL> return translation , rotation , t <EOL> def _get_hpi_info ( info ) : <EOL> """<STR_LIT>""" <EOL> if len ( info [ '<STR_LIT>' ] ) == <NUM_LIT:0> or ( '<STR_LIT>' not in info [ '<STR_LIT>' ] [ <NUM_LIT:0> ] [ '<STR_LIT>' ] [ <NUM_LIT:0> ] ) : <EOL> raise RuntimeError ( '<STR_LIT>' <EOL> '<STR_LIT>' ) <EOL> hpi_result = info [ '<STR_LIT>' ] [ - <NUM_LIT:1> ] <EOL> hpi_coils = sorted ( info [ '<STR_LIT>' ] [ - <NUM_LIT:1> ] [ '<STR_LIT>' ] , <EOL> key = lambda x : x [ '<STR_LIT>' ] ) <EOL> hpi_dig = sorted ( [ d for d in info [ '<STR_LIT>' ] <EOL> if d [ '<STR_LIT>' ] == FIFF . FIFFV_POINT_HPI ] , <EOL> key = lambda x : x [ '<STR_LIT>' ] ) <EOL> pos_order = hpi_result [ '<STR_LIT>' ] - <NUM_LIT:1> <EOL> if not all ( d [ '<STR_LIT>' ] == FIFF . FIFFV_COORD_HEAD for d in hpi_dig ) : <EOL> raise RuntimeError ( '<STR_LIT>' ) <EOL> logger . info ( '<STR_LIT>' <EOL> % ( len ( pos_order ) , '<STR_LIT:U+0020>' . join ( str ( o + <NUM_LIT:1> ) for o in pos_order ) ) ) <EOL> logger . debug ( '<STR_LIT>' <EOL> % ( len ( hpi_result [ '<STR_LIT>' ] ) , <EOL> '<STR_LIT:U+0020>' . join ( str ( h ) for h in hpi_result [ '<STR_LIT>' ] ) ) ) <EOL> hpi_rrs = np . array ( [ d [ '<STR_LIT:r>' ] for d in hpi_dig ] ) [ pos_order ] <EOL> hpi_freqs = np . array ( [ float ( x [ '<STR_LIT>' ] ) for x in hpi_coils ] ) <EOL> hpi_sub = info [ '<STR_LIT>' ] <EOL> if '<STR_LIT>' in hpi_sub : <EOL> hpi_pick = pick_channels ( info [ '<STR_LIT>' ] , <EOL> [ hpi_sub [ '<STR_LIT>' ] ] ) [ <NUM_LIT:0> ] <EOL> else : <EOL> hpi_pick = None <EOL> hpi_on = [ coil [ '<STR_LIT>' ] [ <NUM_LIT:0> ] for coil in hpi_sub [ '<STR_LIT>' ] ] <EOL> hpi_on = np . array ( [ hpi_on [ hc [ '<STR_LIT>' ] - <NUM_LIT:1> ] for hc in hpi_coils ] ) <EOL> assert len ( hpi_coils ) == len ( hpi_on ) <EOL> logger . info ( '<STR_LIT>' <EOL> % ( len ( hpi_freqs ) , '<STR_LIT:U+0020>' . join ( str ( int ( s ) ) for s in hpi_freqs ) ) ) <EOL> return hpi_freqs , hpi_rrs , hpi_pick , hpi_on , pos_order <EOL> def _magnetic_dipole_objective ( x , B , B2 , coils , scale , method ) : <EOL> """<STR_LIT>""" <EOL> if method == '<STR_LIT>' : <EOL> fwd = _magnetic_dipole_field_vec ( x [ np . newaxis , : ] , coils ) <EOL> else : <EOL> from . preprocessing . maxwell import _sss_basis <EOL> fwd = _sss_basis ( dict ( origin = x , int_order = <NUM_LIT:1> , ext_order = <NUM_LIT:0> ) , coils ) . T <EOL> fwd = np . dot ( fwd , scale . T ) <EOL> one = np . dot ( linalg . svd ( fwd , full_matrices = False ) [ <NUM_LIT:2> ] [ : <NUM_LIT:3> ] , B ) <EOL> one *= one <EOL> Bm2 = one . sum ( ) <EOL> return B2 - Bm2 <EOL> def _fit_magnetic_dipole ( B_orig , x0 , coils , scale , method ) : <EOL> """<STR_LIT>""" <EOL> from scipy . optimize import fmin_cobyla <EOL> B = np . dot ( scale , B_orig ) <EOL> B2 = np . dot ( B , B ) <EOL> objective = partial ( _magnetic_dipole_objective , B = B , B2 = B2 , <EOL> coils = coils , scale = scale , method = method ) <EOL> x = fmin_cobyla ( objective , x0 , ( ) , rhobeg = <NUM_LIT> , rhoend = <NUM_LIT> , disp = False ) <EOL> return x , <NUM_LIT:1.> - objective ( x ) / B2 <EOL> def _chpi_objective ( x , coil_dev_rrs , coil_head_rrs ) : <EOL> """<STR_LIT>""" <EOL> d = np . dot ( coil_dev_rrs , quat_to_rot ( x [ : <NUM_LIT:3> ] ) . T ) <EOL> d += x [ <NUM_LIT:3> : ] <EOL> d -= coil_head_rrs <EOL> d *= d <EOL> return d . sum ( ) <EOL> def _unit_quat_constraint ( x ) : <EOL> """<STR_LIT>""" <EOL> return <NUM_LIT:1> - ( x * x ) . sum ( ) <EOL> def _fit_chpi_pos ( coil_dev_rrs , coil_head_rrs , x0 ) : <EOL> """<STR_LIT>""" <EOL> from scipy . optimize import fmin_cobyla <EOL> denom = np . sum ( ( coil_head_rrs - np . mean ( coil_head_rrs , axis = <NUM_LIT:0> ) ) ** <NUM_LIT:2> ) <EOL> objective = partial ( _chpi_objective , coil_dev_rrs = coil_dev_rrs , <EOL> coil_head_rrs = coil_head_rrs ) <EOL> x = fmin_cobyla ( objective , x0 , _unit_quat_constraint , <EOL> rhobeg = <NUM_LIT> , rhoend = <NUM_LIT> , disp = False ) <EOL> return x , <NUM_LIT:1.> - objective ( x ) / denom <EOL> @ verbose <EOL> def _setup_chpi_fits ( info , t_window , t_step_min , method = '<STR_LIT>' , <EOL> exclude = '<STR_LIT>' , add_hpi_stim_pick = True , <EOL> remove_aliased = False , verbose = None ) : <EOL> """<STR_LIT>""" <EOL> from scipy . spatial . distance import cdist <EOL> from . preprocessing . maxwell import _prep_mf_coils <EOL> if not ( check_version ( '<STR_LIT>' , '<STR_LIT>' ) and check_version ( '<STR_LIT>' , '<STR_LIT>' ) ) : <EOL> raise RuntimeError ( '<STR_LIT>' ) <EOL> hpi_freqs , coil_head_rrs , hpi_pick , hpi_ons = _get_hpi_info ( info ) [ : <NUM_LIT:4> ] <EOL> highest = info . get ( '<STR_LIT>' ) <EOL> highest = info [ '<STR_LIT>' ] / <NUM_LIT> if highest is None else highest <EOL> keepers = np . array ( [ h <= highest for h in hpi_freqs ] , bool ) <EOL> if remove_aliased : <EOL> hpi_freqs = hpi_freqs [ keepers ] <EOL> coil_head_rrs = coil_head_rrs [ keepers ] <EOL> hpi_ons = hpi_ons [ keepers ] <EOL> elif not keepers . all ( ) : <EOL> raise RuntimeError ( '<STR_LIT>' <EOL> '<STR_LIT>' <EOL> % ( hpi_freqs [ ~ keepers ] . tolist ( ) , highest ) ) <EOL> line_freqs = np . arange ( info [ '<STR_LIT>' ] , info [ '<STR_LIT>' ] / <NUM_LIT> , <EOL> info [ '<STR_LIT>' ] ) <EOL> logger . info ( '<STR_LIT>' <EOL> % '<STR_LIT:U+0020>' . join ( [ '<STR_LIT>' % l for l in line_freqs ] ) ) <EOL> dev_head_t = info [ '<STR_LIT>' ] [ '<STR_LIT>' ] <EOL> head_dev_t = invert_transform ( info [ '<STR_LIT>' ] ) [ '<STR_LIT>' ] <EOL> n_window = int ( round ( t_window * info [ '<STR_LIT>' ] ) ) <EOL> logger . debug ( '<STR_LIT>' ) <EOL> for d in ( dev_head_t [ <NUM_LIT:0> , : <NUM_LIT:3> ] , dev_head_t [ <NUM_LIT:1> , : <NUM_LIT:3> ] , dev_head_t [ <NUM_LIT:2> , : <NUM_LIT:3> ] , <EOL> dev_head_t [ : <NUM_LIT:3> , <NUM_LIT:3> ] * <NUM_LIT> ) : <EOL> logger . debug ( '<STR_LIT>' . format ( * d ) ) <EOL> slope = np . arange ( n_window ) . astype ( np . float64 ) [ : , np . newaxis ] <EOL> slope -= np . mean ( slope ) <EOL> rads = slope / info [ '<STR_LIT>' ] <EOL> rads *= <NUM_LIT:2> * np . pi <EOL> f_t = hpi_freqs [ np . newaxis , : ] * rads <EOL> l_t = line_freqs [ np . newaxis , : ] * rads <EOL> model = [ np . sin ( f_t ) , np . cos ( f_t ) ] <EOL> model += [ np . sin ( l_t ) , np . cos ( l_t ) ] <EOL> model += [ slope , np . ones ( slope . shape ) ] <EOL> model = np . concatenate ( model , axis = <NUM_LIT:1> ) <EOL> inv_model = linalg . pinv ( model ) <EOL> hp_n = <NUM_LIT:2> ** ( int ( np . ceil ( np . log2 ( n_window ) ) ) + <NUM_LIT:1> ) <EOL> freqs = fftpack . rfftfreq ( hp_n , <NUM_LIT:1.> / info [ '<STR_LIT>' ] ) <EOL> hp_ind = np . where ( freqs >= hpi_freqs . min ( ) ) [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] - <NUM_LIT:2> <EOL> hp_window = np . concatenate ( <EOL> [ [ <NUM_LIT:0> ] , np . repeat ( np . hanning ( hp_ind - <NUM_LIT:1> ) [ : ( hp_ind - <NUM_LIT:1> ) // <NUM_LIT:2> ] , <EOL> <NUM_LIT:2> ) ] ) [ np . newaxis ] <EOL> picks_meg = pick_types ( info , meg = True , eeg = False , exclude = exclude ) <EOL> if add_hpi_stim_pick : <EOL> if hpi_pick is None : <EOL> raise RuntimeError ( '<STR_LIT>' ) <EOL> picks = np . concatenate ( [ picks_meg , [ hpi_pick ] ] ) <EOL> else : <EOL> picks = picks_meg <EOL> megchs = [ ch for ci , ch in enumerate ( info [ '<STR_LIT>' ] ) if ci in picks_meg ] <EOL> templates = _read_coil_defs ( elekta_defs = True , verbose = False ) <EOL> coils = _create_meg_coils ( megchs , '<STR_LIT>' , coilset = templates ) <EOL> if method == '<STR_LIT>' : <EOL> coils = _concatenate_coils ( coils ) <EOL> else : <EOL> coils = _prep_mf_coils ( info ) <EOL> scale = make_ad_hoc_cov ( info , verbose = False ) <EOL> scale = _get_whitener_data ( info , scale , picks_meg , verbose = False ) <EOL> orig_dev_head_quat = np . concatenate ( [ rot_to_quat ( dev_head_t [ : <NUM_LIT:3> , : <NUM_LIT:3> ] ) , <EOL> dev_head_t [ : <NUM_LIT:3> , <NUM_LIT:3> ] ] ) <EOL> dists = cdist ( coil_head_rrs , coil_head_rrs ) <EOL> hpi = dict ( dists = dists , scale = scale , picks = picks , model = model , <EOL> inv_model = inv_model , coil_head_rrs = coil_head_rrs , <EOL> coils = coils , on = hpi_ons , n_window = n_window , method = method , <EOL> freqs = hpi_freqs , line_freqs = line_freqs , <EOL> hp_ind = hp_ind , hp_n = hp_n , hp_window = hp_window ) <EOL> last = dict ( quat = orig_dev_head_quat , coil_head_rrs = coil_head_rrs , <EOL> coil_dev_rrs = apply_trans ( head_dev_t , coil_head_rrs ) , <EOL> sin_fit = None , fit_time = - t_step_min ) <EOL> return hpi , last <EOL> def _time_prefix ( fit_time ) : <EOL> """<STR_LIT>""" <EOL> return ( '<STR_LIT>' % fit_time ) . ljust ( <NUM_LIT> ) <EOL> @ verbose <EOL> def _calculate_chpi_positions ( raw , t_step_min = <NUM_LIT:0.1> , t_step_max = <NUM_LIT> , <EOL> t_window = <NUM_LIT> , dist_limit = <NUM_LIT> , gof_limit = <NUM_LIT> , <EOL> verbose = None ) : <EOL> """<STR_LIT>""" <EOL> from scipy . spatial . distance import cdist <EOL> hpi , last = _setup_chpi_fits ( raw . info , t_window , t_step_min ) <EOL> fit_idxs = raw . time_as_index ( np . arange ( <NUM_LIT:0.> , raw . times [ - <NUM_LIT:1> ] , t_step_min ) , <EOL> use_rounding = True ) <EOL> quats = [ ] <EOL> logger . info ( '<STR_LIT>' <EOL> % ( len ( fit_idxs ) , raw . times [ - <NUM_LIT:1> ] ) ) <EOL> pos_0 = None <EOL> n_freqs = len ( hpi [ '<STR_LIT>' ] ) <EOL> for midpt in fit_idxs : <EOL> fit_time = midpt / raw . info [ '<STR_LIT>' ] <EOL> time_sl = midpt - hpi [ '<STR_LIT>' ] // <NUM_LIT:2> <EOL> time_sl = slice ( max ( time_sl , <NUM_LIT:0> ) , <EOL> min ( time_sl + hpi [ '<STR_LIT>' ] , len ( raw . times ) ) ) <EOL> with use_log_level ( False ) : <EOL> meg_chpi_data = raw [ hpi [ '<STR_LIT>' ] , time_sl ] [ <NUM_LIT:0> ] <EOL> this_data = meg_chpi_data [ : - <NUM_LIT:1> ] <EOL> chpi_data = meg_chpi_data [ - <NUM_LIT:1> ] <EOL> ons = ( np . round ( chpi_data ) . astype ( np . int ) & <EOL> hpi [ '<STR_LIT>' ] [ : , np . newaxis ] ) . astype ( bool ) <EOL> n_on = np . sum ( ons , axis = <NUM_LIT:0> ) <EOL> if not ( n_on >= <NUM_LIT:3> ) . all ( ) : <EOL> logger . info ( _time_prefix ( fit_time ) + '<STR_LIT>' <EOL> '<STR_LIT>' % ( n_on . min ( ) , ) ) <EOL> continue <EOL> this_len = time_sl . stop - time_sl . start <EOL> if this_len == hpi [ '<STR_LIT>' ] : <EOL> model , inv_model = hpi [ '<STR_LIT>' ] , hpi [ '<STR_LIT>' ] <EOL> else : <EOL> model = hpi [ '<STR_LIT>' ] [ : this_len ] <EOL> inv_model = linalg . pinv ( model ) <EOL> X = np . dot ( inv_model , this_data . T ) <EOL> data_diff = np . dot ( model , X ) . T - this_data <EOL> del model , inv_model <EOL> data_diff *= data_diff <EOL> this_data *= this_data <EOL> g_chan = ( <NUM_LIT:1> - np . sqrt ( data_diff . sum ( axis = <NUM_LIT:1> ) / this_data . sum ( axis = <NUM_LIT:1> ) ) ) <EOL> g_sin = ( <NUM_LIT:1> - np . sqrt ( data_diff . sum ( ) / this_data . sum ( ) ) ) <EOL> del data_diff , this_data <EOL> X_sin , X_cos = X [ : n_freqs ] , X [ n_freqs : <NUM_LIT:2> * n_freqs ] <EOL> signs = np . sign ( np . arctan2 ( X_sin , X_cos ) ) <EOL> X_sin *= X_sin <EOL> X_cos *= X_cos <EOL> X_sin += X_cos <EOL> sin_fit = np . sqrt ( X_sin ) <EOL> if last [ '<STR_LIT>' ] is not None : <EOL> corr = np . corrcoef ( sin_fit . ravel ( ) , last [ '<STR_LIT>' ] . ravel ( ) ) [ <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> if fit_time - last [ '<STR_LIT>' ] <= t_step_max - <NUM_LIT> and corr * corr > <NUM_LIT> : <EOL> continue <EOL> last [ '<STR_LIT>' ] = sin_fit . copy ( ) <EOL> sin_fit *= signs <EOL> del signs , X_sin , X_cos , X <EOL> logger . debug ( '<STR_LIT>' <EOL> '<STR_LIT>' % ( fit_time , np . sqrt ( g_sin ) , <EOL> ( np . sqrt ( g_chan ) > <NUM_LIT> ) . sum ( ) ) ) <EOL> outs = [ _fit_magnetic_dipole ( f , pos , hpi [ '<STR_LIT>' ] , hpi [ '<STR_LIT>' ] , <EOL> hpi [ '<STR_LIT>' ] ) <EOL> for f , pos in zip ( sin_fit , last [ '<STR_LIT>' ] ) ] <EOL> this_coil_dev_rrs = np . array ( [ o [ <NUM_LIT:0> ] for o in outs ] ) <EOL> g_coils = [ o [ <NUM_LIT:1> ] for o in outs ] <EOL> these_dists = cdist ( this_coil_dev_rrs , this_coil_dev_rrs ) <EOL> these_dists = np . abs ( hpi [ '<STR_LIT>' ] - these_dists ) <EOL> good = False <EOL> use_mask = np . ones ( n_freqs , bool ) <EOL> while not good : <EOL> d = these_dists [ use_mask ] [ : , use_mask ] <EOL> d_bad = ( d > dist_limit ) <EOL> good = not d_bad . any ( ) <EOL> if not good : <EOL> if use_mask . sum ( ) == <NUM_LIT:2> : <EOL> use_mask [ : ] = False <EOL> break <EOL> badness = ( d * d_bad ) . sum ( axis = <NUM_LIT:0> ) <EOL> exclude = np . where ( use_mask ) [ <NUM_LIT:0> ] [ np . argmax ( badness ) ] <EOL> use_mask [ exclude ] = False <EOL> good = use_mask . sum ( ) >= <NUM_LIT:3> <EOL> if not good : <EOL> warn ( _time_prefix ( fit_time ) + '<STR_LIT>' <EOL> '<STR_LIT>' <EOL> % ( use_mask . sum ( ) , n_freqs ) ) <EOL> continue <EOL> this_quat , g = _fit_chpi_pos ( this_coil_dev_rrs [ use_mask ] , <EOL> hpi [ '<STR_LIT>' ] [ use_mask ] , <EOL> last [ '<STR_LIT>' ] ) <EOL> if g < gof_limit : <EOL> logger . info ( _time_prefix ( fit_time ) + <EOL> '<STR_LIT>' % ( g , ) ) <EOL> continue <EOL> this_dev_head_t = np . concatenate ( <EOL> ( quat_to_rot ( this_quat [ : <NUM_LIT:3> ] ) , <EOL> this_quat [ <NUM_LIT:3> : ] [ : , np . newaxis ] ) , axis = <NUM_LIT:1> ) <EOL> this_dev_head_t = np . concatenate ( ( this_dev_head_t , [ [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1.> ] ] ) ) <EOL> dt = fit_time - last [ '<STR_LIT>' ] <EOL> vs = tuple ( <NUM_LIT> * np . sqrt ( np . sum ( ( last [ '<STR_LIT>' ] - <EOL> this_coil_dev_rrs ) ** <NUM_LIT:2> , <EOL> axis = <NUM_LIT:1> ) ) / dt ) <EOL> logger . info ( _time_prefix ( fit_time ) + <EOL> ( '<STR_LIT>' + <EOL> '<STR_LIT>' . join ( [ '<STR_LIT>' ] * n_freqs ) ) <EOL> % ( ( use_mask . sum ( ) , n_freqs ) + vs ) ) <EOL> est_coil_head_rrs = apply_trans ( this_dev_head_t , this_coil_dev_rrs ) <EOL> errs = <NUM_LIT> * np . sqrt ( np . sum ( ( hpi [ '<STR_LIT>' ] - <EOL> est_coil_head_rrs ) ** <NUM_LIT:2> , <EOL> axis = <NUM_LIT:1> ) ) <EOL> e = <NUM_LIT:0.> <EOL> d = <NUM_LIT:100> * np . sqrt ( np . sum ( last [ '<STR_LIT>' ] [ <NUM_LIT:3> : ] - this_quat [ <NUM_LIT:3> : ] ) ** <NUM_LIT:2> ) <EOL> r = _angle_between_quats ( last [ '<STR_LIT>' ] [ : <NUM_LIT:3> ] , this_quat [ : <NUM_LIT:3> ] ) / dt <EOL> v = d / dt <EOL> if pos_0 is None : <EOL> pos_0 = this_quat [ <NUM_LIT:3> : ] . copy ( ) <EOL> d = <NUM_LIT:100> * np . sqrt ( np . sum ( ( this_quat [ <NUM_LIT:3> : ] - pos_0 ) ** <NUM_LIT:2> ) ) <EOL> for ii in range ( n_freqs ) : <EOL> if use_mask [ ii ] : <EOL> start , end = '<STR_LIT:U+0020>' , '<STR_LIT:/>' <EOL> else : <EOL> start , end = '<STR_LIT:(>' , '<STR_LIT:)>' <EOL> log_str = ( '<STR_LIT:U+0020>' + start + <EOL> '<STR_LIT>' + <EOL> '<STR_LIT>' + <EOL> '<STR_LIT>' + <EOL> end ) <EOL> if ii <= <NUM_LIT:2> : <EOL> log_str += '<STR_LIT>' <EOL> elif ii == <NUM_LIT:3> : <EOL> log_str += '<STR_LIT>' <EOL> vals = np . concatenate ( ( <NUM_LIT:1000> * hpi [ '<STR_LIT>' ] [ ii ] , <EOL> <NUM_LIT:1000> * est_coil_head_rrs [ ii ] , <EOL> [ g_coils [ ii ] , errs [ ii ] ] ) ) <EOL> if ii <= <NUM_LIT:2> : <EOL> vals = np . concatenate ( ( vals , this_dev_head_t [ ii , : <NUM_LIT:3> ] ) ) <EOL> elif ii == <NUM_LIT:3> : <EOL> vals = np . concatenate ( ( vals , this_dev_head_t [ : <NUM_LIT:3> , <NUM_LIT:3> ] * <NUM_LIT> ) ) <EOL> logger . debug ( log_str . format ( * vals ) ) <EOL> logger . debug ( '<STR_LIT>' <EOL> '<STR_LIT>' <EOL> % ( fit_time , <NUM_LIT:100> * e , g , v , r , d ) ) <EOL> quats . append ( np . concatenate ( ( [ fit_time ] , this_quat , [ g ] , [ e ] , [ v ] ) ) ) <EOL> last [ '<STR_LIT>' ] = fit_time <EOL> last [ '<STR_LIT>' ] = this_quat <EOL> last [ '<STR_LIT>' ] = this_coil_dev_rrs <EOL> logger . info ( '<STR_LIT>' ) <EOL> quats = np . array ( quats , np . float64 ) <EOL> quats = np . zeros ( ( <NUM_LIT:0> , <NUM_LIT:10> ) ) if quats . size == <NUM_LIT:0> else quats <EOL> return quats <EOL> @ verbose <EOL> def filter_chpi ( raw , include_line = True , verbose = None ) : <EOL> """<STR_LIT>""" <EOL> if not raw . preload : <EOL> raise RuntimeError ( '<STR_LIT>' ) <EOL> t_window = <NUM_LIT> <EOL> t_step = <NUM_LIT> <EOL> n_step = int ( np . ceil ( t_step * raw . info [ '<STR_LIT>' ] ) ) <EOL> hpi = _setup_chpi_fits ( raw . info , t_window , t_window , exclude = ( ) , <EOL> add_hpi_stim_pick = False , remove_aliased = True , <EOL> verbose = False ) [ <NUM_LIT:0> ] <EOL> fit_idxs = np . arange ( <NUM_LIT:0> , len ( raw . times ) + hpi [ '<STR_LIT>' ] // <NUM_LIT:2> , n_step ) <EOL> n_freqs = len ( hpi [ '<STR_LIT>' ] ) <EOL> n_remove = <NUM_LIT:2> * n_freqs <EOL> meg_picks = hpi [ '<STR_LIT>' ] <EOL> n_times = len ( raw . times ) <EOL> msg = '<STR_LIT>' % n_freqs <EOL> if include_line : <EOL> n_remove += <NUM_LIT:2> * len ( hpi [ '<STR_LIT>' ] ) <EOL> msg += '<STR_LIT>' % len ( hpi [ '<STR_LIT>' ] ) <EOL> msg += '<STR_LIT>' % len ( meg_picks ) <EOL> proj = np . dot ( hpi [ '<STR_LIT>' ] [ : , : n_remove ] , hpi [ '<STR_LIT>' ] [ : n_remove ] ) . T <EOL> logger . info ( msg ) <EOL> chunks = list ( ) <EOL> last_endpt = <NUM_LIT:0> <EOL> last_done = <NUM_LIT:0.> <EOL> next_done = <NUM_LIT> <EOL> for ii , midpt in enumerate ( fit_idxs ) : <EOL> if midpt / raw . info [ '<STR_LIT>' ] >= next_done or ii == len ( fit_idxs ) - <NUM_LIT:1> : <EOL> logger . info ( '<STR_LIT>' <EOL> % ( last_done , min ( next_done , raw . times [ - <NUM_LIT:1> ] ) ) ) <EOL> last_done = next_done <EOL> next_done += <NUM_LIT> <EOL> left_edge = midpt - hpi [ '<STR_LIT>' ] // <NUM_LIT:2> <EOL> time_sl = slice ( max ( left_edge , <NUM_LIT:0> ) , <EOL> min ( left_edge + hpi [ '<STR_LIT>' ] , len ( raw . times ) ) ) <EOL> this_len = time_sl . stop - time_sl . start <EOL> if this_len == hpi [ '<STR_LIT>' ] : <EOL> this_proj = proj <EOL> else : <EOL> model = hpi [ '<STR_LIT>' ] [ : this_len ] <EOL> inv_model = linalg . pinv ( model ) <EOL> this_proj = np . dot ( model [ : , : n_remove ] , inv_model [ : n_remove ] ) . T <EOL> this_data = raw . _data [ meg_picks , time_sl ] <EOL> subt_pt = min ( midpt + n_step , n_times ) <EOL> if last_endpt != subt_pt : <EOL> fit_left_edge = left_edge - time_sl . start + hpi [ '<STR_LIT>' ] // <NUM_LIT:2> <EOL> fit_sl = slice ( fit_left_edge , <EOL> fit_left_edge + ( subt_pt - last_endpt ) ) <EOL> chunks . append ( ( subt_pt , np . dot ( this_data , this_proj [ : , fit_sl ] ) ) ) <EOL> last_endpt = subt_pt <EOL> if ii < len ( fit_idxs ) - <NUM_LIT:1> : <EOL> next_left_edge = fit_idxs [ ii + <NUM_LIT:1> ] - hpi [ '<STR_LIT>' ] // <NUM_LIT:2> <EOL> else : <EOL> next_left_edge = np . inf <EOL> while len ( chunks ) > <NUM_LIT:0> and chunks [ <NUM_LIT:0> ] [ <NUM_LIT:0> ] <= next_left_edge : <EOL> right_edge , chunk = chunks . pop ( <NUM_LIT:0> ) <EOL> raw . _data [ meg_picks , <EOL> right_edge - chunk . shape [ <NUM_LIT:1> ] : right_edge ] -= chunk <EOL> return raw </s>
