![]() |
PlotPoseSequencePURPOSE
Plots a sequence of poses.
SYNOPSIS
function h=PlotPoseSequence(S,color,arrows)
DESCRIPTION
Plots a sequence of poses. Plots a cell array of poses representing a trajectory (either ground truth trajectories or noisy trajectories resulting from the integration of sensor readings). Parameters S: The cell array with the sequence of poses. color: The color for the line representing the trajectory. arrows: True if an arrow has to be added to each pose representing the orientation. Returns the handler of the line representing the sequence of poses. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function h=PlotPoseSequence(S,color,arrows) 0002 % Plots a sequence of poses. 0003 % 0004 % Plots a cell array of poses representing a trajectory (either ground 0005 % truth trajectories or noisy trajectories resulting from the integration 0006 % of sensor readings). 0007 % 0008 % Parameters 0009 % S: The cell array with the sequence of poses. 0010 % color: The color for the line representing the trajectory. 0011 % arrows: True if an arrow has to be added to each pose representing 0012 % the orientation. 0013 % 0014 % Returns the handler of the line representing the sequence of poses. 0015 0016 n=size(S,2); 0017 0018 X=zeros(1,n); 0019 Y=zeros(1,n); 0020 0021 for i=1:n 0022 X(i)=get(S{i},'x'); 0023 Y(i)=get(S{i},'y'); 0024 end 0025 0026 hold on 0027 h=line(X,Y); 0028 set(h, 'marker','o' ,'markersize',3); %3 0029 set(h, 'color',color); 0030 0031 if (arrows) 0032 d=0; 0033 for i=2:n 0034 X(i)=get(S{i},'x'); 0035 Y(i)=get(S{i},'y'); 0036 d=d+norm(X(i)-X(i-1),Y(i)-Y(i-1)); 0037 end 0038 d=d/(n-1); % average displacement (used to plot orientation arrows of 0039 % a reasonable size) 0040 0041 for i=1:n 0042 o=get(S{i},'orientation'); 0043 ax=d*cos(o); 0044 ay=d*sin(o); 0045 h=quiver(X(i),Y(i),ax,ay,0); %Arrow with the robot orientation at each pose 0046 set(h, 'color',color); 0047 end 0048 end 0049 0050 hold off |