Updated some of the matlab utility functions

matlab/covarianceEllipse3D.m

@@ -11,7 +11,7 @@ k = 11.82;
 radii = k*sqrt(diag(s));
 
 % generate data for "unrotated" ellipsoid
-[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3));
+[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
 
 % rotate data with orientation matrix U and center M
 data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc);

matlab/examples/findExampleDataFile.m

@@ -0,0 +1,19 @@
+function datafile = findExampleDataFile(datasetName)
+%FINDEXAMPLEDATAFILE Find a dataset in the examples folder
+
+[ myPath, ~, ~ ] = fileparts(mfilename('fullpath'));
+searchPath = { ...
+    fullfile(myPath, [ '../../examples/Data/' datasetName ]) ...
+    fullfile(myPath, [ 'Data/' datasetName ]) };
+datafile = [];
+for path = searchPath
+    if exist(path{:}, 'file')
+        datafile = path{:};
+    end
+end
+if isempty(datafile)
+    error([ 'Could not find example data file ' datasetName ]);
+end
+
+end
+

matlab/load2D.m

@@ -2,6 +2,8 @@ function [graph,initial] = load2D(filename,model)
 % load2D: read TORO pose graph
 % cannot read noise model from file yet, uses specified model
 
+import gtsam.*
+
 fid = fopen(filename);
 if fid < 0
     error(['load2D: Cannot open file ' filename]);
@@ -13,17 +15,17 @@ fclose(fid);
 lines=columns{1};
 
 % loop over lines and add vertices
-graph = pose2SLAM.Graph;
+graph = NonlinearFactorGraph;
-initial = pose2SLAM.Values;
+initial = Values;
 n=size(lines,1);
 for i=1:n
     line_i=lines{i};
     if strcmp('VERTEX2',line_i(1:7))
         v = textscan(line_i,'%s %d %f %f %f',1);
-        initial.insertPose(v{2}, gtsam.Pose2(v{3}, v{4}, v{5}));
+        initial.insert(v{2}, Pose2(v{3}, v{4}, v{5}));
     elseif strcmp('EDGE2',line_i(1:5))
         e = textscan(line_i,'%s %d %d %f %f %f',1);
-        graph.addRelativePose(e{2}, e{3}, gtsam.Pose2(e{4}, e{5}, e{6}), model);
+        graph.add(BetweenFactorPose2(e{2}, e{3}, Pose2(e{4}, e{5}, e{6}), model));
     end
 end
 

matlab/plot2DPoints.m

@@ -0,0 +1,34 @@
+function plot2DPoints(values, marginals)
+%PLOT2DPOINTS Plots the Point2's in a values, with optional covariances
+%   Finds all the Point2 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.*
+
+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.Point2')
+        if haveMarginals
+            P = marginals.marginalCovariance(key);
+            plotPoint2(p, 'g', P);
+        else
+            plotPoint2(p, 'g');
+        end
+    end
+end
+
+if ~holdstate
+    hold off
+end
+
+end
+

matlab/plot2DTrajectory.m

@@ -1,4 +1,4 @@
-function plot2DTrajectory(values, marginals)
+function plot2DTrajectory(values, linespec, 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
@@ -7,10 +7,17 @@ function plot2DTrajectory(values, marginals)
 
 import gtsam.*
 
+if ~exist('linespec', 'var') || isempty(linespec)
+    linespec = 'k*-';
+end
+
 haveMarginals = exist('marginals', 'var');
 keys = KeyVector(values.keys);
 
-% Store poses and covariance matrices
+holdstate = ishold;
+hold on
+
+% Plot poses and covariance matrices
 lastIndex = [];
 for i = 0:keys.size-1
     key = keys.at(i);
@@ -21,7 +28,7 @@ for i = 0:keys.size-1
             % last pose.
             lastKey = keys.at(lastIndex);
             lastPose = values.at(lastKey);
-            plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], 'k*-');
+            plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec);
             if haveMarginals
                 P = marginals.marginalCovariance(lastKey);
                 plotPose2(lastPose, 'g', P);
@@ -39,5 +46,9 @@ if ~isempty(lastIndex) && haveMarginals
     plotPose2(lastPose, 'g', P);
 end
 
+if ~holdstate
+    hold off
+end
+
 end
 

matlab/plot3DTrajectory.m

@@ -1,18 +1,53 @@
-function plot3DTrajectory(values,style,frames,scale)
+function plot3DTrajectory(values,linespec,frames,scale,marginals)
 % plot3DTrajectory
-if nargin<3,frames=false;end
+if ~exist('scale','var') || isempty(scale), scale=1; end
-if nargin<4,scale=0;end
+if ~exist('frames','var'), scale=[]; end
 
-T=values.translations()
+import gtsam.*
-plot3(T(:,1),T(:,2),T(:,3),style); hold on
+
-if frames
+haveMarginals = exist('marginals', 'var');
-    N=values.size;
+keys = KeyVector(values.keys);
-    for i=0:N-1
+
-        pose = values.pose(i);
+holdstate = ishold;
-        t = pose.translation;
+hold on
-        R = pose.rotation.matrix;
+
-        quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),scale,'r');
+% Plot poses and covariance matrices
-        quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),scale,'g');
+lastIndex = [];
-        quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b');
+for i = 0:keys.size-1
+    key = keys.at(i);
+    x = values.at(key);
+    if isa(x, 'gtsam.Pose3')
+        if ~isempty(lastIndex)
+            % Draw line from last pose then covariance ellipse on top of
+            % last pose.
+            lastKey = keys.at(lastIndex);
+            lastPose = values.at(lastKey);
+            plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
+            if haveMarginals
+                P = marginals.marginalCovariance(lastKey);
+            else
+                P = [];
+            end
+            plotPose3(lastPose, P, scale);
+        end
+        lastIndex = i;
     end
+end
+
+% Draw final pose
+if ~isempty(lastIndex)
+    lastKey = keys.at(lastIndex);
+    lastPose = values.at(lastKey);
+    if haveMarginals
+        P = marginals.marginalCovariance(lastKey);
+    else
+        P = [];
+    end
+    plotPose3(lastPose, P, scale);
+end
+
+if ~holdstate
+    hold off
+end
+
 end

matlab/plotPose3.m

@@ -6,18 +6,20 @@ if nargin<3,axisLength=0.1;end
 gRp = pose.rotation().matrix();  % rotation from pose to global
 C = pose.translation().vector();
 
-% draw the camera axes
+if ~isempty(axisLength)
-xAxis = C+gRp(:,1)*axisLength;
+    % draw the camera axes
-L = [C xAxis]';
+    xAxis = C+gRp(:,1)*axisLength;
-line(L(:,1),L(:,2),L(:,3),'Color','r');
+    L = [C xAxis]';
-
+    line(L(:,1),L(:,2),L(:,3),'Color','r');
-yAxis = C+gRp(:,2)*axisLength;
+    
-L = [C yAxis]';
+    yAxis = C+gRp(:,2)*axisLength;
-line(L(:,1),L(:,2),L(:,3),'Color','g');
+    L = [C yAxis]';
-
+    line(L(:,1),L(:,2),L(:,3),'Color','g');
-zAxis = C+gRp(:,3)*axisLength;
+    
-L = [C zAxis]';
+    zAxis = C+gRp(:,3)*axisLength;
-line(L(:,1),L(:,2),L(:,3),'Color','b');
+    L = [C zAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','b');
+end
 
 % plot the covariance
 if (nargin>2) && (~isempty(P))