plot.m 5.05 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
function varargout = plot(this, varargin)
% Plot routine for itaOptitrack objects
% 
% % options: 'stepSize' ... only display every stepSize frame during animation [double]
%
% <ITA-Toolbox>
% This file is part of the ITA-Toolbox. Some rights reserved.
% You can find the license for this m-file in the license.txt file in the ITA-Toolbox folder.
% </ITA-Toolbox>

sArgs          = struct('stepSize',7);
sArgs          = ita_parse_arguments(sArgs,varargin,1);
stepSize       = sArgs.stepSize; % step size for view/up animation

time = linspace(0,this.info.TotalFrames/this.info.CaptureFrameRate,this.info.TotalFrames);

hFig = figure('units','normalized','outerposition',[0 0 1 1]);
qFig1 = gobjects(this.numRigidBodies,1);
qFig2 = qFig1;
pFig4 = qFig1;
pFigt4 = qFig1;
hl1 = qFig1;
hl2 = qFig1;

vu = zeros(this.info.TotalFrames,6,this.numRigidBodies);
for jdx = 1:this.numRigidBodies
    vu(:,:,jdx) = this.data(jdx).orientation.vu;
end

numColumns=4;
for idx=1:this.numRigidBodies
    
    % initial orientation
    subplot(this.numRigidBodies,numColumns,numColumns*(idx-1)+1)
    plot(time,this.data(idx).orientation.roll_deg)
    hold on
    plot(time,this.data(idx).orientation.pitch_deg,'r')
    plot(time,this.data(idx).orientation.yaw_deg,'g')
    title(['Orientation ',this.data(idx).rigidBodyName])
    xlabel('Time in [sec]')
    ylabel('Orientation in [deg]')
    grid on
Florian Pausch's avatar
Florian Pausch committed
43
    axis square
44 45 46 47 48
    
    % plot time cursor
    minvalOri = min([this.data(idx).orientation.roll_deg; this.data(idx).orientation.pitch_deg; this.data(idx).orientation.yaw_deg]);
    maxvalOri = max([this.data(idx).orientation.roll_deg; this.data(idx).orientation.pitch_deg; this.data(idx).orientation.yaw_deg]);
    hl1(idx) = line([0 0],[minvalOri maxvalOri],'color',[.5 .5 .5]);
49
    legend('Roll','Pitch','Yaw','Current Time Frame')
50 51 52 53 54 55 56 57 58 59 60 61 62 63
    
    % initial vu animation
    subplot(this.numRigidBodies,numColumns,numColumns*(idx-1)+2);
    % plot transparent sphere
    r = 1;
    [x,y,z] = sphere(20);
    x = x*r;
    y = y*r;
    z = z*r;
    
    lightGrey = 0.85*[1 1 1];
    s=surface(x,y,z,'FaceColor', 'none','EdgeColor',lightGrey);
    alpha(s,0.3)
    hold on
Florian Pausch's avatar
Florian Pausch committed
64
        
65 66
    axis([-r r -r r -r r])
    view([-1 1 1])
Florian Pausch's avatar
Florian Pausch committed
67 68
    axis vis3d

69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
    hold on
    
    qFig1(idx)=quiver3(0,0,0,vu(1,1,idx),vu(1,3,idx),vu(1,2,idx),'color','r','maxheadsize',.5,'linewidth',5);
    xlabel('x'),
    ylabel('z'),
    zlabel('y');
    hold on;

    qFig2(idx)=quiver3(0,0,0,vu(1,4,idx),vu(1,6,idx),vu(1,5,idx),'color','g','maxheadsize',.5,'linewidth',5);
    title(['View/Up vector ',this.data(idx).rigidBodyName])
    
    % initial position
    subplot(this.numRigidBodies,numColumns,numColumns*(idx-1)+3)
    plot(time,this.data(idx).position.x)
    hold on
    plot(time,this.data(idx).position.y,'r')
    plot(time,this.data(idx).position.z,'g')
    title(['Position ',this.data(idx).rigidBodyName])
    xlabel('Time in [sec]')
    ylabel('Position in [m]')
    grid on
Florian Pausch's avatar
Florian Pausch committed
90
    axis square
91 92 93 94 95 96 97 98
    
    % plot time cursor
    xmin=min(this.data(idx).position.x); xmax=max(this.data(idx).position.x);
    ymin=min(this.data(idx).position.y); ymax=max(this.data(idx).position.y);
    zmin=min(this.data(idx).position.z); zmax=max(this.data(idx).position.z);
    minvalPos = min([xmin;ymin;zmin]);
    maxvalPos = max([xmax;ymax;zmax]);
    hl2(idx) = line([0 0],[minvalPos maxvalPos],'color',[.5 .5 .5]);
Florian Pausch's avatar
Florian Pausch committed
99
    legend('Roll','Pitch','Yaw','Current Time Frame')
100 101 102 103 104 105 106 107 108 109 110 111 112
    
    % initial position animation
    subplot(this.numRigidBodies,numColumns,numColumns*(idx-1)+4);
    
    pFig4(idx)=plot3(this.data(idx).position.x(1),this.data(idx).position.y(1),this.data(idx).position.z(1),'Marker','o','MarkerSize',10,'LineWidth',1.5);
    set(gca,'Xlim',[xmin-0.3,xmax+0.3], 'Zlim',[zmin-0.3,zmax+0.3])
    grid on;
    view(0,180)
    axis vis3d   
    title(['Position ',this.data(idx).rigidBodyName])
    pFigt4(idx)=text(xmin-0.25,ymin,zmin-0.25,sprintf('y=%.3f m',round(this.data(idx).position.y(1)*1000)/1000));
    xlabel('x')
    zlabel('z')
Florian Pausch's avatar
Florian Pausch committed
113
    axis square
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147

end

%% play vu animation

if stepSize>1    
   fprintf('[itaOptitrack.plot] Only every %d frame is displayed in animation!\n',stepSize)
end

for idx=2:stepSize:this.info.TotalFrames
    for idx2=1:this.numRigidBodies
        
        if ~isnan(vu(idx,1,idx2))
            
            qFig1(idx2).UData = vu(idx,1,idx2);
            qFig1(idx2).VData = vu(idx,3,idx2);
            qFig1(idx2).WData = vu(idx,2,idx2);
            
            qFig2(idx2).UData = vu(idx,4,idx2);
            qFig2(idx2).VData = vu(idx,6,idx2);
            qFig2(idx2).WData = vu(idx,5,idx2);
            
            pFig4(idx2).XData = this.data(idx2).position.x(idx);
            pFig4(idx2).YData = this.data(idx2).position.y(idx);
            pFig4(idx2).ZData = this.data(idx2).position.z(idx);
            
            pFigt4(idx2).String = sprintf('y=%.3f m',round(this.data(idx2).position.y(idx)*1000)/1000);
            
            hl1(idx2).XData = [time(idx) time(idx)];
            hl2(idx2).XData = [time(idx) time(idx)];
            
        end
        
    end
148 149
    
    drawnow
150 151 152 153 154 155 156 157 158
end

if nargout
    varargout = {hFig};
else
    varargout = {};
end

end