Updated more matlab examples

matlab/examples/LocalizationExample.m

@@ -55,7 +55,7 @@ import gtsam.*
 cla;
 hold on;
 
-plot2DTrajectory(result, Marginals(graph, result));
+plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/examples/OdometryExample.m

@@ -52,7 +52,7 @@ import gtsam.*
 cla;
 hold on;
 
-plot2DTrajectory(result, Marginals(graph, result));
+plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/examples/PlanarSLAMExample.m

@@ -25,72 +25,59 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
 j1 = symbol('l',1); j2 = symbol('l',2);
 
 %% Create graph container and add factors to it
-graph = planarSLAM.Graph;
+graph = gtsam.NonlinearFactorGraph;
 
 %% Add prior
 import gtsam.*
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 import gtsam.*
 odometry = Pose2(2.0, 0.0, 0.0);
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(i1, i2, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
-graph.addRelativePose(i2, i3, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 %% Add bearing/range measurement factors
 import gtsam.*
 degrees = pi/180;
 brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
-graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise);
+graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
-graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise);
+graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
-graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise);
+graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
 
 % print
 graph.print(sprintf('\nFull graph:\n'));
 
 %% Initialize to noisy points
 import gtsam.*
-initialEstimate = planarSLAM.Values;
+initialEstimate = Values;
-initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2));
+initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2));
-initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
+initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1));
-initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
+initialEstimate.insert(j1, Point2(1.8, 2.1));
-initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
+initialEstimate.insert(j2, Point2(4.1, 1.8));
 
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initialEstimate,1);
+optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
+result = optimizer.optimizeSafely();
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
 import gtsam.*
 cla;hold on
-marginals = graph.marginals(result);
+
-for i=1:3
+marginals = Marginals(graph, result);
-    key = symbol('x',i);
+plot2DTrajectory(result, [], marginals);
-    pose{i} = result.pose(key);
+plot2DPoints(result, marginals);
-    P{i}=marginals.marginalCovariance(key);
+
-    if i>1
+plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
-        plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-');
+plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
-    end
+plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
-end
-for i=1:3
-    plotPose2(pose{i},'g',P{i})
-end
-point = {};
-for j=1:2
-    key = symbol('l',j);
-    point{j} = result.point(key);
-    Q{j}=marginals.marginalCovariance(key);
-    plotPoint2(point{j},'b',Q{j})
-end
-plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-');
-plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-');
-plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-');
 axis([-0.6 4.8 -1 1])
 axis equal
 view(2)

matlab/examples/PlanarSLAMExample_sampling.m

@@ -14,14 +14,14 @@
 %% Create the same factor graph as in PlanarSLAMExample
 import gtsam.*
 i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
-graph = planarSLAM.Graph;
+graph = NonlinearFactorGraph;
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 odometry = Pose2(2.0, 0.0, 0.0);
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(i1, i2, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
-graph.addRelativePose(i2, i3, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 %% Except, for measurements we offer a choice
 import gtsam.*
@@ -29,48 +29,41 @@ j1 = symbol('l',1); j2 = symbol('l',2);
 degrees = pi/180;
 brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 if 1
-    graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise);
+    graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
-    graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise);
+    graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 else
     bearingModel = noiseModel.Diagonal.Sigmas(0.1);    
-    graph.addBearing(i1, j1, Rot2(45*degrees), bearingModel);
+    graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bearingModel));
-    graph.addBearing(i2, j1, Rot2(90*degrees), bearingModel);
+    graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bearingModel));
 end
-graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise);    
+graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
 
 %% Initialize MCMC sampler with ground truth
-sample = planarSLAM.Values;
+sample = Values;
-sample.insertPose(i1, Pose2(0,0,0));
+sample.insert(i1, Pose2(0,0,0));
-sample.insertPose(i2, Pose2(2,0,0));
+sample.insert(i2, Pose2(2,0,0));
-sample.insertPose(i3, Pose2(4,0,0));
+sample.insert(i3, Pose2(4,0,0));
-sample.insertPoint(j1, Point2(2,2));
+sample.insert(j1, Point2(2,2));
-sample.insertPoint(j2, Point2(4,2));
+sample.insert(j2, Point2(4,2));
 
 %% Calculate and plot Covariance Ellipses
-figure(1);clf;hold on
+cla;hold on
-marginals = graph.marginals(sample);
+marginals = Marginals(graph, sample);
-for i=1:3
+
-    key = symbol('x',i);
+plot2DTrajectory(sample, [], marginals);
-    pose{i} = sample.pose(key);
+plot2DPoints(sample, marginals);
-    P{i}=marginals.marginalCovariance(key);
+
-    if i>1
-        plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-');
-    end
-end
-for i=1:3
-    plotPose2(pose{i},'g',P{i})
-end
 for j=1:2
     key = symbol('l',j);
