Drastic speedup of plotting

matlab/+gtsam/plot2DTrajectory.m

@@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals)
 import gtsam.*
 
 if ~exist('linespec', 'var') || isempty(linespec)
-    linespec = 'k*-';
+  linespec = 'k*-';
 end
 
 haveMarginals = exist('marginals', 'var');
-keys = KeyVector(values.keys);
 
 holdstate = ishold;
 hold on
 
-% Plot poses and covariance matrices
+% Do something very efficient to draw trajectory
-lastIndex = [];
+poses = utilities.extractPose2(values);
-for i = 0:keys.size-1
+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)
+      P = marginals.marginalCovariance(key);
-            % Draw line from last pose then covariance ellipse on top of
+      gtsam.plotPose2(x,linespec(1), P);
-            % 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', []);
-            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
 end
 
 if ~holdstate
-    hold off
+  hold off
 end
 
 end