From a99595dda8608027319c7c82c4fb3014d077304d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 23 Jul 2012 22:15:05 +0000
Subject: [PATCH] Updated some MATLAB utility plotting functions

---
 matlab/examples/PlanarSLAMExample.m          |  2 +-
 matlab/examples/PlanarSLAMExample_sampling.m |  2 +-
 matlab/examples/Pose3SLAMExample_graph.m     |  5 +++--
 matlab/load3D.m                              | 16 +++++++++-------
 matlab/plot2DPoints.m                        |  9 ++++++---
 matlab/plotPoint2.m                          |  7 +++----
 6 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m
index f82b3aea4..9b4600578 100644
--- a/matlab/examples/PlanarSLAMExample.m
+++ b/matlab/examples/PlanarSLAMExample.m
@@ -73,7 +73,7 @@ cla;hold on
 
 marginals = Marginals(graph, result);
 plot2DTrajectory(result, [], marginals);
-plot2DPoints(result, marginals);
+plot2DPoints(result, [], marginals);
 
 plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
 plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m
index 35d3232a1..58686c362 100644
--- a/matlab/examples/PlanarSLAMExample_sampling.m
+++ b/matlab/examples/PlanarSLAMExample_sampling.m
@@ -51,7 +51,7 @@ cla;hold on
 marginals = Marginals(graph, sample);
 
 plot2DTrajectory(sample, [], marginals);
-plot2DPoints(sample, marginals);
+plot2DPoints(sample, [], marginals);
 
 for j=1:2
     key = symbol('l',j);
diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m
index b6a5444a2..6efb58199 100644
--- a/matlab/examples/Pose3SLAMExample_graph.m
+++ b/matlab/examples/Pose3SLAMExample_graph.m
@@ -28,13 +28,14 @@ cla
 first = initial.at(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
 plot3DTrajectory(initial,'g-',false);
+drawnow;
 
 %% Read again, now with all constraints, and optimize
 import gtsam.*
 graph = load3D(datafile, model, false, N);
 graph.add(NonlinearEqualityPose3(0, first));
-optimizer = DoglegOptimizer(graph, initial);
+optimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = optimizer.optimizeSafely();
 plot3DTrajectory(result, 'r-', false); axis equal;
 
-view(0); axis equal;
\ No newline at end of file
+view(3); axis equal;
\ No newline at end of file
diff --git a/matlab/load3D.m b/matlab/load3D.m
index 5c9d08146..c0ccd545f 100644
--- a/matlab/load3D.m
+++ b/matlab/load3D.m
@@ -3,6 +3,8 @@ function [graph,initial] = load3D(filename,model,successive,N)
 % cannot read noise model from file yet, uses specified model
 % if [successive] is tru, constructs initial estimate from odometry
 
+import gtsam.*
+
 if nargin<3, successive=false; end
 fid = fopen(filename);
 if fid < 0
@@ -15,10 +17,10 @@ fclose(fid);
 lines=columns{1};
 
 % loop over lines and add vertices
-graph = pose3SLAM.Graph;
-initial = pose3SLAM.Values;
+graph = NonlinearFactorGraph;
+initial = Values;
 origin=gtsam.Pose3;
-initial.insertPose(0,origin);
+initial.insert(0,origin);
 n=size(lines,1);
 if nargin<4, N=n;end
 
@@ -30,7 +32,7 @@ for i=1:n
         if (~successive && i1<N || successive && i1==0)
             t = gtsam.Point3(v{3}, v{4}, v{5});
             R = gtsam.Rot3.Ypr(v{8}, -v{7}, v{6});
-            initial.insertPose(i1, gtsam.Pose3(R,t));
+            initial.insert(i1, gtsam.Pose3(R,t));
         end
     elseif strcmp('EDGE3',line_i(1:5))
         e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
@@ -41,12 +43,12 @@ for i=1:n
                 t = gtsam.Point3(e{4}, e{5}, e{6});
                 R = gtsam.Rot3.Ypr(e{9}, e{8}, e{7});
                 dpose = gtsam.Pose3(R,t);
-                graph.addRelativePose(i1, i2, dpose, model);
+                graph.add(BetweenFactorPose3(i1, i2, dpose, model));
                 if successive
                     if i2>i1
-                        initial.insertPose(i2,initial.pose(i1).compose(dpose));
+                        initial.insert(i2,initial.at(i1).compose(dpose));
                     else
-                        initial.insertPose(i1,initial.pose(i2).compose(dpose.inverse));
+                        initial.insert(i1,initial.at(i2).compose(dpose.inverse));
                     end
                 end
             end
diff --git a/matlab/plot2DPoints.m b/matlab/plot2DPoints.m
index 0065b55c4..d03676699 100644
--- a/matlab/plot2DPoints.m
+++ b/matlab/plot2DPoints.m
@@ -1,4 +1,4 @@
-function plot2DPoints(values, marginals)
+function plot2DPoints(values, linespec, 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
@@ -6,6 +6,9 @@ function plot2DPoints(values, marginals)
 
 import gtsam.*
 
+if ~exist('linespec', 'var') || isempty(linespec)
+    linespec = 'g';
+end
 haveMarginals = exist('marginals', 'var');
 keys = KeyVector(values.keys);
 
@@ -19,9 +22,9 @@ for i = 0:keys.size-1
     if isa(p, 'gtsam.Point2')
         if haveMarginals
             P = marginals.marginalCovariance(key);
-            plotPoint2(p, 'g', P);
+            plotPoint2(p, linespec, P);
         else
-            plotPoint2(p, 'g');
+            plotPoint2(p, linespec);
         end
     end
 end
diff --git a/matlab/plotPoint2.m b/matlab/plotPoint2.m
index ae45455f6..fcc274d90 100644
--- a/matlab/plotPoint2.m
+++ b/matlab/plotPoint2.m
@@ -1,11 +1,10 @@
 function plotPoint2(p,color,P)
-% plotPose2: show a Pose2, possibly with covariance matrix
+% plotPoint2: show a Point2, possibly with covariance matrix
 if size(color,2)==1
     plot(p.x,p.y,[color '*']);
 else
     plot(p.x,p.y,color);
 end
-if nargin>2
-    pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame
-    covarianceEllipse([p.x;p.y],pPp,color(1));
+if exist('P', 'var')
+    covarianceEllipse([p.x;p.y],P,color(1));
 end
\ No newline at end of file