Institut de Robòtica i Informàtica Industrial

PlotPoseSequence

PURPOSE ^

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:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • get Get function for EKF filters.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • set Set functino for PoseData objects.
  • get Get function for Gaussians.
  • size Size (rows/columns) of an interval matrix.
  • get Get function for poses.
  • size Number of parameters of the pose.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
  • size Number of readings stored in the Sensor.
This function is called by:

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


Institut de Robòtica i Informàtica Industrial

Generated on Fri 24-Jul-2009 12:32:50 by m2html © 2003