12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

axisLength for plotting Pose2 in MATLAB

release/4.3a0
Duy-Nguyen Ta 2012-06-06 09:40:24 +00:00
parent 3a28baf3c8
commit bb89cdda76
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
examples/matlab/plotPose2.m
View File

@ -1,11 +1,13 @@
function plotPose2(p,color,P)
function plotPose2(pose,color,P,axisLength)
% plotPose2: show a Pose2, possibly with covariance matrix
plot(p.x,p.y,[color '*']);
c = cos(p.theta);
s = sin(p.theta);
quiver(p.x,p.y,c,s,0.1,color);
if nargin<4,axisLength=0.1;end
plot(pose.x,pose.y,[color '*']);
c = cos(pose.theta);
s = sin(pose.theta);
quiver(pose.x,pose.y,c,s,axisLength,color);
if nargin>2
pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame
gRp = [c -s;s c]; % rotation from pose to global
covarianceEllipse([p.x;p.y],gRp*pPp*gRp',color);
covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color);
end