doRes2Gait

 do_gait for res

0001 % do_gait for res
0002 gspec = getGlobalSpec;
0003 calibLen = gspec.lengthCalib; % in cm
0004 extraTime = 0.5 * res.fps; % in frames
0005 Q = [repmat(6,1,6),repmat(1,1,6)];
0006 legIdx = [3 5 1 6 2 4];
0007 
0008 if exist('xv')==0
0009     % Get roach image descriptors
0010     dcr = srcGetDcr( srcFromIni( res.ini ));
0011     
0012     % Convert to complex position
0013     x0 = res.pthX + i * res.pthY;
0014     
0015     % Mark untracked points for interpolation
0016     x0([false(0,size(x0,2));diff(x0)==0]) = nan;
0017     
0018     % start and finish times
0019     tMin = max([1, res.bf(3) - extraTime]);
0020     tMax = min([size(x0,1), res.bl(3) + extraTime]);
0021     
0022   % Compute tracked image centroid
0023   xyCtr = dcr.sz*[i;1;0]./2;
0024 
0025   % Use l1 as the origin
0026     xy0 = res.l1 * [1;i;0];
0027 
0028     % Interpolate (linearly) all missing points
0029     x1 = fillNans(x0(tMin:tMax,:))-xyCtr;
0030     ang = fillNans(res.ang(tMin:tMax));
0031     loc = fillNans(res.loc(tMin:tMax,:))-xy0;
0032 
0033   % Kalman smooth for position and velocity
0034      sp = newKalmanConstV( x1(1,:).',[],Q );
0035     [xv, V, VV] = kalman_smooth( x1.', sp );
0036 end
0037 
0038 % Figure 1 is gait diagram
0039 figure(1); clf;
0040 gaitDiagram( real(xv(legIdx+6,:)).' );
0041 subplot( 5, 1, 1:4 );
0042 legLbl = {'R1','R2','R3','L3','L2','L1'};
0043 set( gca, 'YTickLabel',legLbl(legIdx));
0044 tfl = [res.bf(3); res.bl(3)]-tMin;
0045 ax=axis;
0046 PH1 = patch( tfl([1 2 2 1 1]), ax([3 3 4 4 3]), 'c' );
0047 subplot( 5, 1, 5 );
0048 ax=axis;
0049 PH2 = patch( tfl([1 2 2 1 1]), ax([3 3 4 4 3]), 'c' );
0050 
0051 set( [PH1, PH2], 'EdgeColor', [1,1,0], 'FaceAlpha', 0.5 );
0052 return
0053 
0054 
0055 return
0056 
0057 
0058 % !!!! BROKEN - bv changed to beltV
0059 % Figure 2 is path in belt coordinates
0060 figure(2); clf;
0061 
0062 % Rotation dilatation from camera to mm
0063 scl = calibLen ./ (res.l2*[1;i;0] - xy0);
0064 
0065 % Tarsus positions in camera coord, pixels, xyOrig as 0,0
0066 c2 = xv(1:6,:).'.*repmat(exp(-i*ang),1,6) + repmat(loc,1,6);
0067 
0068 bLen = norm(diff(res.bodyAx));
0069 bv = (res.bv2-res.bv1);
0070 bv = bv(1:2)*[1;i]./bv(3);
0071 bax = exp(-i*ang)*bLen;
0072 b2 = [loc-bax/2,loc+bax/2, loc];
0073 %!!! convert to mm/sec, rel to l1-->l2
0074 c3 = c2 + repmat(bv .* [0:tMax-tMin].',1,6);
0075 bx3 = b2 + repmat(bv .* [0:tMax-tMin].',1,3);
0076 fprintf( 'Belt moving at %g pix/fr == %g cm/sec\n', bv, bv * res.fps * abs(scl) );
0077 
0078 clf; plot(bx3); axis tight; axis equal; ax = axis; 
0079 
0080 for k=1:size(b2,1)-4; 
0081   clf; plot(c3(k+[0:4],:),'.'); 
0082   hold on; plot([bx3(k+[0:4],1:2).';repmat(nan,1,5)],'-k'); 
0083   plot(bx3(:,3),'b:');
0084   axis(ax); drawnow; pause(0.1); 
0085 end