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

Rotate covariance ellipses correctly (plotPose2 stolen from ASPN)

release/4.3a0
Frank Dellaert 2012-06-05 00:17:37 +00:00
parent 647b38c758
commit 9a8e083697
2 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/matlab/Pose2SLAMExample_graph.m
View File

@ -34,6 +34,6 @@ marginals = graph.marginals(result);
for i=1:result.size()-1
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);
covarianceEllipse([pose_i.x;pose_i.y],P{i},'b')
plotPose2(pose_i,'b',P{i})
end
fprintf(1,'%.5f %.5f %.5f\n',P{99})

11
examples/matlab/plotPose2.m Normal file
View File

@ -0,0 +1,11 @@
function plotPose2(p,color,P)
% 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>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);
end