diff --git a/examples/matlab/Pose2SLAMExample_graph.m b/examples/matlab/Pose2SLAMExample_graph.m index 142c9b36d..5564ef30e 100644 --- a/examples/matlab/Pose2SLAMExample_graph.m +++ b/examples/matlab/Pose2SLAMExample_graph.m @@ -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}) \ No newline at end of file diff --git a/examples/matlab/plotPose2.m b/examples/matlab/plotPose2.m new file mode 100644 index 000000000..7dbdebf5a --- /dev/null +++ b/examples/matlab/plotPose2.m @@ -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 \ No newline at end of file