-    point{j} = sample.point(key);
+    point{j} = sample.at(key);
     Q{j}=marginals.marginalCovariance(key);
     S{j}=chol(Q{j}); % for sampling
-    plotPoint2(point{j},'b',Q{j})
 end
-plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-');
+
-plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-');
+plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
-plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-');
+plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
-axis equal
+plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
+view(2); axis auto; axis equal
 
 %% Do Sampling on point 2
 N=1000;

matlab/examples/Pose2SLAMExample.m

@@ -19,58 +19,55 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAM.Graph;
+graph = NonlinearFactorGraph;
 
 %% Add prior
 import gtsam.*
 % gaussian for prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 import gtsam.*
 % general noisemodel for odometry
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
+graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
-graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
-graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
-graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
 
 %% Add pose constraint
 import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model);
+graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 % print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
 import gtsam.*
-initialEstimate = pose2SLAM.Values;
+initialEstimate = Values;
-initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 ));
+initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
-initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 ));
+initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 ));
-initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2));
+initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2));
-initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi  ));
+initialEstimate.insert(4, Pose2(4.0, 2.0, pi  ));
-initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2));
+initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initialEstimate,1);
+optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
+result = optimizer.optimizeSafely();
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
 cla;
-X=result.poses();
+hold on
-plot(X(:,1),X(:,2),'k*-'); hold on
+plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2);
-plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-');
+marginals = Marginals(graph, result);
-marginals = graph.marginals(result);
+
+plot2DTrajectory(result, [], marginals);
 
-for i=1:result.size()
-    pose_i = result.pose(i);
-    P = marginals.marginalCovariance(i)
-	plotPose2(pose_i,'g',P);
-end
 axis([-0.6 4.8 -1 1])
 axis equal
 view(2)

matlab/examples/Pose2SLAMExample_advanced.m

@@ -21,22 +21,22 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAM.Graph;
+graph = NonlinearFactorGraph;
 
 %% Add prior
 import gtsam.*
 % gaussian for prior
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 import gtsam.*
 % general noisemodel for odometry
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-graph.addRelativePose(1, 2, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
-graph.addRelativePose(2, 3, odometry, odometryNoise);
+graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 
 %% Add measurements
 import gtsam.*
@@ -48,23 +48,23 @@ graph.print('full graph');
 
 %% Initialize to noisy points
 import gtsam.*
-initialEstimate = pose2SLAM.Values;
+initialEstimate = Values;
-initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
+initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
-initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
+initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
 
 initialEstimate.print('initial estimate');
 
 %% set up solver, choose ordering and optimize
-%params = NonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15);
+params = DoglegParams;
-%
+params.setAbsoluteErrorTol(1e-15);
-%ord = graph.orderingCOLAMD(initialEstimate);
+params.setRelativeErrorTol(1e-15);
-%
+params.setVerbosity('ERROR');
-%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);                      
+params.setVerbosityDL('VERBOSE');
-%result.print('final result');
+params.setOrdering(graph.orderingCOLAMD(initialEstimate));
+optimizer = DoglegOptimizer(graph, initialEstimate, params);                      
 
-%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = optimizer.optimizeSafely();
-result = graph.optimize(initialEstimate,1);
 result.print('final result');
 
 %% Get the corresponding dense matrix
@@ -78,3 +78,4 @@ Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:));
 A = Ab(:,1:end-1);
 b = full(Ab(:,end));
 spy(A);
+title('Non-zero entries in measurement Jacobian');

matlab/examples/Pose2SLAMExample_circle.m

@@ -16,33 +16,37 @@ p0 = hexagon.pose(0);
 p1 = hexagon.pose(1);
 
 %% create a Pose graph with one equality constraint and one measurement
-fg = pose2SLAM.Graph;
+import gtsam.*
-fg.addPoseConstraint(0, p0);
+fg = NonlinearFactorGraph;
+fg.add(NonlinearEqualityPose2(0, p0));
 delta = p0.between(p1);
-covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
-fg.addRelativePose(0,1, delta, covariance);
+fg.add(BetweenFactorPose2(0,1, delta, covariance));
-fg.addRelativePose(1,2, delta, covariance);
+fg.add(BetweenFactorPose2(1,2, delta, covariance));
-fg.addRelativePose(2,3, delta, covariance);
+fg.add(BetweenFactorPose2(2,3, delta, covariance));
-fg.addRelativePose(3,4, delta, covariance);
+fg.add(BetweenFactorPose2(3,4, delta, covariance));
-fg.addRelativePose(4,5, delta, covariance);
+fg.add(BetweenFactorPose2(4,5, delta, covariance));
-fg.addRelativePose(5,0, delta, covariance);
+fg.add(BetweenFactorPose2(5,0, delta, covariance));
 
 %% Create initial config
