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

Drastic speedup of plotting

release/4.3a0
dellaert 2014-05-25 17:46:30 -04:00
parent 6384e4d336
commit 852e1e1f2f
1 changed files with 21 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
matlab/+gtsam/plot2DTrajectory.m
View File

@ -12,44 +12,32 @@ if ~exist('linespec', 'var') || isempty(linespec)
end
haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
holdstate = ishold;
hold on
% Plot poses and covariance matrices
lastIndex = [];
for i = 0:keys.size-1
% Do something very efficient to draw trajectory
poses = utilities.extractPose2(values);
X = poses(:,1);
Y = poses(:,2);
theta = poses(:,3);
plot(X,Y,linespec);
% Quiver can also be vectorized if no marginals asked
if ~haveMarginals
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
else
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose2')
if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of
% last pose.
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
gtsam.plotPose2(lastPose, 'g', P);
else
gtsam.plotPose2(lastPose, 'g', []);
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
end
end
lastIndex = i;
end
end
% Draw final covariance ellipse
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
gtsam.plotPose2(lastPose, 'g', P);
else
gtsam.plotPose2(lastPose, 'g', []);
end
end