Home > polypedal > tarTr > makeResGait.m

makeResGait

PURPOSE ^

SYNOPSIS ^

function res = makeResGait( res )

DESCRIPTION ^

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 function res = makeResGait( res )
0002   extraTime = 0.5 * res.fps; % in frames
0003     % start and finish times
0004     tMin = max([1, res.bf(3) - extraTime]);
0005     tMax = min([size(res.pthX,1), res.bl(3) + extraTime]);
0006   q = all(isnan(res.pthX).' ...
0007     | [false(1,size(res.pthX,2)); diff(res.pthX)==0].' );
0008   mx = max(find(q(2:end)>q(1:end-1)));
0009   if ~isempty(mx) && mx<tMax
0010     tMax = mx;
0011   end
0012   mn = min(find(q(2:end)<q(1:end-1)));
0013   if ~isempty(mn) && mn>tMin
0014     tMin = mn;
0015   end
0016   res.tRng = [tMin,tMax];
0017   
0018   if ~isfield(res,'xv')
0019     % run Kalman filter -- skipped for now
0020     %   res.xv = filterTracks(res, [tMin:tMax] );
0021   else
0022     warning 'Reusing xv tracking data'
0023   end
0024   
0025   if nargout<1
0026         % Figure 1 is gait diagram
0027          legIdx = [3 5 1 6 2 4];
0028         legLbl = {'R1','R2','R3','L3','L2','L1'};
0029         figure(1); clf;
0030         gaitDiagram( real(res.xv(legIdx+6,:)).' );
0031         subplot( 5, 1, 1:4 );
0032         set( gca, 'YTickLabel',legLbl(legIdx));
0033         tfl = [res.bf(3); res.bl(3)]-tMin;
0034         ax=axis;
0035         PH1 = patch( tfl([1 2 2 1 1]), ax([3 3 4 4 3]), 'c' );
0036         subplot( 5, 1, 5 );
0037         ax=axis;
0038         PH2 = patch( tfl([1 2 2 1 1]), ax([3 3 4 4 3]), 'c' );
0039         set( [PH1, PH2], 'EdgeColor', [1,1,0], 'FaceAlpha', 0.5 );
0040       
0041     fn = [res.ini{1}.pth, '/gait.png'];
0042     fn = inputdlg( 'Save gait diagram as','Save',1,{fn});
0043     if ~isempty(fn)
0044       print( '-noui', '-r300', sprintf('-f%d',1), '-dpng', fn{1});
0045     end 
0046   end
0047 return 
0048 
0049 function xv = filterTracks( res, rng )
0050     Q = [repmat(6,1,6),repmat(1,1,6)];
0051 
0052   % Get roach image descriptors
0053     dcr = srcGetDcr( srcFromIni( res.ini ));
0054     
0055     % Convert to complex position
0056     x0 = res.pthX + i * res.pthY;
0057     
0058     % Mark untracked points for interpolation
0059     x0([false(0,size(x0,2));diff(x0)==0]) = nan;
0060     
0061     
0062   % Compute tracked image centroid
0063   xyCtr = dcr.sz*[i;1;0]./2;
0064 
0065   % Use l1 as the origin
0066     xy0 = res.l1 * [1;i;0];
0067 
0068     % Interpolate (linearly) all missing points
0069     x1 = fillNans(x0(rng,:))-xyCtr;
0070     ang = fillNans(res.ang(rng));
0071     loc = fillNans(res.loc(rng,:))-xy0;
0072 
0073   % Kalman smooth for position and velocity
0074      sp = newKalmanConstV( x1(1,:).',[],Q );
0075     [xv, V, VV] = kalman_smooth( x1.', sp );
0076 return

Generated on Mon 02-Aug-2010 16:44:38 by m2html © 2003