-initial = pose2SLAM.Values;
+initial = Values;
-initial.insertPose(0, p0);
+initial.insert(0, p0);
-initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
+initial.insert(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
-initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));
-initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]'));
+initial.insert(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]'));
-initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]'));
-initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]'));
+initial.insert(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]'));
 
 %% Plot Initial Estimate
-figure(1);clf
+cla
-plot(initial.xs(),initial.ys(),'g-*'); axis equal
+plot2DTrajectory(initial, 'g*-'); axis equal
 
 %% optimize
-result = fg.optimize(initial);
+optimizer = DoglegOptimizer(fg, initial);
+result = optimizer.optimizeSafely;
 
 %% Show Result
-hold on; plot(result.xs(),result.ys(),'b-*')
+hold on; plot2DTrajectory(result, 'b*-');
+view(2);
+axis([-1.5 1.5 -1.5 1.5]);
 result.print(sprintf('\nFinal result:\n'));

matlab/examples/Pose2SLAMExample_graph.m

@@ -10,33 +10,39 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+%% Find data file
+datafile = findExampleDataFile('w100-odom.graph');
+
 %% Initialize graph, initial estimate, and odometry noise
 import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
-[graph,initial]=load2D('../../examples/Data/w100-odom.graph',model);
+[graph,initial] = load2D(datafile, model);
 initial.print(sprintf('Initial estimate:\n'));
 
 %% Add a Gaussian prior on pose x_1
 import gtsam.*
 priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
-graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph
 
 %% Plot Initial Estimate
-figure(1);clf
+cla
-plot(initial.xs(),initial.ys(),'g-*'); axis equal
+plot2DTrajectory(initial, 'g-*'); axis equal
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initial);
+optimizer = LevenbergMarquardtOptimizer(graph, initial);
-hold on; plot(result.xs(),result.ys(),'b-*')
+result = optimizer.optimizeSafely;
+hold on; plot2DTrajectory(result, 'b-*');
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
-marginals = graph.marginals(result);
+marginals = Marginals(graph, result);
 P={};
 for i=1:result.size()-1
-    pose_i = result.pose(i);
+    pose_i = result.at(i);
     P{i}=marginals.marginalCovariance(i);
     plotPose2(pose_i,'b',P{i})
 end
+view(2)
+axis([-15 10 -15 10]); axis equal;
 fprintf(1,'%.5f %.5f %.5f\n',P{99})

matlab/examples/Pose2SLAMwSPCG.m

@@ -17,41 +17,42 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAM.Graph;
+graph = NonlinearFactorGraph;
 
 %% Add prior
 import gtsam.*
 % gaussian for prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
+graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 import gtsam.*
 % general noisemodel for odometry
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
+graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
-graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
-graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
-graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
 
 %% Add pose constraint
 import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model);
+graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 % print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAM.Values;
+initialEstimate = Values;
-initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 ));
+initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
-initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 ));
+initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 ));
-initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2));
+initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2));
-initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi  ));
+initialEstimate.insert(4, Pose2(4.0, 2.0, pi  ));
-initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2));
+initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimizeSPCG(initialEstimate);
+optimizer = DoglegOptimizer(graph, initialEstimate);
+result = optimizer.optimizeSafely();
 result.print(sprintf('\nFinal result:\n'));

matlab/examples/Pose3SLAMExample.m

@@ -16,37 +16,38 @@ p0 = hexagon.pose(0);
 p1 = hexagon.pose(1);
 
 %% create a Pose graph with one equality constraint and one measurement
-fg = pose3SLAM.Graph;
+import gtsam.*
-fg.addPoseConstraint(0, p0);
+fg = NonlinearFactorGraph;
+fg.add(NonlinearEqualityPose3(0, p0));
 delta = p0.between(p1);
-covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
-fg.addRelativePose(0,1, delta, covariance);
+fg.add(BetweenFactorPose3(0,1, delta, covariance));
-fg.addRelativePose(1,2, delta, covariance);
+fg.add(BetweenFactorPose3(1,2, delta, covariance));
-fg.addRelativePose(2,3, delta, covariance);
+fg.add(BetweenFactorPose3(2,3, delta, covariance));
-fg.addRelativePose(3,4, delta, covariance);
+fg.add(BetweenFactorPose3(3,4, delta, covariance));
-fg.addRelativePose(4,5, delta, covariance);
+fg.add(BetweenFactorPose3(4,5, delta, covariance));
-fg.addRelativePose(5,0, delta, covariance);
+fg.add(BetweenFactorPose3(5,0, delta, covariance));
 
 %% Create initial config
