From f97869cf202eaf4a5e28c1d90b1f9fe994ae4a15 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sun, 22 Jul 2012 00:57:39 +0000
Subject: [PATCH] Added plot2DTrajectory matlab function to find all Pose2s in
 a Values and plot them as a trajectory with optional covariance ellipses

---
 matlab/plot2DTrajectory.m | 43 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)
 create mode 100644 matlab/plot2DTrajectory.m

diff --git a/matlab/plot2DTrajectory.m b/matlab/plot2DTrajectory.m
new file mode 100644
index 000000000..82c62bb40
--- /dev/null
+++ b/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
+