Added matlab utility functions for plotting 3D points

matlab/plot3DPoints.m

@@ -0,0 +1,36 @@
+function plot3DPoints(values, linespec, marginals)
+%PLOT3DPOINTS Plots the Point3's in a values, with optional covariances
+%   Finds all the Point3 objects in the given Values object and plots them.
+% If a Marginals object is given, this function will also plot marginal
+% covariance ellipses for each point.
+
+import gtsam.*
+
+if ~exist('linespec', 'var') || isempty(linespec)
+    linespec = 'g';
+end
+haveMarginals = exist('marginals', 'var');
+keys = KeyVector(values.keys);
+
+holdstate = ishold;
+hold on
+
+% Plot points and covariance matrices
+for i = 0:keys.size-1
+    key = keys.at(i);
+    p = values.at(key);
+    if isa(p, 'gtsam.Point3')
+        if haveMarginals
+            P = marginals.marginalCovariance(key);
+            plotPoint3(p, linespec, P);
+        else
+            plotPoint3(p, linespec);
+        end
+    end
+end
+
+if ~holdstate
+    hold off
+end
+
+end

matlab/plotPoint3.m

@@ -0,0 +1,13 @@
+function plotPoint3(p, color, P)
+%PLOTPOINT3 Plot a Point3 with an optional covariance matrix
+if size(color,2)==1
+    plot3(p.x,p.y,p.z,[color '*']);
+else
+    plot3(p.x,p.y,p.z,color);
+end
+if exist('P', 'var')
+    covarianceEllipse3D([p.x;p.y;p.z],P);
+end
+
+end
+