crossTrackAnalysis

 crossTrackAnalysis Analyze cross trajectories
   crossTrackAnalysis( dataset, files, plotflags, analysisflags, animals, trials, flags, overwrite )
 INPUT:
 (required)
   dataset -- string -- prefix for *_analysis.mat's
   files -- string -- argument to dir command specifying files,
     e.g. 'data/*.avi' or 'data/20080806-1511-r4.avi'
         -- or --
   files -- cell array of strings giving argument to dir command specifying files,
     e.g. {'mov/20080806*avi','mov/20080806-1511-r4.avi'}
 (optional)
   plotflags -- cell array -- strings specifying things to plot
   analysisflags -- cell array -- strings specifying analyses to do
   animals -- array -- ID#'s of animals to include in analysis
     empty to include all 
   trials -- array -- indices of trials to include in analysis
     empty to include all 
   overwrite -- boolean -- whether to overwrite previous data

 Performs analysis on cross trajectories, including phase estimation.
 
 $Revision: 1.20 $
 By Sam Burden, Berkeley 2008

0001 function [] = crossTrackAnalysis( dataset, files, plotflags, analysisflags, animals, trials overwrite)
0002 % crossTrackAnalysis Analyze cross trajectories
0003 %   crossTrackAnalysis( dataset, files, plotflags, analysisflags, animals, trials, flags, overwrite )
0004 % INPUT:
0005 % (required)
0006 %   dataset -- string -- prefix for *_analysis.mat's
0007 %   files -- string -- argument to dir command specifying files,
0008 %     e.g. 'data/*.avi' or 'data/20080806-1511-r4.avi'
0009 %         -- or --
0010 %   files -- cell array of strings giving argument to dir command specifying files,
0011 %     e.g. {'mov/20080806*avi','mov/20080806-1511-r4.avi'}
0012 % (optional)
0013 %   plotflags -- cell array -- strings specifying things to plot
0014 %   analysisflags -- cell array -- strings specifying analyses to do
0015 %   animals -- array -- ID#'s of animals to include in analysis
0016 %     empty to include all
0017 %   trials -- array -- indices of trials to include in analysis
0018 %     empty to include all
0019 %   overwrite -- boolean -- whether to overwrite previous data
0020 %
0021 % Performs analysis on cross trajectories, including phase estimation.
0022 %
0023 % $Revision: 1.20 $
0024 % By Sam Burden, Berkeley 2008
0025 
0026 if nargin < 8
0027   overwrite = 0;
0028 if nargin < 6
0029   trials = [];
0030 if nargin < 5
0031   animals = [];
0032 if nargin < 1
0033     error('sam:nofiles', ...
0034           'Must specify input files, e.g. ''mov/*.avi'' or ''mov/20080806-1511-r4.avi'' \n or {''mov/20080806*.avi'',''mov/20080806-1511-r4.avi''.');
0035 end
0036 end
0037 end
0038 end
0039 end
0040 
0041 % Initialize plotting flags
0042 plot_impulse = 0;
0043 plot_backpack = 0;
0044 plot_states = [0 0 0 0 0 0 0 0 0 0];
0045 plot_xy = 0;
0046 plot_heading = 0;
0047 plot_tarsus = 0;
0048 plot_phase = 0;
0049 plot_classification = 0;
0050 
0051 do_impulse = 0;
0052 do_backpack = 0;
0053 do_tarsus = 0;
0054 do_phase = 0;
0055 do_classification = 0;
0056 
0057 save_plots = 1;
0058 
0059 if nargin >= 2
0060   % Wrap single strings
0061   if ~iscell(plotflags)
0062     plotflags = {plotflags};
0063   end
0064   
0065   % Loop through provided flags
0066   for f = 1:length(plotflags)
0067     switch lower(plotflags{f})
0068       case 'impulse'
0069         plot_impulse = 1;
0070       case 'backpack'
0071         plot_backpack = 1;
0072       case 'pitch'
0073         plot_states(1) = 1;
0074       case 'roll'
0075         plot_states(2) = 1;
0076       case 'yaw'
0077         plot_states(3) = 1;
0078       case 'x'
0079         plot_states(4) = 1;
0080       case 'y'
0081         plot_states(5) = 1;
0082       case 'z'
0083         plot_states(6) = 1;
0084       case 'vx'
0085         plot_states(7) = 1;
0086       case 'vy'
0087         plot_states(8) = 1;
0088       case 'ax'
0089         plot_states(9) = 1;
0090       case 'ay'
0091         plot_states(10) = 1;
0092       case 'xy'
0093         plot_xy = 1;
0094       case {'heading','orientation'}
0095         plot_heading = 1;
0096       case {'tarsus','tarsi'}
0097         plot_tarsus = 1;
0098       case {'phaser','phase'}
0099         plot_phase = 1;
0100       case {'classification','class'}
0101         plot_classification = 1;
0102       case {'all','ensemble'}
0103         plot_ensemble = 1;
0104       case {'ind','individual','individuals'}
0105         plot_individuals = 1;
0106     end
0107   end
0108 end
0109 
0110 if nargin >= 3
0111   % Wrap single strings
0112   if ~iscell(analysisflags)
0113     analysisflags = {analysisflags};
0114   end
0115   
0116   % Loop through provided flags
0117   for f = 1:length(analysisflags)
0118     switch lower(analysisflags{f})
0119       case 'impulse'
0120         do_impulse = 1;
0121       case 'backpack'
0122         do_backpack = 1;
0123       case {'tarsus','tarsi'}
0124         do_tarsus = 1;
0125       case {'phaser','phase'}
0126         do_phase = 1;
0127       case {'classification','class'}
0128         do_classification = 1;
0129     end
0130   end
0131 end
0132 
0133 % Get file list
0134 files0 = files;
0135 files = fileList(files);
0136 
0137 % Include all trials if left unspecified
0138 if isempty(trials)
0139   trials = 1:length(files);
0140 end
0141 
0142 % Warn user if no files were found
0143 if isempty(files)
0144   error('Error:  no files found.  Please check filenames.');
0145 end
0146 
0147 % High-speed video frames per second
0148 fps = 500;
0149 
0150 % Different color for each cockroach
0151 colors = flipud(lines(10));
0152 
0153 % Different linetype for each cockroach
0154 symbol = {'.','o','x','+','*','s','d','v','^','<'};
0155 
0156 % Set prefix for figure names
0157 % figprefix = ['N',num2str(size(impulse_data,1)),'_'];
0158 figprefix = '';
0159 
0160 
0161 % Initialize cart impulse struct
0162 impulse = struct('threshold',20,'frames',[nan nan nan],'data',[],'markers',[],'markers2',[],'geom',[]);
0163 
0164 
0165 % Initialize cockroach ID array
0166 animal = nan * zeros(length(files),1);
0167 
0168 if ~do_impulse && (do_backpack || do_tarsus) && exist(['data/',dataset,'_impulse.mat'],'file')
0169   disp('Loading impulse data from file . .');
0170   load(['data/',dataset,'_impulse.mat'], 'impulse', 'impulse_data', 'n_throwaway', ...
0171        'impulse_frame','befimp','aftimp','befmean','befse','befstd','aftmean','aftse','aftstd','impmean',...
0172        'conf_int','animal');
0173 end
0174 
0175 if do_impulse && overwrite
0176   
0177   % Loop through files
0178   for f = 1:length(files)
0179     
0180     % Get filename prefix
0181     prefix = strtok(files(f).name,'.');
0182     
0183     % Add threshold to data structure
0184     impulse(f).threshold = impulse(1).threshold;
0185     impulse(f).frames = [nan nan nan];
0186     
0187     % If cart has been tracked
0188     if exist([files(f).directory,prefix,'-cart-track.mat'],'file') || ...
0189        exist([files(f).directory,prefix,'-cart-track-trim.mat'],'file')
0190       
0191       % Extract cockroach ID# from filename
0192       num = str2num(files(f).name(16));
0193       if ~isempty(num)
0194         animal(f) = num;
0195       else
0196         animal(f) = 1;
0197       end
0198       
0199       % Skip files that aren't from the specified animals
0200       if ~isempty(animals) && ~any(animals == animal(f))
0201         continue;
0202       end
0203       
0204       % Load cart data
0205       disp(['Loading ',files(f).directory,prefix,'-cart-track']);
0206       if exist([files(f).directory,prefix,'-cart-track-trim.mat'],'file')
0207         load([files(f).directory,prefix,'-cart-track-trim.mat'],'cart');
0208       elseif exist([files(f).directory,prefix,'-cart-track.mat'],'file')
0209         load([files(f).directory,prefix,'-cart-track.mat'],'cart');
0210       end
0211       
0212       % Load cart geometry
0213       g = cart.geom;
0214       
0215       impulse(f).tforms = {};
0216       
0217       % Loop through video frames
0218       for v = 1:size(cart.tforms,2)
0219         
0220         % Get projective transformation for frame
0221         % This maps from cart geometry to pixels
0222         T = maketform('projective',reshape([cart.tforms(1:8,v); 1]',3,3));
0223         
0224         %       keyboard
0225         
0226         % Apply transformation to marker positions
0227         x = cart.traj(v,:);
0228         x = [real(x)', imag(x)'];
0229         x = applyProjection(T.tdata.Tinv,x);
0230         x = geompx2cm(x); % Convert from px to cm
0231         impulse(f).markers(v,:) = (x * [1; i])';
0232         
0233         impulse(f).markers2(v,:) = (applyProjection(T.tdata.T,g) * [1; i])';
0234         
0235         % Store transformation
0236         impulse(f).tforms{v+cart.frames(1)-1} = T;
0237         
0238       end
0239       
0240       % TODO: Kalman filter cart position data to estimate vel and accel
0241       % -- use Shai's newKalmanConstA function . .
0242       
0243       % Rotate direction of motion to align with x-axis
0244       r = mean(impulse(f).markers,2);
0245       r = r(end)-r(1);
0246       r = r / abs(r);
0247       x = impulse(f).markers / r;
0248       
0249       % Store rotation that aligns motion with x-axis
0250 %       impulse(f).rot = r;
0251       
0252       % Store motion along x-axis
0253       impulse(f).pos = mean(impulse(f).markers,2);
0254       
0255       
0256       % Create a Butterworth filter with specified order and cutoff frequency
0257       [B, A] = butter(3, 0.2);
0258       
0259       % Filter the position data
0260       xf = filtfilt(B, A, x);
0261       
0262       % Filter the velocity data
0263       vff = filtfilt(B, A, diff(xf));
0264       
0265       % Filter the acceleration data
0266       aff = filtfilt(B, A, diff(vff));
0267       aff = aff;
0268       
0269       
0270       % Estimate cart acceleration using Kalman smoothing filter
0271       s = real(mean(x,2))';
0272   
0273       Q = [0.0001, 0.001, 0.01]; % system noise covariance
0274       R = sqrt(2); % measurement noise covariance
0275 
0276       spec = newKalmanConstA( s(1), 0, 0, Q, R );
0277       
0278       [sk,V,VV] = kalman_smooth( s, spec );
0279 
0280       
0281       % Assume noise is independent between acceleration measurements,
0282       % average over measurements to obtain a better estimate of
0283       % actual cart acceleration.
0284       affm = mean(aff,2);     
0285       
0286 %       figure;
0287 %       hold on;
0288 %       plot(real(mean(x,2)),'g','LineWidth',3);
0289 %       plot(sk(1,:),'b','LineWidth',2);
0290 %       legend({'markers','kalman'});
0291       
0292 %       figure;
0293 %       hold on;
0294 %       plot(real(affm),'g','LineWidth',3);
0295 %       plot(sk(3,:),'b','LineWidth',2);
0296 %       legend({'filtfilt','kalman'});
0297 %
0298 %       return
0299 %
0300 %       keyboard
0301 
0302       
0303       % Lose a frame from each of two diff's
0304       impulse(f).frames(1) = cart.frames(1);
0305       impulse(f).frames(3) = cart.frames(3);
0306       impulse(f).fr = impulse(f).frames(1):impulse(f).frames(3);
0307       
0308       n_throwaway = 25;
0309       
0310       % First and last stretches of data have filtering boundary effects
0311       affm(1:n_throwaway) = [];
0312       affm(end-n_throwaway+1:end) = [];
0313       sk(:,1:n_throwaway) = [];
0314       sk(:,end-n_throwaway+1:end) = [];
0315       
0316       % Integrate acceleration to obtain net momentum change
0317       impulse(f).data = nan * (1:impulse(f).frames(3));
0318 %       fr = (impulse(f).frames(1)+n_throwaway+1):(impulse(f).frames(3)-n_throwaway-1);
0319 %       impulse(f).data(fr) = cumsum(real(affm))' * fps;
0320       fr = (impulse(f).frames(1)+n_throwaway):(impulse(f).frames(3)-n_throwaway);
0321       impulse(f).data(fr) = cumsum(sk(3,:)) * fps;
0322       
0323       impulse(f).kalman = sk;
0324       
0325       
0326       
0327 %       figure;
0328 %       hold on;
0329 %       plot(fr,impulse(f).data(fr),'k','LineWidth',4);
0330 %       plot(cumsum(sk(3,:)*fps),'b','LineWidth',2);
0331 %       legend({'filtfilt','kalman'});
0332       
0333 %       keyboard
0334 
0335       
0336       % Record frame number at which impulse reached threshold
0337       frames = find(impulse(f).data >= impulse(f).threshold);
0338       impulse(f).frames(2) = frames(1);
0339       
0340       % Store geometry
0341       impulse(f).geom = geompx2cm(cart.geom);
0342       
0343 
0344       % Free unneeded memory
0345       clear cart
0346       
0347     end % If cart has been tracked
0348     
0349   end % Loop through files
0350 
0351 
0352   % Don't consider files that didn't have appropriate cart data
0353   f = 1;
0354   while f <= length(files)
0355     if isnan(impulse(f).frames(2))
0356       disp(['Warning:  no cart data found for ',files(f).directory,files(f).name,'.  Removing.']);
0357       files(f) = [];
0358       impulse(f) = [];
0359       animal(f) = [];
0360       trials(f) = [];
0361     else
0362       f = f+1;
0363     end
0364   end 
0365 
0366 
0367   % Collect impulse data in one big array
0368   [impulse_data,impulse_frame] = oneBigArray(impulse);
0369 
0370   % Before and after impulse frame ranges
0371   bi = [100,15];
0372   ai = [20,200];
0373   
0374   % Normalize impulse data by zeroing on segment before impulse
0375   impulse_data = impulse_data - repmat(nanmean(impulse_data(:,1:impulse_frame-bi(2))')',1,size(impulse_data,2));
0376 
0377   
0378   impmean = nanmean(impulse_data);
0379   befimp = (impulse_frame-bi(1)):impulse_frame-bi(2);
0380   befmean = nanmean(impulse_data(:,befimp));
0381   befres = impulse_data(:,befimp) - repmat(befmean,size(impulse_data,1),1);
0382   befse = nanstd(befres(:))/sqrt(length(befres(:)));
0383   befstd = nanstd(befres(:));
0384 %   befse = nanstd(impulse_data(:,befimp))/sqrt(size(impulse_data,1));
0385   befmean = mean(befmean);
0386   
0387   aftimp = (impulse_frame+ai(1)):impulse_frame+ai(2);
0388   aftmean = nanmean(impulse_data(:,aftimp));
0389 %   aftse = nanstd(impulse_data(:,aftimp))/sqrt(size(impulse_data,1));
0390   aftres = impulse_data(:,aftimp) - repmat(aftmean,size(impulse_data,1),1);
0391   aftse = nanstd(aftres(:))/sqrt(length(aftres(:)));
0392   aftstd = nanstd(aftres(:));
0393   aftmean = mean(aftmean);
0394   
0395   % Find confidence interval for start and end of impulse
0396   conf_int = impulse_frame*[1,1];
0397   
0398   for r = 1:size(impulse_data,1)
0399     conf_int(1) = min([conf_int(1),max(find(impulse_data(r,:)<=befmean+befstd))]);
0400     conf_int(2) = max([conf_int(2),min(find(impulse_data(r,:)>=aftmean-aftstd))]);
0401   end
0402 
0403   % Save data
0404   disp(['Saving to ',dataset,'_impulse.mat . .']); 
0405   if exist(['data/',dataset,'_impulse.mat'],'file')
0406     system(['mv data/',dataset,'_impulse.mat data/',dataset,'_impulse.mat.bak']);
0407   end
0408   save(['data/',dataset,'_impulse.mat'])
0409 
0410 end
0411 
0412 % keyboard
0413 
0414 % Stop analysis if no data was found
0415 if length(files) == 0
0416   disp('Warning:  no data files found.  Quitting.');
0417   return;
0418 end
0419 
0420 % keyboard
0421 
0422 
0423 if plot_impulse
0424   figure;
0425   hold on;
0426   
0427   plot_intervals = 0;
0428   
0429 %   t = (1:size(impulse_data,2));
0430   t = (1:size(impulse_data,2))./fps;
0431   
0432   plot(t(t<t(impulse_frame)),(befmean+befstd),'--','Color',0.5*[1,1,1]);
0433   plot(t(t>t(impulse_frame)),(aftmean-aftstd),'--','Color',0.5*[1,1,1]);
0434     
0435   % Intermittant timepoints
0436   dt = 25;
0437   impf = impulse_frame;
0438   ti = [fliplr(impf:-dt:1),(impf+dt):dt:size(impulse_data,2)];
0439 
0440 %   ci = impulse_frame*[1,1];
0441   
0442   for r = 1:size(impulse_data,1)
0443     plot(t, impulse_data(r,:),'color',colors(animal(r),:));
0444       
0445     % Plot symbols intermittantly
0446     plot(t(ti),impulse_data(r,ti),symbol{animal(r)},'color',colors(animal(r),:));
0447     
0448 %     ci(1) = min([ci(1),max(find(impulse_data(r,:)<=befmean+befstd))]);
0449 %     ci(2) = max([ci(2),min(find(impulse_data(r,:)>=aftmean-aftstd))]);
0450   end
0451 %   plot(t,impmean,'r','LineWidth',2);
0452 
0453   if plot_intervals
0454     plot(t(befimp),(befmean), 'k');
0455     %   plot(t(befimp),(befmean+1.96*befse), 'k--');
0456     %   plot(t(befimp),(befmean-1.96*befse), 'k--');
0457     plot(t(befimp),(befmean+befstd), 'k--');
0458     plot(t(befimp),(befmean-befstd), 'k--');
0459     
0460     plot(t(aftimp),(aftmean), 'k');
0461     %   plot(t(aftimp),(aftmean+1.96*aftse), 'k--');
0462     %   plot(t(aftimp),(aftmean-1.96*aftse), 'k--');
0463     plot(t(aftimp),(aftmean+aftstd), 'k--');
0464     plot(t(aftimp),(aftmean-aftstd), 'k--');
0465   end
0466   
0467   set(gca,'YLim',[-5,80]);
0468   
0469   ylim = get(gca,'ylim');
0470   plot(t(impulse_frame) * [1,1], ylim, 'k');
0471   plot([t(conf_int([1,1])),nan,t(conf_int([2,2]))], [ylim(:);nan;flipud(ylim(:))], 'k--');
0472   xlabel('Time (sec)','FontSize',18,'FontName','Times');
0473   ylabel('Impulse (cm/sec)','FontSize',18,'FontName','Times');
0474   set(gca,'FontSize',14,'FontName','Times');
0475   title([dataset,' cart impulse, N = ',num2str(size(impulse_data,1))],'FontSize',18,'FontName','Times');
0476 
0477   if save_plots
0478     saveas(gca,['fig/',dataset,'/fig/',figprefix,'impulse.fig'],'fig');
0479     saveas(gca,['fig/',dataset,'/',figprefix,'impulse.png'],'png');
0480     saveas(gca,['fig/',dataset,'/eps/',figprefix,'impulse.eps'],'eps');
0481   end
0482   
0483 end
0484 
0485 
0486 if exist(['data/',dataset,'_backpack.mat'])
0487   disp('Loading backpack data from file . .');
0488   load(['data/',dataset,'_backpack.mat'], 'backpack', 'backpack_data', 'files', ...
0489        'states','units','ylims','impulse_frame','animal');
0490 end
0491 
0492 if do_backpack && overwrite
0493   
0494   % Initialize backpack struct
0495   backpack = struct('data',[],'frames',[nan nan nan]);
0496   
0497   % Loop through files
0498   for f = 1:length(files)
0499     
0500     % Get filename prefix
0501     prefix = strtok(files(f).name,'.');
0502     
0503     % If cross has been tracked
0504     if exist([files(f).directory,prefix,'-cross-track.mat'],'file') 
0505       
0506       % Keep track of valid frame range
0507       valid_frames = [];
0508       
0509       disp(['Loading ',files(f).directory,prefix,'-cross-*.mat . .']);
0510       
0511       % Load cross initialization
0512       if exist([files(f).directory,prefix,'-cross-init.mat'],'file') 
0513         load([files(f).directory,prefix,'-cross-init.mat'],'cross');
0514         if isfield(cross,'valid_frames')
0515           % Believe the frames in the -init.mat more than the -track.mat
0516           valid_frames = cross.valid_frames;
0517           if valid_frames(2) == valid_frames(1)
0518             continue;
0519           end
0520         end
0521       end
0522       
0523       % Load cross data
0524       load([files(f).directory,prefix,'-cross-track.mat'],'cross');
0525       if isempty(valid_frames) && isfield(cross,'valid_frames')
0526         valid_frames = cross.valid_frames;
0527       end
0528       
0529       % Record valid frame range
0530       cross.valid_frames = valid_frames;
0531       
0532       % Fix cross frame numbers
0533 %       if cross.valid_frames(2) > size(cross.traj,1)
0534       if cross.valid_frames(2) > cross.frames(3)
0535         disp(['Warning:  specified frames [',num2str(cross.valid_frames),'] exceeds trajectory range [',num2str(cross.frames([1,3])),']']);
0536 %         cross.valid_frames(2) = size(cross.traj,1);
0537         cross.valid_frames(2) = cross.frames(3);
0538       end
0539       if cross.valid_frames(1) < cross.frames(1)
0540         disp(['Warning:  specified frames [',num2str(cross.valid_frames),'] exceeds trajectory range [',num2str(cross.frames([1,3])),']']);
0541 %         cross.valid_frames(2) = size(cross.traj,1);
0542         cross.valid_frames(1) = cross.frames(1);
0543       end
0544       
0545       % Assign frame numbers
0546       backpack(f).frames = [cross.valid_frames(1), impulse(f).frames(2), cross.valid_frames(2)];
0547       backpack(f).fr = backpack(f).frames(1):backpack(f).frames(3);
0548       
0549       % Get video direct linear transformation
0550       backpack(f).dlt = cross.camspec.dlt;
0551       
0552       % Copy cross state data
0553       try
0554 %         backpack(f).data = cross.state(:,backpack(f).frames(1):backpack(f).frames(2));
0555         backpack(f).data = nan * ones(size(cross.state,1),backpack(f).frames(3));
0556         backpack(f).data(:,backpack(f).fr) = cross.state(:,backpack(f).fr - cross.frames(1) + 1);
0557       catch
0558         disp(['Backpack data copy error']);
0559         keyboard
0560       end
0561       
0562     end % If cross has been tracked
0563     
0564   end % Loop through files
0565   
0566 %   keyboard
0567   
0568   % Don't consider files that didn't have appropriate backpack data
0569   f = 1;
0570   while f <= length(files)
0571     if isempty(backpack(f).data)
0572       disp(['Warning:  no cross data found for ',files(f).directory,files(f).name,'.  Removing.']);
0573       files(f) = [];
0574       impulse(f) = [];
0575       backpack(f) = [];
0576       animal(f) = [];
0577       trials(f) = [];
0578     else
0579       f = f+1;
0580     end
0581   end 
0582 
0583 
0584   % Helpful declarations for plotting
0585   states = {'pitch','roll','yaw','x','y','z','vx','vy','ax','ay'};
0586   % units = {'degrees','degrees','degrees','cm','cm','cm','cm/sec','cm/sec','g','g'};
0587   units = {'degrees','degrees','degrees','cm','cm','cm','cm/sec','cm/sec','g','g'};
0588   ylims = {[-25,25],[-40,40],[-70,50],[-20,30],[-6,6],[],[],[],[],[]};
0589   
0590   
0591   % Collect impulse data in one big array [n_trials] x [n_frames] x [n_states]
0592   [backpack_data, impulse_frame] = oneBigArray(backpack);
0593   
0594   % Convert angles to degrees from radians
0595   backpack_data(:,:,[1,2,3]) = backpack_data(:,:,[1,2,3]) * 180 / pi;
0596   
0597   % Convert distances to cm from mm
0598   backpack_data(:,:,[4,5,6]) = backpack_data(:,:,[4,5,6]) / 10;
0599   
0600   % Convert velocities to cm/sec from mm/frame
0601   backpack_data(:,:,[7,8]) = backpack_data(:,:,[7,8]) * 500 / 10;
0602   
0603   % Convert accelerations to g's from mm/frame/frame
0604   backpack_data(:,:,[7,8]) = backpack_data(:,:,[7,8]) * 500 * 500 / 100 / 9.81;
0605 
0606   disp(['Saving to ',dataset,'_backpack.mat . .']); 
0607   if exist(['data/',dataset,'_backpack.mat'],'file')
0608     system(['mv data/',dataset,'_backpack.mat data/',dataset,'_backpack.mat.bak']);
0609   end
0610   save(['data/',dataset,'_backpack.mat'])
0611 
0612 end
0613 
0614 % Vectorized creation of rotation matrices from pitch, roll, yaw
0615 %   x is 3xN
0616 %   x(1,:) is pitch
0617 %   x(2,:) is roll
0618 %   x(3,:) is yaw
0619 %
0620 %   R is 3x3xN
0621 %   R(:,:,n) is the n^th 3x3 rotation matrix
0622 
0623 
0624 if do_backpack
0625 
0626 % Normalize backpack data
0627 
0628 % Copy backpack dataq
0629 bdata = backpack_data;
0630 
0631 % Loop over data
0632 for f = 1:length(files)
0633   
0634 %   keyboard
0635     
0636   
0637   % Normalize x & y positions
0638   % -------------------------
0639   
0640   % Cart motion
0641   c = mean(impulse(f).markers,2);
0642   c = [real(c),-imag(c)];
0643   c = c * [1; i];
0644   
0645   % Loop through data
0646   for k = 1:size(backpack(f).data,2)
0647 
0648     % Get (x,y,z) position of COM
0649     cxyz = backpack(f).data([4,5,6],k);
0650 
0651     % Get (x,y,z) position of head
0652     hxyz = geomToCalib(backpack(f).data(:,k),[-10,0,0])';
0653     
0654     % Don't process missing datapoints
0655     if all(~isnan(cxyz))
0656       
0657       % Apply camera direct linear transformation, transforming from cross
0658       % geometry COM to raw video pixels
0659       cxy = backpack(f).dlt*[cxyz; 1];
0660       cxy = cxy(1:2) ./ cxy(3); % Correct for change of scale
0661       hxy = backpack(f).dlt*[hxyz; 1];
0662       hxy = hxy(1:2) ./ hxy(3); % Correct for change of scale
0663       
0664 %       keyboard
0665       
0666       % Cart motion compensation; transforms points into cart frame
0667       cxy = applyProjection(impulse(f).tforms{k}.tdata.Tinv,cxy') * [1; i];
0668       hxy = applyProjection(impulse(f).tforms{k}.tdata.Tinv,hxy') * [1; i];
0669       
0670       % Rotate points to align with cart motion
0671 %       xy = xy / impulse(f).rot;
0672 
0673       % Subtract cart motion
0674       cxy = geompx2cm(cxy) - c(k);
0675       hxy = geompx2cm(hxy) - c(k);
0676 
0677 %       cxy = [1,i]*cxy;
0678 %       hxy = [1,i]*hxy;
0679       
0680       % Fix position in data structure
0681       backpack(f).data([4,5],k) = [real(cxy),imag(cxy)];
0682       backpack(f).data([11,12],k) = [real(hxy),imag(hxy)] - backpack(f).data([4,5],k)';
0683 
0684     end
0685 
0686   end
0687   
0688   % Filter position data, then differentiate to obtain velocity
0689   x = backpack(f).data(4,:);
0690   y = backpack(f).data(5,:);
0691   
0692   % Get actual data samples
0693   fr = 1:length(x);
0694   fr(isnan(x)) = [];
0695   
0696   % Create a Butterworth filter with specified order and cutoff frequency
0697   [B, A] = butter(3, 0.3);
0698   
0699   % Filter the position data
0700   x = filtfilt(B, A, x(fr));
0701   y = filtfilt(B, A, y(fr));
0702       
0703   % Differentiate to obtain velocity
0704   vx = diff(x);
0705   vy = diff(y);
0706   
0707   % Replace velocity data in structure
0708   backpack(f).data([7,8],:) = nan;
0709   backpack(f).data([7,8],fr(1:end-1)) = [vx;vy];
0710   
0711 %   figure;
0712 %   hold on;
0713 %   plot(backpack(f).data(7,:),'g');
0714 %   plot(backpack(f).data(8,:),'r');
0715 %
0716 %   keyboard
0717 
0718 
0719 end
0720 
0721 
0722 % Collect impulse data in one big array [n_trials] x [n_frames] x [n_states]
0723 [backpack_data, impulse_frame] = oneBigArray(backpack);
0724 
0725 
0726 % keyboard
0727 
0728 
0729 % Copy backpack data
0730 bdata = backpack_data;
0731 
0732 for f = 1:length(files)
0733   
0734   % Translate positions to coincide at perturbation
0735   bdata(f,:,4) = -(bdata(f,:,4) - nanmean(bdata(f,befimp,4)));
0736   bdata(f,:,5) = bdata(f,:,5) - nanmean(bdata(f,befimp,5));
0737   bdata(f,:,11) = -bdata(f,:,11);
0738   
0739   % Translate velocities to coincide at perturbation
0740   bdata(f,:,7) = bdata(f,:,7) - nanmean(bdata(f,befimp,7));
0741   bdata(f,:,8) = bdata(f,:,8) - nanmean(bdata(f,befimp,8));
0742   
0743   
0744   % Normalize pitch, roll, and yaw
0745   % ------------------------------
0746 
0747   % Average pitch, roll, and yaw over normalization window
0748   t1 = nanmean(bdata(f,befimp,1));
0749   t2 = nanmean(bdata(f,befimp,2));
0750   t3 = nanmean(bdata(f,befimp,3));
0751   
0752   % Generate normalizing rotation
0753   R0 = rotationMatrix([t1; t2; t3]);
0754   
0755   % Generate sequence of rotations from pitch, roll, yaw data
0756   x = [];
0757   for k = 1:3
0758     x = [x; bdata(f,:,k)];
0759   end
0760   R = rotationMatrix( x );
0761   
0762   % Apply normalizing transformation
0763   Rh = zeros(size(R));
0764   R0i = inv(R0);
0765   for k = 1:size(R,3)
0766     Rh(:,:,k) = R0i * R(:,:,k);
0767   end
0768   
0769   % Extract normalized pitch, roll, yaw
0770   bdata(f,:,1) = atan2( Rh(1,3,:), Rh(3,3,:) );
0771   bdata(f,:,2) = atan2( Rh(2,3,:), Rh(3,3,:) );
0772   bdata(f,:,3) = atan2( Rh(1,2,:), Rh(1,1,:) );
0773   
0774   % Average pitch, roll, and yaw over normalization window
0775   t1 = nanmean(bdata(f,befimp,1));
0776   t2 = nanmean(bdata(f,befimp,2));
0777   t3 = nanmean(bdata(f,befimp,3));
0778   
0779   % Sanity check on normalizing transformation
0780   R1 = rotationMatrix([t1; t2; t3]) - eye(3);
0781 end
0782 
0783 % keyboard
0784 
0785 % Convert angles to degrees from radians
0786 bdata(:,:,[1,2,3]) = bdata(:,:,[1,2,3]) * 180 / pi;
0787 
0788 % Convert distances to cm from mm
0789 % backpack_data(:,:,[4,5,6]) = backpack_data(:,:,[4,5,6]) / 10;
0790 
0791 % Convert velocities to cm/sec from cm/frame
0792 bdata(:,:,[7,8]) = bdata(:,:,[7,8]) * 500;
0793 
0794 % Convert accelerations to g's from mm/frame/frame
0795 % backpack_data(:,:,[7,8]) = backpack_data(:,:,[7,8]) * 500 * 500 / 100 / 9.81;
0796 
0797 end % If do_backpack
0798 
0799 if plot_backpack || any(plot_states) || plot_xy || plot_heading
0800 %   colors = hsv(size(bdata,1));
0801 %   colors = hsv(length(unique(animal)));
0802 
0803 %   t = (1:size(impulse_data,2));
0804   t = (1:size(backpack_data,2))./fps;
0805   
0806   % Set y limits
0807   ylims = {[-25,25],[-40,40],[-80,50],[-20,30],[-6,6],[],[],[],[],[]};
0808     
0809   % Intermittant timepoints
0810   dt = 25;
0811   impf = impulse_frame;
0812   ti = [fliplr(impf:-dt:1),(impf+dt):dt:size(bdata,2)];
0813   
0814   for s = 1:length(states)
0815     if plot_backpack || plot_states(s)
0816       
0817       if plot_individuals
0818         
0819         % Loop through animals
0820         au = unique(animal);
0821         for a = 1:length(au)
0822         
0823           figure(1000+s);
0824           clf;
0825           hold on;
0826           
0827           % Trials involving animal
0828           r = find(animal == au(a));
0829           
0830           % Data involving animal
0831           dx = [];
0832           dxi = [];
0833           dy = [];
0834           dyi = [];
0835           for k = 1:length(r)
0836             dx = [dx,nan,t];
0837             dxi = [dxi,nan,t(ti)];
0838             dy = [dy,nan,bdata(r(k),:,s)];
0839             dyi = [dyi,nan,bdata(r(k),ti,s)];
0840           end
0841 
0842           plot(dx,dy,'color',colors(au(a),:),'LineWidth',3);
0843           
0844           % Plot symbols intermittantly
0845           plot(dxi,dyi,symbol{au(a)},'color',colors(au(a),:),'MarkerSize',12);
0846         
0847           % Fix y limits across datasets
0848           if ~isempty(ylims{s})
0849             set(gca,'YLim',ylims{s});
0850           end
0851           
0852           ylim = get(gca,'ylim');
0853           plot(t(impulse_frame) * [1,1], ylim, 'k');
0854           plot([t(conf_int([1,1])),nan,t(conf_int([2,2]))], [ylim(:);nan;flipud(ylim(:))], 'k--');
0855           xlabel('Time (sec)','FontSize',18,'FontName','Times');
0856           ylabel([states{s},' (',units{s},')'],'FontSize',18,'FontName','Times');
0857           title([dataset,': animal #',num2str(au(a)),', ',states{s},' (',units{s},'), N = ',num2str(length(r))],'FontSize',18,'FontName','Times');
0858           set(gca,'FontSize',14,'FontName','Times');
0859           
0860           if save_plots
0861             saveas(gca,['fig/',dataset,'/fig/',figprefix,states{s},'_a',num2str(au(a)),'.fig'],'fig');
0862             saveas(gca,['fig/',dataset,'/',figprefix,states{s},'_a',num2str(au(a)),'.png'],'png');
0863             saveas(gca,['fig/',dataset,'/eps/',figprefix,states{s},'_a',num2str(au(a)),'.eps'],'eps');
0864           end
0865           
0866         end
0867         
0868       end
0869         
0870       if plot_ensemble
0871         
0872         figure(1000+s);
0873         clf
0874         hold on;
0875         for r = 1:size(bdata,1)
0876           plot(t(1:size(bdata,2)),bdata(r,:,s),'color',colors(animal(r),:));
0877           
0878           % Plot symbols intermittantly
0879           plot(t(ti),bdata(r,ti,s),symbol{animal(r)},'color',colors(animal(r),:));
0880           
0881         end
0882         
0883         % Fix y limits across datasets
0884         if ~isempty(ylims{s})
0885           set(gca,'YLim',ylims{s});
0886         end
0887         
0888         ylim = get(gca,'ylim');
0889         plot(t(impulse_frame) * [1,1], ylim, 'k');
0890         plot([t(conf_int([1,1])),nan,t(conf_int([2,2]))], [ylim(:);nan;flipud(ylim(:))], 'k--');
0891         xlabel('Time (sec)','FontSize',18,'FontName','Times');
0892         ylabel([states{s},' (',units{s},')'],'FontSize',18,'FontName','Times');
0893         title([dataset,': ',states{s},' (',units{s},'), N = ',num2str(size(bdata,1))],'FontSize',18,'FontName','Times');
0894         set(gca,'FontSize',14,'FontName','Times');
0895         
0896         if save_plots
0897           saveas(gca,['fig/',dataset,'/fig/',figprefix,states{s},'.fig'],'fig');
0898           saveas(gca,['fig/',dataset,'/',figprefix,states{s},'.png'],'png');
0899           saveas(gca,['fig/',dataset,'/eps/',figprefix,states{s},'.eps'],'eps');
0900         end
0901       end
0902     end
0903   end
0904   
0905   % Plot y vs x
0906   if plot_backpack || plot_xy
0907     
0908     % Intermittant timepoints
0909     dt = 25;
0910     impf = impulse_frame;
0911     t = [fliplr(impf:-dt:1),(impf+dt):dt:size(bdata,2)];    
0912     
0913     figure(2000);
0914     hold on;
0915     axis equal;
0916     for r = 1:size(bdata,1)
0917       ix = nanmean(bdata(r,befimp,4));
0918       iy = nanmean(bdata(r,befimp,5));
0919 %       c = find(unique(animal) == animal(r));
0920       plot(bdata(r,:,4)-ix,bdata(r,:,5)-iy,'color',colors(animal(r),:));
0921       
0922       ylim = get(gca,'ylim');
0923       plot(0,0,'.','color',colors(animal(r),:),'MarkerSize',20);
0924       
0925       % Plot symbols intermittantly
0926       plot(bdata(r,t,4)-ix,bdata(r,t,5)-iy,symbol{animal(r)},'color',colors(animal(r),:));
0927 
0928     end
0929     
0930     % Set x- and y-limits using the limits from
0931     % x- and y- position
0932     set(gca,'XLim',ylims{4},'YLim',ylims{5});
0933     
0934     xlabel([states{4},' (',units{4},')'],'FontSize',18,'FontName','Times');
0935     ylabel([states{5},' (',units{5},')'],'FontSize',18,'FontName','Times');
0936     title([dataset,' ',states{4},states{5},' (',units{4},'), N = ',num2str(size(bdata,1))],'FontSize',18,'FontName','Times');
0937     set(gca,'FontSize',14,'FontName','Times');
0938     
0939     if save_plots
0940       saveas(gca,['fig/',dataset,'/fig/',figprefix,states{4},states{5},'.fig'],'fig');
0941       saveas(gca,['fig/',dataset,'/',figprefix,states{4},states{5},'.png'],'png');
0942       saveas(gca,['fig/',dataset,'/eps/',figprefix,states{4},states{5},'.eps'],'eps');
0943     end
0944   end
0945   
0946   % Plot orientation on top of xy (i.e. heading)
0947   if plot_backpack || plot_heading
0948     
0949     % Loop through trials
0950     for r = 1:size(bdata,1)
0951   
0952       % Orientation sanity check:  compare with yaw plot
0953 %       figure(1003);
0954 %       hold on;
0955 %       plot(t(1:size(bdata,2)),atan2(bdata(r,:,12),bdata(r,:,11))*180/pi,'color','k');
0956       
0957       figure(2001);
0958       clf;
0959       hold on;
0960       axis equal;
0961       
0962       ix = nanmean(bdata(r,befimp,4));
0963       iy = nanmean(bdata(r,befimp,5));
0964 
0965       plot(bdata(r,:,4)-ix,bdata(r,:,5)-iy,'color',colors(animal(r),:));
0966 %       plot(backpack_data(r,:,4),backpack_data(r,:,5)-iy,'color',colors(animal(r),:));
0967       
0968       % Orientation plot indices
0969       dt = 25;
0970       impf = impulse_frame;
0971       t = [fliplr(impf:-dt:1),(impf+dt):dt:size(bdata,2)];
0972       
0973       % Plot symbols intermittantly
0974       plot(bdata(r,t,4)-ix,bdata(r,t,5)-iy,symbol{animal(r)},'color',colors(animal(r),:));
0975 
0976       p0 = [bdata(r,t,4);bdata(r,t,5)];
0977 %       p1 = bdata(r,t,3)*pi/180;
0978 %       p1 = backpack_data(r,t,3);
0979 %       p1 = [cos(p1); sin(p1)];
0980       p1 = [bdata(r,t,11);bdata(r,t,12)];
0981 
0982 %       p1 = flipud(p1);
0983       
0984       for k = 1:size(p0,2)
0985         plot(p0(1,k)+[0,p1(1,k)],p0(2,k)+[0,p1(2,k)],'color',colors(animal(r),:),'LineWidth',2);
0986         plot(p0(1,k),p0(2,k),'.','color',colors(animal(r),:),'MarkerSize',10);
0987       end
0988       
0989 %       keyboard
0990       
0991       plot(0,0,'x','color',colors(animal(r),:),'MarkerSize',40);
0992     
0993       % Set x- and y-limits using the limits from
0994       % x- and y- position
0995       set(gca,'XLim',ylims{4},'YLim',ylims{5});
0996       
0997       xlabel([states{4},' (',units{4},')'],'FontSize',18,'FontName','Times');
0998       ylabel([states{5},' (',units{5},')'],'FontSize',18,'FontName','Times');
0999       title([dataset,' animal ',num2str(animal(r)),' heading, N = ',num2str(size(bdata,1))],'FontSize',18,'FontName','Times');
1000       set(gca,'FontSize',14,'FontName','Times');
1001       
1002 %       keyboard
1003       
1004       if save_plots
1005         fileprefix = ['fig/',dataset];
1006         filename = [figprefix,'heading_',num2str(r)];
1007         saveas(gca,[fileprefix,'/fig/',filename,'.fig'],'fig');
1008         saveas(gca,[fileprefix,'/',filename,'.png'],'png');
1009         saveas(gca,[fileprefix,'/eps/',filename,'.eps'],'eps');
1010       end    
1011     end
1012   end
1013 end
1014 
1015 % keyboard
1016 
1017 
1018 if 0 && exist(['data/',dataset,'_tarsus.mat'])
1019   disp('Loading tarsus data from file . .');
1020   load(['data/',dataset,'_tarsus.mat'], 'tarsus', 'files', 'impulse', 'backpack');
1021 end
1022 
1023 if do_tarsus && overwrite
1024   
1025   % Initialize tarsus track struct
1026   tarsus = struct('pthX',[],'pthY',[]);
1027   
1028   rr = struct([]);
1029   
1030   % Loop through files
1031   for f = 1:length(files)
1032     
1033     % Get filename prefix
1034     prefix = strtok(files(f).name,'.');
1035     
1036     % If tarsi have been tracked
1037     if exist([files(f).directory,prefix,'-tt-res.mat'],'file') ...
1038         || exist([files(f).directory,prefix,'-res-tt.mat'],'file') ...
1039         || exist([files(f).directory,prefix,'-res.mat'],'file')  
1040       
1041       % Load tarsus track
1042       if exist([files(f).directory,prefix,'-tt-res.mat'],'file')
1043         disp(['Loading ',files(f).directory,prefix,'-tt-res.mat']);
1044         load([files(f).directory,prefix,'-tt-res.mat'],'res');
1045       elseif exist([files(f).directory,prefix,'-res-tt.mat'],'file')
1046         disp(['Loading ',files(f).directory,prefix,'-res-tt.mat']);
1047         load([files(f).directory,prefix,'-res-tt.mat'],'res');
1048       elseif exist([files(f).directory,prefix,'-res.mat'],'file')
1049         disp(['Loading ',files(f).directory,prefix,'-res.mat']);
1050         load([files(f).directory,prefix,'-res.mat'],'res');
1051       else
1052         continue;
1053       end
1054       
1055       % Load result from srcAutoTrack
1056 %       disp(['Loading ',files(f).directory,prefix,'-roach-track.mat . .']);
1057 %       load([files(f).directory,prefix,'-roach-track.mat'],'roach');
1058 
1059       % Store tarsi data in useful data structure
1060       tarsus(f).pthX = res.pthX;
1061       tarsus(f).pthY = res.pthY;
1062 %       tarsus(f).loc = roach.position * [1; i];
1063 %       tarsus(f).ang = roach.angle;
1064       
1065     end % If tarsi have been tracked
1066     
1067   end % Loop through files
1068   
1069   % Don't consider files that didn't have appropriate backpack data
1070   f = 1;
1071   while f <= length(files)
1072     if isempty(tarsus(f).pthX)
1073       disp(['Warning:  no tarsus data found for ',files(f).directory,files(f).name,'.  Removing.']);
1074       files(f) = [];
1075       impulse(f) = [];
1076       if exist('backpack','var')
1077         backpack(f) = [];
1078       end
1079       tarsus(f) = [];
1080       animal(f) = [];
1081       trials(f) = [];
1082     else
1083       f = f+1;
1084     end
1085   end
1086 
1087   
1088   do_bleach = 1;
1089 
1090 
1091   % Loop through tarsus data
1092   for f = 1:length(tarsus)
1093 
1094     if do_bleach
1095       disp(['Bleaching tarsus data . .']);
1096 
1097       % Interpolate missing track results
1098       [p,gp,t] = interpolate_tarsus_v2(tarsus(f));
1099 
1100     else
1101       p = tarsus(f).pthX + i * tarsus(f).pthY;
1102       %     p(diff(real(p))==0) = nan;
1103       p = p.';
1104       gp = p;
1105       t = 1:size(p,2);
1106     end
1107 
1108     % Store data
1109     tarsus(f).p = p;
1110     tarsus(f).gp = gp;
1111     tarsus(f).t = t;
1112     
1113     % Create RoachRunStruct
1114     if exist([files(f).directory,files(f).name,'-roach-track.mat'],'file')
1115       disp(['Creating RoachRunStruct . .']);
1116       load([files(f).directory,files(f).name,'-roach-track.mat'],'roach');
1117       rr = [rr;cpRoachRunStruct(tarsus(f),[],impulse(f).kalman,roach,[],[files(f).directory,'/',files(f).name])];
1118     end
1119   
1120 %     trials = 1:length(files);
1121     legs = 1:6;
1122     
1123     % Compute baseline with period roughly 4-5 strides
1124     [B, A] = butter(3, 0.012);
1125     for d = legs
1126       tarsus(f).b(d,:) = filtfilt(B, A, tarsus(f).gp(d,:));
1127     end
1128     
1129     % Create a Butterworth filter with specified order and cutoff frequency
1130     [B, A] = butter(2, 0.3);
1131       
1132     % Apply a really weak low-pass filter to data
1133     for d = legs
1134       tarsus(f).gpf(d,:) = filtfilt(B, A, tarsus(f).gp(d,:)-tarsus(f).b(d,:));
1135     end    
1136 
1137   end
1138 
1139   disp(['Saving this increment of analysis to ',dataset,'_tarsus.mat']);
1140   if exist(['data/',dataset,'_tarsus.mat'],'file')
1141     system(['mv data/',dataset,'_tarsus.mat data/',dataset,'_tarsus.mat.bak']);
1142   end
1143   save(['data/',dataset,'_tarsus.mat'])
1144   
1145   disp(['Saving RoachRunStruct to ',dataset,'_roachrun.mat']);
1146   if exist(['data/',dataset,'_roachrun.mat'],'file')
1147     system(['mv data/',dataset,'_roachrun.mat data/',dataset,'_roachrun.mat.bak']);
1148   end
1149   save(['data/',dataset,'_roachrun.mat'],'rr')
1150     
1151   
1152 end
1153 
1154 if plot_tarsus
1155   
1156   legs = 1:6;
1157 
1158   for f = 1:length(tarsus)
1159     
1160     close all;
1161     
1162     % Raw tarsus data
1163     figure;
1164 %     clf;
1165 %     if include_y, subplot(2,1,1), end
1166     hold on;
1167 %     plot(tarsus(f).pthX(tarsus(f).t,legs),'.')
1168 %     plot(real(tarsus(f).p(legs,:))')
1169     plot(real(tarsus(f).p(legs,:))','.')
1170     plot(tarsus(f).gp(legs,:)',':')
1171     plot(tarsus(f).b(legs,:)','--');
1172     plot((tarsus(f).gpf(legs,:)+tarsus(f).b(legs,:))')
1173 %     if do_gpr
1174 %       plot(real(tarsus(f).gp2(legs,:))','x')
1175 %     end
1176     xlabel('Sample #','FontName','Times','FontSize',22);
1177     ylabel('Tarsus {x}-position','FontName','Times','FontSize',22);
1178     title([dataset,'/',files(f).name],'FontName','Times','FontSize',24);
1179     set(gca,'FontName','Times','FontSize',22);
1180 
1181 %     if include_y
1182 %       subplot(2,1,2)
1183 %       hold on;
1184 %       plot(tarsus(f).pthY(tarsus(f).t,legs),'.')
1185 %       plot(imag(tarsus(f).p(legs,:))','g')
1186 %       plot(imag(tarsus(f).p(legs,:))','g.')
1187 %       ylabel('$y$ trajectory');
1188 %     end
1189 %
1190 %     leglegend = {};
1191 %     for k = 1:length(legs)
1192 %       leglegend = {leglegend{:}, num2str(legs(k))};
1193 %     end
1194 %     legend(leglegend);
1195 
1196     drawnow;
1197 %     keyboard
1198 
1199   
1200     if save_plots
1201       saveas(gca,['fig/',dataset,'/fig/tarsus',num2str(f),'.fig'],'fig');
1202       saveas(gca,['fig/',dataset,'/eps/tarsus',num2str(f),'.eps'],'eps');
1203       saveas(gca,['fig/',dataset,'/tarsus',num2str(f),'.png'],'png');
1204     end
1205   end
1206 end
1207 
1208 
1209 if 0 & ~do_phase && do_classification && exist(['data/',dataset,'_phaser.mat'])
1210   disp('Loading phaser data from file . .');
1211   load(['data/',dataset,'_phaser.mat'],'phase','tarsus','impulse','backpack','dat', ...
1212        'phip','phase_data','phase_impulse_frame','good','windowend');
1213 end
1214 
1215 if do_phase && overwrite
1216   
1217   % Initialize tarsus track struct
1218   phaser = struct('pthX',[],'pthY',[]);
1219   
1220   legs = 1:6;
1221   
1222   disp('Loading tarsus data from file . .');
1223   load(['data/',dataset,'_tarsus.mat'], 'tarsus', 'files', 'impulse');
1224 
1225 %   trials = 1:length(tarsus);
1226 
1227   % Loop through files
1228   dat = {};
1229   for f = trials
1230     
1231     % Collect tarsus x-coordinate data
1232     dat{end+1} = real(tarsus(f).gpf(legs,:));
1233   end
1234   
1235   
1236   phase = struct('phi',[],'frames',[nan nan nan]);
1237   
1238   disp(['Phaser analysis for legs [',num2str(legs),']']);
1239   
1240   % Create (and train) a Phaser using GP fitted, de-trended tarsus data
1241   phr = newPhaser( dat, [], [], inline('([1,-1,1,-1,1,-1] * x).''') );
1242   
1243   % Collect phase estimates
1244   disp(['Collecting phase estimates . . ']);
1245   for f = trials
1246     phase(f).phi = feval( phr.phi, phr, dat{find(trials==f)} )';
1247   end
1248 
1249   % Fit Fourier series to phases
1250   ord = 5;
1251   disp(['Fitting order ',num2str(ord),' Fourier series to phase estimates . . ']);
1252   for f = trials
1253     phase(f).fc = ftFit( ord, phase(f).phi', dat{find(trials==f)}' );
1254   end
1255   
1256   % Evaluate Fourier series at a variety of phases
1257   disp(['Evaluating Fourier series . . ']);
1258   for f = trials
1259     phase(f).ft = ftVal(phase(f).fc, phase(f).phi');
1260   end
1261   
1262   
1263   % Get sample corresponding to perturbation, corresponding phase
1264   tp = nan * zeros(1,length(files));
1265   phip = nan * zeros(1, length(files));
1266   for f = trials
1267     tp(f) = find(tarsus(f).t == impulse(f).frames(2));
1268     phip(f) = mod(phase(f).phi(tp(f)),2*pi);
1269     
1270     phase(f).frames = [min(tarsus(f).t), impulse(f).frames(2), max(tarsus(f).t)];
1271   end
1272   
1273   % Remove unwanted trials
1274   for f = setdiff(1:length(phase),trials)
1275     phase(f).phi = [];
1276   end
1277   
1278   % Collect phase data into one big array
1279   [phase_data, phase_impulse_frame] = oneBigArray(phase, 'phi');
1280 %   phase_data = mod(phase_data,2*pi);
1281 
1282   
1283   % Remove rows of NaN's
1284   good = ~all(isnan(phase_data),2);
1285   
1286   % Find nearest data start and end point
1287   windowstart = find(isnan(mean(phase_data(good,:))) & (1:size(phase_data,2)) < phase_impulse_frame,1);
1288   windowend = find(isnan(mean(phase_data(good,:))) & (1:size(phase_data,2)) > phase_impulse_frame,1);
1289 
1290   
1291 
1292   disp(['Saving to ',dataset,'_phaser.mat . .']); 
1293   if exist(['data/',dataset,'_phaser.mat'],'file')
1294     system(['mv data/',dataset,'_phaser.mat data/',dataset,'_phaser.mat.bak']);
1295   end
1296   save(['data/',dataset,'_phaser.mat'])
1297   
1298 end
1299 
1300 if plot_phase
1301   
1302 %   trials = 1:length(files);
1303 
1304   for f = trials
1305     
1306     % Phase data for tarsus
1307     figure(f+length(phase));
1308     hold on;
1309     plot(mod(phase(f).phi,2*pi), dat{find(trials==f)}, '.');
1310     plot(mod(phase(f).phi,2*pi), phase(f).ft);
1311     xlabel('Sample #','FontName','Times','FontSize',18);
1312     ylabel('Phase (radians)','FontName','Times','FontSize',18);
1313     title([dataset,'/',files(f).name],'FontName','Times','FontSize',18);
1314     set(gca,'FontName','Times','FontSize',14);
1315 
1316     if save_plots
1317       saveas(gca,['fig/',dataset,'/fig/phaser_sanity-check',num2str(f),'.fig'],'fig');
1318       saveas(gca,['fig/',dataset,'/eps/phaser_sanity-check',num2str(f),'.eps'],'eps');
1319       saveas(gca,['fig/',dataset,'/phaser_sanity-check',num2str(f),'.png'],'png');
1320     end
1321   end
1322 end
1323 
1324 
1325 
1326 if ~do_classification && exist(['data/',dataset,'_classification.mat'])
1327   disp('Loading classification data from file . .');
1328   load(['data/',dataset,'_classification.mat'],'phase','tarsus','impulse','backpack','dat', ...
1329        'phip','phase_data','phase_impulse_frame','good','windowend','phit','e', ...
1330        'Nwin','Nrands','pdata','offset','phic');
1331 end
1332 
1333 if do_classification && overwrite
1334   
1335   disp('Loading phaser data from file . .');
1336   load(['data/',dataset,'_phaser.mat'],'phase_data','phase_impulse_frame','trials');
1337   
1338   load(['data/',dataset,'_impulse.mat'],'conf_int');
1339   
1340   % Remove rows of NaN's
1341   good = ~all(isnan(phase_data),2);
1342   
1343   % Samples per stride
1344   stride = 50;
1345   
1346   % Number of strides to apply robust fit to
1347   nrfit = 2;
1348   
1349   % De-trend line fit
1350   detrend = [];
1351   
1352   % Loop through trials
1353 %   trials = find(good);
1354   for f = 1:size(phase_data,1)
1355     if good(f)
1356       % Find nearest data start and end point
1357       ds(f) = find(isnan(phase_data(f,:)) & (1:size(phase_data,2)) < phase_impulse_frame,1,'last');
1358       de(f) = find(isnan(phase_data(f,:)) & (1:size(phase_data,2)) > phase_impulse_frame,1);
1359     else
1360       ds(f) = nan;
1361       de(f) = nan;
1362     end
1363   end
1364   
1365   good = (conf_int(1) - ds > nrfit*stride) & (de - phase_impulse_frame >= 1);
1366   
1367   for f = 1:size(phase_data,1)
1368     if good(f)
1369       samples = (conf_int(1)-nrfit*stride):(conf_int(1));
1370       detrend(f,:) = robustfit(samples,phase_data(f,samples));
1371     else
1372       detrend(f,:) = [nan,nan];
1373     end
1374   end
1375   
1376   if 0
1377     close all;
1378     figure;
1379     hold on;
1380     plot(phase_data')
1381     plot(phase_impulse_frame*[1,1],5*[-1,1],'k--')
1382 
1383     keyboard
1384   end
1385   
1386   % Detrend phase
1387   phi = phase_data;
1388   phires = phi - (detrend(:,2)*(1:size(phase_data,2)) + detrend(:,1)*ones(1,size(phase_data,2)));
1389   
1390   % Convert phase residual to complex numbers
1391   % (we average complex number representations of rotations;
1392   %  it doesn't make sense to average angles directly)
1393 %   phires = exp(i*phires);
1394   
1395 %   keyboard
1396   
1397   if 0
1398     close all;
1399     figure;
1400     hold on;
1401 %     plot(unwrap((angle(phires)+repmat(mod(phi(:,phase_impulse_frame),2*pi),1,size(phires,2)))'))
1402     plot(unwrap((angle(phires))'))
1403     plot(phase_impulse_frame*[1,1],5*[-1,1],'k--')
1404 
1405     xlabel('sample')
1406     ylabel('residual phase')
1407 
1408     keyboard
1409   end
1410   
1411   phit = linspace(0, pi, 180);
1412   e = zeros(size(phit));
1413   window = phase_impulse_frame:min(de(de > 0))-1;
1414   Nwin = window(end)-window(1)+1;  
1415   
1416   Nrands = 1000;
1417   
1418   disp(['Classifying phase at perturbation for ',num2str(Nrands),' randomized datasets'])
1419   
1420   % Restrict residuals to window following perturbation
1421   phires = phires(:,window);
1422 
1423   if 0
1424     figure;
1425     hold on;
1426 
1427     q = [];
1428     
1429     phioffset = 2*pi*rand(25,1);
1430     phioffset = 0*phioffset;
1431 
1432     for k = 1:300
1433 
1434       trials = ceil(rand(1,25)*size(phires,1));
1435 
1436       % Trials to include
1437 %       inc = ~isnan(phi(trials,phase_impulse_frame));
1438       inc = good(trials);
1439 
1440       [c,q(k,:),Q] = phaseClass(phi(trials,phase_impulse_frame)+phioffset, phires(trials,:), inc);
1441       q(k,:) = q(k,:) - mean(q(k,:));
1442 
1443     end
1444 
1445     %   plot(q')
1446 
1447     plot(mean(q));
1448     plot(mean(q)+std(q),'--');
1449     plot(mean(q)-std(q),'--');
1450     
1451     keyboard
1452     
1453     return;
1454   end
1455   
1456 %   keyboard
1457 
1458   % Create matrix of random phase offsets
1459   offset = 2*pi*rand(size(phi,1),Nrands+1);
1460   offset(:,1) = 0;
1461   
1462   % Initialize matrix of classification qualities
1463   q = [];
1464   
1465 %   keyboard
1466   
1467   tic
1468 
1469   % Loop through perturbation phase randomizations
1470   for n = 1:Nrands+1
1471     
1472     [c,q(n,:)] = phaseClass(phi(:,phase_impulse_frame)+offset(:,n), phires, good);
1473 %     q(n,:) - mean(q(n,:));
1474     
1475     % Store classification for real data
1476     if n == 1
1477       class = c;
1478     end
1479   end
1480   
1481   toc
1482   
1483   phic = phit(round(median(find(q(1,:) == max(q(1,:))))));
1484   disp(['Classification phase = ',num2str(phic),' rad']);
1485 
1486   disp(['Saving to ',dataset,'_classification.mat . .']); 
1487   if exist(['data/',dataset,'_classification.mat'],'file')
1488     system(['mv data/',dataset,'_classification.mat data/',dataset,'_classification.mat.bak']);
1489   end
1490   save(['data/',dataset,'_classification.mat'])
1491   
1492 %   keyboard
1493   
1494 end
1495 
1496 
1497 
1498 if plot_classification
1499   
1500   load(['data/',dataset,'_classification.mat'],'q','c0','Nrands');
1501   
1502   figure;
1503   hold on;
1504   plot((1:size(q,2))*180/size(q,2),q(1,:),'k','LineWidth',4);
1505   xlabel('Partition angle (degrees)', 'FontSize', 18,'FontName','Times');
1506   ylabel('Quality of Classification','FontSize',18,'FontName','Times');
1507 
1508   plot((1:size(q,2))*180/size(q,2),q(2:end,:),'color',0.75*[1,1,1]);
1509 
1510   ylim = get(gca,'YLim');
1511   set(gca,'XLim',[0,180],'FontSize',14,'FontName','Times');
1512   title([dataset,' ',num2str(Nrands),' random offsets'],'FontSize',18,'FontName','Times');
1513     
1514   legend({'Coherent at Perturbation','Incoherent at Perturbation'}, 'Location','SouthWest')
1515     
1516   if save_plots
1517     saveas(gca,['fig/',dataset,'/fig/phase_classification_',num2str(Nrands),'.fig'],'fig');
1518     saveas(gca,['fig/',dataset,'/eps/phase_classification_',num2str(Nrands),'.eps'],'eps');
1519     saveas(gca,['fig/',dataset,'/phase_classification_',num2str(Nrands),'.png'],'png');
1520   end
1521   
1522   % Compute empirical distribution function
1523   [F,X] = ecdf(max(q,[],2));
1524 
1525   figure;
1526   hold on;
1527   plot(X,F,'LineWidth',4)
1528   ylim = get(gca,'YLim');
1529   plot(max(q(1,:))*[1,1],ylim,'k--','LineWidth',4);
1530   xlabel('Minimum classification error', 'FontSize', 18,'FontName','Times');
1531   ylabel('Empirical Probability of Minimum','FontSize',18,'FontName','Times');
1532 
1533   pvalue = (length(find(max(q,[],2) >= max(q(1,:))))-1)/Nrands;
1534   disp(['P-value = ',num2str(pvalue)]);
1535 
1536   set(gca,'FontSize',14,'FontName','Times');
1537   title([dataset,' EDF of classification error, N = ',num2str(Nrands), ', P = ',num2str(pvalue)],'FontSize',18,'FontName','Times');
1538 
1539   if save_plots
1540     saveas(gca,['fig/',dataset,'/fig/phaser_classification_edf_',num2str(Nrands),'.fig'],'fig');
1541     saveas(gca,['fig/',dataset,'/eps/phaser_classification_edf_',num2str(Nrands),'.eps'],'eps');
1542     saveas(gca,['fig/',dataset,'/phaser_classification_edf_',num2str(Nrands),'.png'],'png');
1543   end
1544 end
1545 
1546 keyboard
1547 
1548 
1549 
1550 % Vectorized creation of rotation matrices from pitch, roll, yaw
1551 %   x is 3xN
1552 %   x(1,:) is pitch
1553 %   x(2,:) is roll
1554 %   x(3,:) is yaw
1555 %
1556 %   R is 3x3xN
1557 %   R(:,:,n) is the n^th 3x3 rotation matrix
1558 function R = rotationMatrix( x )
1559 % Problem dimensions
1560 N = size(x,2);
1561 
1562 % Trig functions of rotation angles
1563 C1=cos(x(1,:)); C2=cos(x(2,:)); C3=cos(x(3,:));
1564 S1=sin(x(1,:)); S2=sin(x(2,:)); S3=sin(x(3,:));
1565 
1566 % Vectorized creation of Rx*Ry*Rz
1567 R=[ C3.*C2; S3.*C1+C3.*S2.*S1; S3.*S1-C3.*S2.*C1;...
1568     -S3.*C2; C3.*C1-S3.*S2.*S1; C3.*S1+S3.*S2.*C1;...
1569     S2;           -C2.*S1;            C2.*C1 ];
1570 
1571 % Reshape to proper dimensions
1572 R = reshape( R, [3,3,N] );
1573 
1574 
1575 function h = trimJumps( h )
1576 dh = abs(diff(h));
1577 dh = find(dh>mean(dh)+3*std(dh));
1578 if ~isempty(dh)
1579   h(dh)=nan;
1580   h(dh+1)=nan;
1581   h = fillNans(h);
1582 end
1583 
1584 
1585 function [cm,p] = unskip( cm, p )
1586 sk = abs(diff(cm));
1587 sk = find(sk-mean(sk)>3*std(sk));
1588 if isempty(sk)
1589   return;
1590 end
1591 cm0=[cm(1),diff(cm)];
1592 cm0(1+sk)=0;
1593 cm0 = cumsum(cm0);
1594 if nargout>1
1595   p = p - repmat( cm-cm0, size(p,1), 1);
1596 end
1597 cm = cm0;
1598 
1599 
1600 % Interpolate missing results in tarsus track
1601 function [pos, gppos, trange] = interpolate_tarsus_v2(res, trange)
1602 % [pos, trange] = interpolate_tarsus_v2
1603 %
1604 % Inputs:
1605 % res - tarsus tracking results from res.mat
1606 % trange - time range to inerpolate (defaults to entire trajectory)
1607 %
1608 % Outputs:
1609 % pos - positions in video coordinates (pixels)
1610 % gppos - positions with nan's filled via Gaussian process regression
1611 % trange - time range for valid data (frames)
1612 %
1613 % by Sam Burden, Berkeley 2009
1614 
1615 % Number of data points is the minimum over each data set
1616 N = min([size(res.pthX,1), size(res.pthY,1)]);
1617 
1618 % If time range wasn't specified, include everything
1619 if nargin < 2
1620   trange = 20:(N-20);
1621 
1622 % Otherwise, parse input
1623 else
1624   if length(trange) == 2
1625     trange = [trange(1):trange(2)];
1626   end
1627   
1628   % Saturate time range to avoid errors
1629   trange = trange(trange > 0);
1630   trange = trange(trange <= N);
1631 end
1632 
1633 % Only consider certain columns of data
1634 legs = 1:6;
1635 res.pthX = res.pthX(:,legs);
1636 res.pthY = res.pthY(:,legs);
1637 
1638 % Interpret consecutive constant datapoints as not tracked
1639 not_tracked = diff(res.pthX(trange,:)) == 0 & diff(res.pthY(trange,:)) == 0;
1640 
1641 % Create a list of all the 'bad' datapoints
1642 bad = [isnan(res.pthX(trange,:)),isnan(res.pthY(trange,:)), ...
1643        [false;any(not_tracked,2)] ];
1644 
1645 % Restrict to frames between first and last fully tracked points
1646 if sum(any(bad,2))>1
1647   good = find(all(~bad,2));
1648   trange = trange(min(good):max(good));
1649 end
1650 
1651 % Collect data into a single array of complex numbers
1652 X0 = res.pthX(trange,:) + i*res.pthY(trange,:);
1653 
1654 % Interpret consecutive constant x datapoints as not tracked
1655 X0([false(0,size(X0,2));diff(X0)==0]) = nan;
1656 
1657 % Store data
1658 pos = X0.';
1659 
1660 % Store interpolated data
1661 warning('off','optim:fminunc:SwitchingMethod');
1662 gppos = gpFillNans(real(pos)',...
1663   {'covSum', {'covTPeriodic','covNoise'} },...
1664   [128; 2.3; 36; 0.7],1)';
1665 
1666 
1667 % Collect multiple data runs from structure into one big array.
1668 % Assumes s has the following fields:
1669 %   data = [num_states] x [traj_length] array containing data to collect
1670 %   frames = [num_runs] x 3 array with length(s(i).data) = s(i).frames(3)-s(i).frames(1)+1
1671 %     and s(i).frames(2) indicating frame to match up between runs
1672 function [data,frame] = oneBigArray(s,F)
1673 
1674 if nargin < 2
1675   F = 'data';
1676 end
1677 
1678 % Determine size of returned array
1679 frames = vertcat(s(:).frames);
1680 % m = max(frames(:,2) - frames(:,1));
1681 % M = max(frames(:,3) - frames(:,2));
1682 
1683 m = max(frames(:,2));
1684 M = max(frames(:,3));
1685 
1686 % Allocate space for array
1687 k = 1;
1688 while size(s(k).(F),1) == 0
1689   k = k+1;
1690 end
1691 data = nan * zeros(length(s), m + M, size(s(k).(F),1));
1692 
1693 % Loop through runs
1694 for f = 1:length(s)
1695   % Loop through state variables
1696   for v = 1:size(data,3)
1697     if ~isempty(s(f).(F))
1698       % Collect data from run
1699       fr = (s(f).frames(1):s(f).frames(3));
1700 %       data(f,fr + m-s(f).frames(2),v) = s(f).(F)(v,fr)';
1701       data(f,fr + m-s(f).frames(2),v) = s(f).(F)(v,:)';
1702     end
1703   end
1704 end
1705 
1706 % keyboard
1707 
1708 frame = m;
1709 
1710 
1711 % Apply direct linear transformation to a collection of points.
1712 % For instance, can be used to transform from world (cm) coordinates to raw
1713 % video (pixel) coordinates
1714 function y = applyDLT(T,x)
1715 
1716 % Apply direct linear transformation
1717 y = T*[x; ones(1,size(x,2))];
1718 
1719 % Correct for change of scale
1720 y = y(1:2,:) ./ [y(3,:); y(3,:)];
1721 
1722 
1723 % Apply projective transformation to a collection of points in a Nx2 array.
1724 % For instance, can be used to transform from raw video (pixel) coordinates
1725 % to world (mm) coordinates
1726 function y = applyProjection(T,x)
1727 
1728 % Apply projective transformation
1729 y = [x, ones(size(x,1),1)]*T;
1730 
1731 % Correct for change of scale
1732 y = y(:,[1,2]) ./ y(:,[3,3]);
1733 
1734 % Add translation
1735 y = y + repmat(T(3,[1,2]),size(y,1),1);
1736 
1737 
1738 % Compute pairwise distance measurements for complex numbers in x
1739 function d = pairwiseDistances(x)
1740 
1741 x = x(:);
1742 
1743 d = repmat(x,1,size(x,1));
1744 d = triu(abs(d - d'));
1745 d = d(find(triu(ones(size(d)))));
1746 
1747 
1748 % Convert from convenient geometry pixel coordinates to cm
1749 %   x is a collection of points to transform
1750 %   m is the maximum x coordinate value in the original geometry,
1751 %     m = max(cart.geom0(:,1))
1752 function y = geompx2cm(x,m,b)
1753 
1754 if nargin < 3
1755   % Pixel boundary around the geometry
1756   % NOTE:  this value wasn't originally stored in cart data structure;
1757   % hopefully it's correct for your data!
1758   b = 50;
1759   
1760   if nargin < 2
1761     % m = max(cart.geom0(:,1));
1762     m = 29.5097;
1763   end
1764 end
1765   
1766 
1767 % Original transformation to scale g to fit a 1024 x h pixel image
1768 % cart.geom = cart.geom0.*((1024-2*border)/max(cart.geom0(:,1)));
1769 
1770 % Rescale points
1771 y = (m / (1024 - 2 * b)) .* x;