![]() |
InitFilterPlotPURPOSE
Initializes the plot of a filter.
SYNOPSIS
function p=InitFilterPlot(F,Parameters)
DESCRIPTION
Initializes the plot of a filter. Initializes the plot of the trajectory stored in a filter, F. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function p=InitFilterPlot(F,Parameters) 0002 % Initializes the plot of a filter. 0003 % 0004 % Initializes the plot of the trajectory stored in a filter, F. 0005 0006 if Parameters.plot 0007 p.h=gcf; 0008 clf; 0009 rangeX=double(Parameters.Domain(1)); 0010 rangeY=double(Parameters.Domain(2)); 0011 0012 axis([rangeX rangeY]); 0013 0014 axis xy equal manual; 0015 0016 hold on; 0017 0018 % Get the Gaussian of the first pose 0019 G=F.initEstimation; 0020 0021 % X-Y for the ellipse represnting the Gaussian 0022 p.poseXY=get(G,'ellipse'); 0023 p.pose=line(p.poseXY(1,:),p.poseXY(2,:)); % create the ellipse representing the 0024 % current pose 0025 set(p.pose, 'color','k'); 0026 0027 % an point for the beginning of the trajectory 0028 pm=get(G,'mean'); 0029 p.traceX=[pm(1) pm(1)]; 0030 p.traceY=[pm(2) pm(2)]; 0031 p.trace=line(p.traceX,p.traceY); % create the line representing the trajectory 0032 set(p.trace, 'marker','.' ,'markersize',15,'color','r'); 0033 0034 p.last=line([pm(1) pm(1)],[pm(2) pm(2)]); 0035 0036 p.robotPoints=[[-0.5 0.5 -0.5 -0.5];[0.25 0 -0.25 0.25];[1 1 1 1]]; 0037 0038 T=[[1 0 pm(1)];[0 1 pm(2)];[0 0 1]]*[[cos(pm(3)) -sin(pm(3)) 0];[sin(pm(3)) cos(pm(3)) 0];[0 0 1]]; 0039 rp=T*p.robotPoints; 0040 p.robot=line(rp(1,:),rp(2,:)); 0041 set(p.robot,'color','b','linewidth',2); 0042 0043 % a hidden line to be used to show attempts to close loops 0044 p.loopTest=line([0 0],[1 1],'linestyle','none'); 0045 uistack(p.loopTest,'bottom'); 0046 else 0047 p=0; 0048 end 0049 |