Added plot2DTrajectory matlab function to find all Pose2s in a Values and plot them as a trajectory with optional covariance ellipses

matlab/plot2DTrajectory.m

@@ -0,0 +1,43 @@
+function plot2DTrajectory(values, marginals)
+%PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances
+%   Finds all the Pose2 objects in the given Values object and plots them
+% in increasing key order, connecting consecutive poses with a line.  If
+% a Marginals object is given, this function will also plot marginal
+% covariance ellipses for each pose.
+
+import gtsam.*
+
+haveMarginals = exist('marginals', 'var');
+keys = KeyVector(values.keys);
+
+% Store poses and covariance matrices
+lastIndex = [];
+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 ], 'k*-');
+            if haveMarginals
+                P = marginals.marginalCovariance(lastKey);
+                plotPose2(lastPose, 'g', P);
+            end
+        end
+        lastIndex = i;
+    end
+end
+
+% Draw final covariance ellipse
+if ~isempty(lastIndex) && haveMarginals
+    lastKey = keys.at(lastIndex);
+    lastPose = values.at(lastKey);
+    P = marginals.marginalCovariance(lastKey);
+    plotPose2(lastPose, 'g', P);
+end
+
+end
+