-initial = pose3SLAM.Values;
+initial = Values;
 s = 0.10;
-initial.insertPose(0, p0);
+initial.insert(0, p0);
-initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
+initial.insert(1, hexagon.pose(1).retract(s*randn(6,1)));
-initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1)));
+initial.insert(2, hexagon.pose(2).retract(s*randn(6,1)));
-initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1)));
+initial.insert(3, hexagon.pose(3).retract(s*randn(6,1)));
-initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
+initial.insert(4, hexagon.pose(4).retract(s*randn(6,1)));
-initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
+initial.insert(5, hexagon.pose(5).retract(s*randn(6,1)));
 
 %% Plot Initial Estimate
 cla
-T=initial.translations();
+plot3DTrajectory(initial, 'g-*');
-plot3(T(:,1),T(:,2),T(:,3),'g-*');
 
 %% optimize
-result = fg.optimize(initial,1);
+optimizer = DoglegOptimizer(fg, initial);
+result = optimizer.optimizeSafely();
 
 %% Show Result
-hold on; plot3DTrajectory(result,'b-*', true, 0.3);
+hold on; plot3DTrajectory(result, 'b-*', true, 0.3);
 axis([-2 2 -2 2 -1 1]);
 axis equal
 view(-37,40)

matlab/examples/Pose3SLAMExample_graph.m

@@ -10,23 +10,31 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+%% Find data file
 N = 2500;
-% filename = '../../examples/Data/sphere_smallnoise.graph';
+% dataset = 'sphere_smallnoise.graph';
-% filename = '../../examples/Data/sphere2500_groundtruth.txt';
+% dataset = 'sphere2500_groundtruth.txt';
-filename = '../../examples/Data/sphere2500.txt';
+dataset = 'sphere2500.txt';
+
+datafile = findExampleDataFile(dataset);
 
 %% Initialize graph, initial estimate, and odometry noise
-model = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+import gtsam.*
-[graph,initial]=load3D(filename,model,true,N);
+model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+[graph,initial]=load3D(datafile,model,true,N);
 
 %% Plot Initial Estimate
-figure(1);clf
+cla
-first = initial.pose(0);
+first = initial.at(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
 plot3DTrajectory(initial,'g-',false);
 
 %% Read again, now with all constraints, and optimize
-graph = load3D(filename,model,false,N);
+import gtsam.*
-graph.addPoseConstraint(0, first);
+graph = load3D(datafile, model, false, N);
-result = graph.optimize(initial);
+graph.add(NonlinearEqualityPose3(0, first));
-plot3DTrajectory(result,'r-',false); axis equal;
+optimizer = DoglegOptimizer(graph, initial);
+result = optimizer.optimizeSafely();
+plot3DTrajectory(result, 'r-', false); axis equal;
+
+view(0); axis equal;

matlab/examples/gtsamExamples.m

@@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin)
 
 % Edit the above text to modify the response to help gtsamExamples
 
-% Last Modified by GUIDE v2.5 13-Jun-2012 08:13:23
+% Last Modified by GUIDE v2.5 23-Jul-2012 13:12:19
 
 % Begin initialization code - DO NOT EDIT
 gui_Singleton = 1;
@@ -104,6 +104,20 @@ echo on
 Pose2SLAMExample
 echo off
 
+% --- Executes on button press in Pose2SLAMCircle.
+function Pose2SLAMCircle_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+echo on
+Pose2SLAMExample_circle
+echo off
+
+% --- Executes on button press in Pose2SLAMManhattan.
+function Pose2SLAMManhattan_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+echo on
+Pose2SLAMExample_graph
+echo off
+
 % --- Executes on button press in Pose3SLAM.
 function Pose3SLAM_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
@@ -111,6 +125,13 @@ echo on
 Pose3SLAMExample
 echo off
 
+% --- Executes on button press in Pose3SLAMSphere.
+function Pose3SLAMSphere_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+echo on
+Pose3SLAMExample_graph
+echo off
+
 % --- Executes on button press in PlanarSLAM.
 function PlanarSLAM_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
@@ -118,6 +139,13 @@ echo on
 PlanarSLAMExample
 echo off
 
+% --- Executes on button press in PlanarSLAMSampling.
+function PlanarSLAMSampling_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+echo on
+PlanarSLAMExample_sampling
+echo off
+
 % --- Executes on button press in SFM.
 function SFM_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
@@ -138,7 +166,3 @@ axes(handles.axes3);
 echo on
 StereoVOExample
 echo off
-
-% --- Executes on button press in Future.
-function Future_Callback(hObject, eventdata, handles)
-fprintf(1,'Future demo not implemented yet :-)\n');