From 62f28bb798dca3845eb843bda045c855b29919f7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:21:05 +0000 Subject: [PATCH] Updated more matlab examples --- matlab/examples/LocalizationExample.m | 2 +- matlab/examples/OdometryExample.m | 2 +- matlab/examples/PlanarSLAMExample.m | 59 +++++++----------- matlab/examples/PlanarSLAMExample_sampling.m | 61 ++++++++----------- matlab/examples/Pose2SLAMExample.m | 43 ++++++------- matlab/examples/Pose2SLAMExample_advanced.m | 33 +++++----- matlab/examples/Pose2SLAMExample_circle.m | 44 +++++++------ matlab/examples/Pose2SLAMExample_graph.m | 22 ++++--- matlab/examples/Pose2SLAMwSPCG.m | 29 ++++----- matlab/examples/Pose3SLAMExample.m | 41 +++++++------ matlab/examples/Pose3SLAMExample_graph.m | 30 +++++---- matlab/examples/gtsamExamples.fig | Bin 9270 -> 10455 bytes matlab/examples/gtsamExamples.m | 34 +++++++++-- 13 files changed, 211 insertions(+), 189 deletions(-) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index 1c38bc979..e4ee76d7c 100644 --- a/matlab/examples/LocalizationExample.m +++ b/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 diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index 16323a616..81733cfb0 100644 --- a/matlab/examples/OdometryExample.m +++ b/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 diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index e664d66fc..f82b3aea4 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/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.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, 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.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); -graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, 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.insertPose(i1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); -initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); +initialEstimate = Values; +initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1)); +initialEstimate.insert(j1, Point2(1.8, 2.1)); +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 - key = symbol('x',i); - pose{i} = result.pose(key); - 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 -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-'); + +marginals = Marginals(graph, result); +plot2DTrajectory(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-'); +plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index ec68d81e6..35d3232a1 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/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.addRelativePose(i2, i3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(i1, i2, 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.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); + graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), 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.addBearing(i2, j1, Rot2(90*degrees), bearingModel); + graph.add(BearingFactor2D(i1, j1, Rot2(45*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.insertPose(i1, Pose2(0,0,0)); -sample.insertPose(i2, Pose2(2,0,0)); -sample.insertPose(i3, Pose2(4,0,0)); -sample.insertPoint(j1, Point2(2,2)); -sample.insertPoint(j2, Point2(4,2)); +sample = Values; +sample.insert(i1, Pose2(0,0,0)); +sample.insert(i2, Pose2(2,0,0)); +sample.insert(i3, Pose2(4,0,0)); +sample.insert(j1, Point2(2,2)); +sample.insert(j2, Point2(4,2)); %% Calculate and plot Covariance Ellipses -figure(1);clf;hold on -marginals = graph.marginals(sample); -for i=1:3 - key = symbol('x',i); - pose{i} = sample.pose(key); - 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 +cla;hold on +marginals = Marginals(graph, sample); + +plot2DTrajectory(sample, [], marginals); +plot2DPoints(sample, marginals); + 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([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-'); -axis equal + +plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); +plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); +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; diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index 4bb072fa8..1e26ce33d 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/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.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(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(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, 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.insertPose(1, Pose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +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(); -plot(X(:,1),X(:,2),'k*-'); hold on -plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); -marginals = graph.marginals(result); +hold on +plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); +marginals = Marginals(graph, 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) diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index fc23d67f1..b496c44e7 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/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.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, 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.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +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); -% -%ord = graph.orderingCOLAMD(initialEstimate); -% -%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); -%result.print('final result'); +params = DoglegParams; +params.setAbsoluteErrorTol(1e-15); +params.setRelativeErrorTol(1e-15); +params.setVerbosity('ERROR'); +params.setVerbosityDL('VERBOSE'); +params.setOrdering(graph.orderingCOLAMD(initialEstimate)); +optimizer = DoglegOptimizer(graph, initialEstimate, params); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +result = optimizer.optimizeSafely(); 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'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index 355f1a10e..e16edf9cb 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/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; -fg.addPoseConstraint(0, p0); +import gtsam.* +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); -covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); +fg.add(BetweenFactorPose2(0,1, delta, covariance)); +fg.add(BetweenFactorPose2(1,2, delta, covariance)); +fg.add(BetweenFactorPose2(2,3, delta, covariance)); +fg.add(BetweenFactorPose2(3,4, delta, covariance)); +fg.add(BetweenFactorPose2(4,5, delta, covariance)); +fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config -initial = pose2SLAM.Values; -initial.insertPose(0, p0); -initial.insertPose(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.insertPose(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.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); +initial = Values; +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.pose(4).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 -plot(initial.xs(),initial.ys(),'g-*'); axis equal +cla +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')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index ac6d1e317..2cfe900a0 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/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 -plot(initial.xs(),initial.ys(),'g-*'); axis equal +cla +plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initial); -hold on; plot(result.xs(),result.ys(),'b-*') +optimizer = LevenbergMarquardtOptimizer(graph, initial); +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}) \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index 5db736fab..dca31ac94 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/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.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(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(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, 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.insertPose(1, Pose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +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')); \ No newline at end of file diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index d4a8baf99..86c5c23c8 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/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; -fg.addPoseConstraint(0, p0); +import gtsam.* +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]); -fg.addRelativePose(0,1, delta, covariance); -fg.addRelativePose(1,2, delta, covariance); -fg.addRelativePose(2,3, delta, covariance); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); +fg.add(BetweenFactorPose3(0,1, delta, covariance)); +fg.add(BetweenFactorPose3(1,2, delta, covariance)); +fg.add(BetweenFactorPose3(2,3, delta, covariance)); +fg.add(BetweenFactorPose3(3,4, delta, covariance)); +fg.add(BetweenFactorPose3(4,5, 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.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla -T=initial.translations(); -plot3(T(:,1),T(:,2),T(:,3),'g-*'); +plot3DTrajectory(initial, '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) diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index cace357ae..b6a5444a2 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/examples/Pose3SLAMExample_graph.m @@ -10,23 +10,31 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% Find data file N = 2500; -% filename = '../../examples/Data/sphere_smallnoise.graph'; -% filename = '../../examples/Data/sphere2500_groundtruth.txt'; -filename = '../../examples/Data/sphere2500.txt'; +% dataset = 'sphere_smallnoise.graph'; +% dataset = 'sphere2500_groundtruth.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]); -[graph,initial]=load3D(filename,model,true,N); +import gtsam.* +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 -first = initial.pose(0); +cla +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); -graph.addPoseConstraint(0, first); -result = graph.optimize(initial); -plot3DTrajectory(result,'r-',false); axis equal; +import gtsam.* +graph = load3D(datafile, model, false, N); +graph.add(NonlinearEqualityPose3(0, first)); +optimizer = DoglegOptimizer(graph, initial); +result = optimizer.optimizeSafely(); +plot3DTrajectory(result, 'r-', false); axis equal; + +view(0); axis equal; \ No newline at end of file diff --git a/matlab/examples/gtsamExamples.fig b/matlab/examples/gtsamExamples.fig index be72daf9c4674e0533e2c13c720f7edb58c385cf..d0037027882c4ee90461dea6d8bd52368f58de0a 100644 GIT binary patch literal 10455 zcma*NcQ71o^e(PT5TXmBw~*+)Z4e}S3Bt0X1<||JmWbYaCwfHhy+rSX=-pzmi>$s_ zdq3ZMf4`ag=Y8g#_q>0ddFPyYX3lfY(Ny@Tp`gej#xKaCsqpd57kfvW*DN}Y7M@>R z+?}LZbd-!fXo-uyW>IptvGBC9W^r+rX3=zUW>NEUWDyo&5fYIW5|$Q{Vi6V;5@z{- zga$X({{i*~O|t)9gIrixzPX4`Q`(FYR1y>6T(3B0K`nCc{3~!M6mv`|#C{2X4+y5h zAxfM#9F&3Qd1VQ#h=^YPwepxqQm=e`dQu8w^tUTz@_YN0PuYOlKsU?}Q6FUGZ#(=N zfl&Mr=WFSEA$x&WgeqNZ3-X1LchqGu&m>}OMxY*_p2B)l7A$V8E2DFKFlYpW?1jd#zGR7oXxVdNbn zEyV7RRwd0O9F_HF8B=UDzF z^L5**gRG7{p=;lIU+LtGRM`{mq}zSs2nry!?U+JwKdWD2aZFPo0yo9tpHE)8RCGeC#JD#&6u6FBphCcDx%zUPlu}asGbW*uI7M8R(phyV2kR69n zI(EjrXkqX7U7eAYmaHA%DOiebxfm*NLoj|Ei+T%T-%qLFL{KbW@{iOcPeVuzWM7|~nxg=?4 zDyl^<5m!8H0IhNZRORjpTz%MqxZ`-?s@*7DA=p!EZ9mTheu_9AgU2e`J1jC_dmL*G z)*Icp?(M4N$^Hf%Qt$)McpOv&8N{5cN7@G3-6C{nzfzy`?LVkJkyIe>b)T+{edyy> zK1So?GS{6|*G%#oUX_qZY}t4UYL5;-^XiUnI`jgzl+^H=)f1fDj4*$Xm2c@;1>fyY z&kIf1KW%paO7sL8Jxl>3vTOzN18DZ$2-b#c*7~l)rJP$)KVlbU=^k2C@1P@A<3Z-8 zH;XjRTgF5oLhgH(OmhAFPGwC(J6pDQaP!*ovq4Tt_6-iW6H2t7^a>c_C-MAs_Mft% zw%9g2k0~GH_y@~1t}Z42ZW=FrP!)OVV5+lLZ6(XCzaS$|ZHQ(H(kYrZ-l6G61e zp}Bn@g)h;=cG`VjSPGQ)4kwrx!A%X47_$ek9<{2|U3jy{@BQs(iB-}L@ajv+R8e3N z{JiQ)@dT%}Pj=E*ZMR1BhdKCd@3_zicskklyE;R3HUMc;g+iJ7e zCc54Z7~qpVh3`srFa7}zw|$-nM2sihLXZ=W_eaX?+T4Ms)cWmw%*X5ScG;U>m&QIL zX--iNTIl!%D^F&L$QV5#vhc|ib~?<8tMsF90&)jEG7vh#x5P&L?Y%sVV~|8OuUJ~^ zA@c5s%B0>!sSeIt$;FIFuT|$M5U3kEv&zCJ9RN{}18N1PYc>nI4IsYF-ut}#Ef~`M zyO)YyqL~Gy54@$1u76=(DKAVL_yDEq1q6)6w3E-x?@^sWJ?d%`P$^4#ynI^KU?Wuwsf&922{{K znlzy8dQW1vz9X6Ha);VWsq40s{1; ztRry)(=inBtgDaz6K%TU?!b%>D;sF58M|=)uFgDg_9pbSGBNXba#DjlK=m0$cfXdh z5|*}4tEwvy&&#i0upAU`G8W|QwK9Xj$HKy35XRctlnPXkRGQy9`m#J+!=ge&?-YYm z{tWU{MM2b9Xe13RR{3pge9BkycwN~*dzRn0qnP_r2<3xS$u_Sni-ee1hkdV$WHw&< zNWXUMq~fr!70C+R0QcS;Oy%5cI$L4QI$zP$%+rMOa4?!Q_|aE|#_=r~0R#hxoq`>uhr|zf98r_%#ndBOcUqAHFMknDbN-Nn($ z-Kl%#?JE95s5j2zmRES}wO(2gWoC8-?GvaM^SU^8#vHYB<6EDLR#a6Zt|n+vfZ5y_{%n?!gAC`4!^t#_d%r z;(mf^Kl!)-VVF#~un2Ng3qr^jJ&cZU3!s^Zu8TbLP;L#8Vp(*2&&r1d1^i&j>F_}> zaGnRq`~aW;J%??_gTKow@3RNfh+R=4>IW2sxIw zw51EZA+M%nc;*Fu$|c+d_HJH@600(zUCA*1ziW?sYQwzx>wi-ApW$=g`~PPD4N9%K zf#^Gq53Ed%4uX{X&Ht-={$uo~8HfKEonO}5oK_wA@%hupmLaCqJvJgP`>#K<=PXzC zMCrK4#LYTajJ+kAPZa#-K;MbJDL+ukQq}rhe(omfj{K;Qcs&~wDDDQiIm}c)tc~YrF^y#A3w7!gF)^-Tmx_OUG`iY(3b& zQ!1G&Q}|X1ABfHzHUwz=+VX2zEwB(Zek7PAk%iFx15evzGv{< z6=#FXvP`_^ABLE4m5M+uX#4mf`1R$ouc#HVjAmA>@ZY#sJnBbJGYfsaA3yEOxecr4 zRS1&;q<>F+cvV72pZtQ68~om6miQdpwr*m)j;{|I3Dip>F=2nt@U5afHr!oJI@Y=* zL@;%qBS-}{Fwpi^=uEXvLdZVK+2whfCwmkPSE5Ox8zZM4*wNYndTy z>6Y!LY}?#v|980O9fYYCr$OQo%X+^Yz4u|rt7bp~h@ZXS68j$8-^Kq+CzJpr#LE|6 z5-B=$+tF1^YCMD_p;cvz5EvQ}Oa1eCks--iHz6UMtEjN>R&%TG2iaX2HhPg!QcvsSJQu5>GpJ{O)dEwLkm_2znb>D+ch@u?_m?^$$7^YXum^G2dbo zg0^cZ0NZZ2f1Wyfp%ii^$X-8qp+Y9ej(?L;n07^)@&ghJal{Dme(w`5J_O&HIs^%t z|L4QvW-8;bQd9L|n}^5DoQ;;kBZ29`;GyrBk;l2I$ZVm1M^$D$^nehgFbdm=oFF5` z2YjkXWY*|>_`&<|_9r>uQ+>Tlb_{LO#2#V&`>?^jef-6);2NiDaEPfb=-=JF5@xvi znL6lmYwfZ$_vfeFQ05_*7av=gPki5sA!^?x1ZBi4Fyw{;VA^dM?En6Z!F@1?Jy<-F z{UiP#X0CtIjHObhs>ws|xUk~VD=Uq@`&LKJ2H@hM@3gfw_OZ?|^GV;~j?tMnlhL1> zU+`Y)BPE0mz2DXT$KiKwsrVOHRhyntLy|5O`GXc0$zez0_lGXl@M=cGOY@D$JaNsA zrnK0ZSFcg6dAHwL<@JI1*=(yJm%0FJsIyXeqNe8i9i1${x(*j6!B9fHj!Ru%|xj>%9=-qlZ3wT==a#OvV( zdKZdPt&LD|Z#IgPcYZMZ?&diUxyiYA&JBI2h-ERO#%Iw{fn7LG^NJ(dls6@?x42KQ zhw=O?IRONUYe|9AwPcTvi4>>RM5?H60xBdqTJvd;?23x^rIj*|1ZJn{!)BsL7e<+}q`m5n#EYC8|tPVgigZ~QI`v2t()D2W)M%GY|XwjWgpp{rXKJuG#yQXyq==`nV#|6u7(ke;(b6sZ^(7 z*V9_ z{m6Pm)V~m3?QesVRNP^09C0w?*$sK82(rN|-R^`#ZRx$@IB)JtcECqB-4%Gu3LMv! z2^<5*Fg(cmH}`h7ZPK1Yf)^jjOn~CWj8$y3!_I@sl!NbB-jWENE2q>%WxKx+5~SxI zR06LlHL*&se?`2nJhZEPPeAg6THjSInmv}tp^-rSo1oo?8~FX1+uW78z{}hHxe(wg zWvA53`QE)Mv0vOn+}LLGMaW^Of6t5DppsH1uharDHR+YJBIZ}aSC^N!Zm36bzX_^G zb1dRNns25tB|{^8b7%)*S%HhBd`Czjn60CTbECEO`(q-9i~OzkXP2u%tw5*)p!eTV z4E%DcfcX*qOtb`orY`j&cep2&XR-Q?Seh57Hfy-AGKsMjOVKJsXUmq8TeBLAn_e6n zv!7077Az|xb|_i|tM7%ar9=jQ4+nj8k&QSaIsd+`EZ->>v8%@p!WXXv4AI3jQxA3q zn@-N4>QI?P6iop>+a>)^ds+=;B}fyXUpK|8vJ*vAsTzujjI~%Nc&%9hagO16{a)k7 z%WfqLgNQ-9kBP5fO3594WEpvXe`7zd4bAm=(t;Y+6c&()E|OV*bV#*wU5R`M_;#lK zbml25DG<@+#}K%HN;sL(k2DI$-(4JQ_vSpT>TS6IMD>I?_D*u5dYa63d?W&sp|c!F zwUE6nZjn{K*2jmFU)~~(N8_q%E?n-9p`1IHC_}BMBF5E~?4-X$NIenpSs_l(K&>y%czgaaz+vgVbI}v8}-6Jup z`Q-Tx^$nhPZOAF2h8Qd#Uo0HT2=Oe}>O!`&ES47tneK?bD&+mZbRoUSOm%~VGt>zp_8n8y>i0@BX=6nc*;-kr?z zJ`$Ja)$)ga3rseLvi1%q1=}hjojGsQlq`C=&{ABrX;he@%isd-ZQ7Cz_58=TJ^$$0 z`&BHAcQxK2@vG#7U!Mwg_rGd2b^Irq$Xl-&-Ff3mc(+{S{MHju6z25lKpL;T+7Ist zrT2v(>+vnL7cmeh#1{{jl$j8Iv3GS`+h z!LxskeVPY#<$NaB7~^LmQL2=rzk9A>vO&wt~4PXk!y^5cj-=d{U4S*>G_|O z%OW{K%Z4O|QNPE9GRteR=6|0rRwzc8kEe#)ud%d2usy}^L0SAZEfXSwTvwor4ILi zz;3#y%ijLHRDzElX8xbp=Tjk{+%}lv17mgq(m&ImqHw@6+jn2ClY5LojiL5H+`M;% z?BLnJUC3+!x~B-0JgKgS6m{e}o%mT0)FVs;nv>`M;VPT%!?QKY{Mx}h3+SaqDQI4tpX4b>UwGB;wgX6ZW^?6gNLF0MWSG1#U@VvWKbP#W z-|fIerLc!@E96lhdXh0iIF#~t0B9$A<(load2n8H99z~ib-de{R&ML%W=Lz>)jJ|H z)p6@{laoCED+cnvh12>cxC@m5Re?N9@OvUE6Gd>qA2Jn6QI+36TTAiRo)Fl*BGVN? zY&yGSPLnh?hB@sL_M$Z3f|Nrb3QWpEILvWRljnTEf9KNYiaz7={^0VR*^p}N^zq5v zz_z&`$W(Dyl@_iw1hvyI(=^0JGF6F+4p4O5n!A0hudAr|bMu$$_}CCn z>Emfd;;$F`uYMU8)+cVh z^8y+Vd73z=rUV2#;}WmNJ?&WdMQ!PK9`3!IHXbH#xfTwxLChKvxX!A@{1T6w2TwGyz^+N@2oatmk z-&m}uD%LnHXuNGk?{zQK86QrW93ccGpZ@~f2v~6M?N=6?X{MR*dDEQvtDZ(|Q?o+9 z6wkGaOutgCdEuQ|nEh8-LcyZ@_wOn>mxhN8swz~-?2^7`ucWH9O2yR($JMaKO(!ks z%DsPMo5w5mC425`Ljv!@TlQz;S6KHRQ50JRVe#^A5E@=2+IX;(r9gD~%~gF)t~G6n zX6R9!n+LLzK|1YZE>9KzsW(dT{3hakti;m8=iK%^Rf#GtgNdnH`uez$o!0e{pkLNx z!dnGXR9XZHU8rt}bCmf!W2b7|)=W^cHCe+x<3QG)oWPj8Hu#n1`brl!wN2t)a&5qf zOLKzRZV*;)>4T5$?fnc*QoP3r;a1n>cFI$|OD!vuunk+eWGcyd_^1tpnq!5UM|>m8 zBTuyc^YeK7Uhp__w|l?0ZlW~5Rn_i$88YWXP+WL8cSkC-(1?whZ+z5D@@a0=)5(onKa=ZP4M0k<7B@3lCMXAIIZj9Tfgbq2l6ir;VlM3 z0>4z-C8#vPg|ZnJg`-Kdd=*===dGGt7r9(h9wxi8=J7Kg8Odh3Gt$CM+4L=Nlmd+> zPEf8}T80hnl1Thyg6>d!wy%m<^o$|0-2Meu0MC3Kg`Ks@Y>}&cxF-E)*AG*Ik4JhB z^Le*QtdB3JY(t=XAb>#hov|>W_0G6B(q*booWAO#SpMHW@lw6JPc=}t)k#l`KWmEL zMd#V)7%(=foyAFqJ^KwpeI91jG0z-?A1^N?u)F=d4Wr|n;e->PVgwIn-y4nS!*=(f zam^e>15Ju^p?jAS!9wRS+3l&+Ab0;>JV_Oe5Cy$CnT*DWNy=N+QgjE53T8LnCWAjb zS|Q%&<3M5ZkMi`X*n-Ig4uwHjJUm-ppw=m}vM6FJw^E;hk; zq!6qwwQ;L!4PVLZ?aGl$@l_;u|AqRTe7 z(@lIk?I=n(@yIKhL$-z<#CX8;M2*~H`5VFAgT@n%&x2-Xgr$9M67q-y=yt~hI1#?# z0NH610{7x~;TM-Ec?yVd5InAKXR-UzG?rO)YwhUb53bZR2WGJ7+oK~cXcy&UyWPS zn~T|#9Xi>Qowyk67=aPv1zpOzh&UxQLx!Xd{o|pJeI8^>WH-G_J&C}{a#?fPrH5&u zSlk&xDd$*1I32L|ad}rN#p!WYs|MIh13mv$yyyLG^i964@3aLYw>DO|{+-ok5uuNJ ze3FYc?y&yd*a_M{n8CWUPt4s;+2lUp)FgWi!e)!l~FSd#- z8tQx71E^!3RZ_#hdkDtZFc0{6?E0?f^fj{Ow7$;>Gk?Z+Vxf>--wci^vzTbSxReEthLUX}d z%f3i`z)1_?mR zoDoj~7WuAIF3$Fu;p8^8(ihp75uDc1HJsM5fv`SvIaq?%oBI?fitzum5`m6X26Xx9 zF+WFNg6=f_U=H?GYcI?^77zTfIfJm4AP{DA;iv@denvozufyB*hhWQ z>A7ZQo1k~SvN&>&u72X1HhGk-2@aw1rqao07m*=;B{LcY4aQzV2^Ku~*$R6mAIiry zY`Mt}>khj9p^+H&*p}PY{sO<0YXQ>T%}q1M=LA~N?1NP?0Woe!X|-l28#>mTGwks9 zsTRSn*RY!I*ESc`q!iOdViRixTJ+=Frsp@kvfVS+-{6r_IGxdq%X<_cPv9g2*Bvw1 z@rm>0SVp75;jHcunpxxCL$aHtytQDE$Qd2kwG8U_RdLPP20lr0DGBc0ZM#(}`U?n! z%v^e+B+#BhrZH(gy8tTJ~e2i-a?wndJoWYpa0XNv!J}uYTWp=A{_7A3d{Vi&>BauidLC>HdJ4 z0A}AjSwC#w&<816-<0)yQbh9+DSR3hveY^M*zZ*s2Zvk|ZcBqhceqx@1iu0S=CerkUV}?pM{?#7=ET9c74U-J`2yu$MW^-5BQkld6)m+3xP)di zZtKjz>vueqcfZkRO#-(x7h|^n7ktsu zRO|l5n(W<@dO^kISmDf`Z-n4bUE`t1lpQuIyc@KjY+U=OuF89Q)Em7X9nL6__vfCtFmGW&$r9!Shy{I=fN*^f{EkADY%$@oP zMVj|5OPKBQZ|@TLdpU5Fd@bUy9N+cjr+D-xJ{l@;S)RR9Y=AUU_4Q@-?(-w}pIAY0 z{e#|qrLkfSl!dcNHaS109C35q8z?XRO&m(_%F{PH7oYMa`?r_sYY}iSlC_V15-85o zz(h^)NjZi2@83*{e-%x$Wyy70MsJRb_V2|!#k|f*pPn?ke{|6U!WZ7J*BBHFN8;bZ zJv9x#MIk>HWG~nla44qrT@HbO91aH)e6mt!GelMwq%B2XEjsYM--mo>u6#_{4@?#m zKU2PZT7@vaK|VU{9jn_&nNM}AYzFb)rzLzbw}E|FEBW60!0}k+6?_KdvN4qS1o`w= z$ugzvmtPlvSNUDX8~dSILr>!*oiB(T$Gin%Lv4T z5XS5gC3Z@h2I$pa2G8IV>J#Hbw;gI`4`1StIh>+Tvf?i2p|UGS`{Y-;mSecPi;7CH&hI}6p8 zl8Pyx8iJiEE4rMW4C1sGA6P5MTQ|Nbw-m;F6MA9d+2LBqVjaX375&SRtWro8cl|iH z?(VnS;=|;2&qPuF*8uo&Ul6G7s9q@P=sN)ZIJv)@Ile%RD7u{~XnYgsJA46GYuTzs z7R3w875D%Mx(+pMK=4~fC~YqwB`#-j3SKx9`*j;M=UZGU=Q97?o)0UZm0{@tgqluJ zU6!II&#y|M?q9z9X7;BjLMe?ph*$AL!N4F7C{=<-ie5Azx7F#EV zQ~xuiDk#x(99m5X<%z_5w`Tdn{F|~;gZ)=>Cp$qWoErt!*{O?Pd<7dbb~K)BwWlXux6Fku%2-I zcGLK(p*3#^$6*;8fs$WP{#a`rt5eK*7KL9)$4U|F-$5+I@dP*IrKNPo5bLLE8D62J zM!nGZFGr7eoU@;LJBRLQORfkd`Aut61%(Bh^gYT-{nIRi+~fiJ?J`pGLQg{j<5ZX9 z#8%=$hz>Qhcl;`DcYI~XkdcGLSk?mzQbwLuzt@z$FUivW!ZrtzKfYk?p}RH|h~r~# z4m%DslZAf<7GrZg<{6uY(^SR#aVdPBuxVap z!@9dpM*8@^{N3#m!+CZSeWnttS`jZ)Lw9- z&PTmriT0fRg|t!l3_-0qbnX-2Y~Wt4Wn1;yXpnq9Cw?{seYn3>@bKAP*WT957%;un zf76%-^0C{Xatdukfb%r?QT#9rK2lwC8g zQaH6o8pgWRKNzvq}wQSqC!vWu=7I>{^$6qPY(Ehjx zxNKdb-JO0oz1#i6Tq6)Iu*J36i<;97bbBURATTg>HT2gpTG>%QWQWkXtxJ;PY^HDL z)azql;X6bpv*=mQ&g_t^p-jI(JLc)yDckJ-?6QSo|IaSl$tM}Dfw#|J)&inZmg@_5 zc#e{Ec-V-D4qx-pak9C;##LMoW_QI?Q;Y8-w`;>y{VV|*Nl2oMoYO(PFK#^pf4Kpp zUx{-Ronj}I>y&95v}FT@0lP#7Po~DC^LUuX8HY4`?T%_2=kgZB)R#2kXQtuco}w@L zpO%0Qo`%iQgu>bfF^D3K7&g9?cQ?I?kVeoDSr(ZA#@VIk%=X^4dp3I(T0faJsYv5l z%Cxd@B|Z0$RC9K5v=NZS-6+~?Vm&s{(AEBC&R*q&)kL%hvY1X4yA~@G+ZpR0>q7^d znYZm4@hw1NL$RT((1B<7AJ1@^ff}^IKi7hP$^`%H4F2gK{L=?Z_Y9}#6t`#-_gyzx z-YtN+gP=U1#O$K(STDqjJYCKMoe>%AEu{4RLGzY2sb{(3tG`?a-0esT@i3yOt6 zw{9gKBMBbL}OZ*{#;*OpFqAnwp?VW|D?@^qKHp1@-PA39&C9jUF81)q)jAi literal 9270 zcma)h^-~lM&^I8ZD2Jrv(W#V_G}2O1B3;srbQ~dgbV;X3gM=UT8ozKo{DHv!fyk!>U7i88_FnDF}lka@pnT1|63ra}77Lxo=5f&5@X8!-B0al#4a`diF|$6dbi1aj?BELv={EMG>`B$af- zHs3I(|3~P`j))n{^TW{K7)Wmm9X<&oz?%Mz9P+@i9MU+@$xI-Ma8ufWF%;+94 zXvzIhu`57@)qoqtVXuID^-tw2>AX4xcNe^p-#OgsphX=Ye3^VKdp=Sy|Pf6>d>h?Ts`IRpA66u`yJ5aYxBDsTHehBGYpvo#| zl0L^x-5ZbA0OmeXQk5>Q=)MbhdK!dgxwfD=Baa@JRfUvvXI5sn!Q?6ZIx1Km9V8RV z#~(qv%2X=?osSX8HbO3Tbj#1O)bPa6>w(GZ;O^OYJp(N~=e-uKQ~57wj-S4>8p~Ps zbS$+JfxyK62H%#{N_Kbg6>7*l^@~55rQ5}{M2Q#Tot1>NYD!}oyOIhMpBZIIk%h^P zUI1rM^qh`x`(~<9vPW``1a+vpUanWvx7{PkmC-6vRNarjVLh1L(b$fSl%QJ{#FmV; zoQ?_L>q6eq5Al%)Y&();-f~la%RnaFwQbLqt)3f@C}lPxeSZ8`3bQn8I@HNM9_kAJ zcpWdt&{d>O=PP-7RZsn`H*B= z0_hsDy@5`lPUxGt#Ia32=-}WH-s4D3^!dlJvlr>i>)mcg#_v4D#6L12{%o4D3XEm| z`^yFHJ+p`gGH)dh_8<-_-S77i$zw}qpG)rEG?$F~=%kl%E{%scUuAbhr91(s@&z$k zw1=p{yh{dCTwJcMQcm+PJ9lWtltj8hmpz<17X+}mX@0-A_M*0w_hgjb9FNnRa`zB{ zm#jIaJY59>EhOzw#Yi$YAk;VzZAv{}`uY95)?N!Yun z*x189x-&gZJR%`#6fl#5c3%YacF#y3lMK(%#BnXqq@gXvu`#Iv>92316T^d0SLX4R zQl1_-62|~l1W~BmNq1Z{5C6-TpGMD1i{+}WeXV`X)I+=&FuQH{!nakYFr9xOcaEE@ zFkBoQEcV2>$75cevb)sDQCFYfu>xO8Wz_fD$~hnfxnde2*>|NXH4=)_tN|@ex1wJH zGk!1HF-CC|6*&nN~rmQSbiasLPbYGows+lGnBs6S|KHTzTBF&;Z;?2+Q+DmN@K<` zEC-?>(^DK~*J&C=c9FEDbs<2^ss8CNd=k31EA90pybH`Kbl)2zl?RRgni=wt+g?#s zrnC3uhdq(F0-Np24NU->p}O`vcaEHI3beKN1#}V;lWA<;8)|I`0XF0r-Ek7uLPTn^ z8W4n$lkhf3CisN~PH%j*)n|@(H;ZnilgGR-O+speR*be$tc|k%HZO$?MycJ2m8DrX zgj>o!p~gLFkKfC-;@BCIUl;fBXc>5_ysxJD#SE)@O`7ETo`J_mSy*pm$J#~K#`14} zxBHYVt0DnUym0kVZ{Mmb(9zbbl4*Pz_v}x2M7p}&XQeYD`g@^s!P9MXs|(hAM(g)I z^-5<={Bndj!HhR6ymI?PPX}y-hWG{YiNO@b5hPy8Sf`2yA-y@o!Q_W>c^?|b{`RwpXzB*W2+u?Aa+?0oHzK*&Y z&AkNVK>~=UosWz&D%fa`adp>x)JfK5RRAI*t9KTCSbBrMA`{p*$$ybNO9Gh$*4RAf z`KU<#W!}3mzyTUN*1fcQk~`}_`Fy@fVXf5bcuQwUhk2B=1B)Ra^l_XT>HP*AULDy`M$E^)w*bk6W%0 zK04X$!eFW7E+}WOp+?=%vf|@>ef1;QqV!LaW!Cx|htsl$d1o`wW!~?-Se1P{#FrbY zF#gT!x8FfqxFs5Lg^Zxveck&&`%o%b40Txi7S}nVept@3OCNm;_3cc(3x~2fk1q(d z{@r`xcoq@_Mby^W*4!Ku9h za(n_V=C8G*>1xCKu@Ng;EJ>y&I4eXr#Az1FLC+4Um^XQ!{Ykhsu`Qi+8F{ZTmXM&= z4$t5HjU@z&GJy|ecCT;pw)oi8Fgp>%8T9Hv9UiZjtQ?ZK8By#?9xQHpM%HHA7eQ}2 zgT65u5{mU#cq<0A@9r#BC(fQ2WhjRP1k2Agm?oedo!!IPx?KwN zT~FryLq+Orm#1Hj1MoEB!M1&$Eg#j-cFc4Q=|nWU_%DTp1XGQ0=p4!4Jm~!?Sr?D- z>7zL$LA*xi4}Ff9zxsFphgY&wFVPD#a#Zd;=Z=kK$^I1J_wW%a<6HjajlLDb)d9XZ z$53jdD7%-0_GL=TBb|Sf()l%00%sPAAoaTjR+|rOj>nPk(bBJa?^qMqOCto+OJ4*I zE%k4f%6pw7$vjwhUrKF#*IDp-ss&&Xef3Qu?l)5df_O=ZBWD)4M34x6T2;+H#A3J_ z;#%7OVVvVESl(}=>L;@n=b`So^A$8k&?6OrpW_t@C(U(?7rC9i_dM2gnyX|O9A>&#{jdvCQ2waA}_zR z^2>SNDg=@D@Sn0-u<>lF7Uej-pc6W%dPZnU*$O9$zMRH@-ESp z*iB;Yt}>M2R-u$j$7cHy&&8tmre&p5p8vT(O1GTswa=mTwpJI!|9Z?p!@|nH4kZcW zAXgufi-RzXoJ+64!{4qHB{QM<>A#3cNP(sw4PpRRJN0PbHntt+zZVv6`M*QD@lnfl zLy5A`6}0x-DT7+c^sxdyua*+aGlsFE-&?MxjT|g)Vw>euDHChoHjB4tZKqpS3njw2 z->P*Pc)pFHz+>md*Yl`sWsNY{{!1WAPUP5Tz_17vbC%@pzm~=2fAwiW~iq|hXS|=N#7mTe|2pjcUQL2&U0X0QV?i}N$;cxQZ+e1 z4D2)RW?^7NQnHxKw|7~49%fkdGH@>I06MYWQK$A0?;{o1NUOALbfI8FYF(J)i%lE4 z>im&4cTfdCfHJwKcD`^s_=WfTmnexwan}b(6VHuI-Vx**i5#qDEjGFKvML$EK@ zUR+DY|NP3>OnSJx_3Vcijqga%&DwRxd2MOEoNx=g32ih7Celmp_J#O{VGGXPSLhd3 z^p_Pndu9q;HwTbL%uUv_W$6UdGXLahgu886bY5_BYc>U#yH_i=OUS;6FWFYjAh=Yi z5N`UG1HaATx=Bo=0)ms~8>a_)Tv8y3kxLl$3J0~Wf~u+A7yH8vkDd4sPT7&y|5#13&&U|r>lOW_WMAMMe*xGp(E@*dl2f%rgnc6h9$d1k zO%(tAO`cT>mnM>2uc*CxK)=2Icr=RN`a+gZbjr6~6AP#=l9 zgflG2YQMNVXyF6s1Ff4wPYtM4VtH{9It;BaJ#d>rVTC>>x|f#RdmJ7Fh~9(*W+3-M zDrs7K)}&d?wYWB0vD_FI^{o(;E#!4Zw9sCJc$i$!VaDmuSv~9_fGF_=By`;AdrxgQ zg{6SPaq)8X!%gJD14UBt~^gaEBxqpl`(4vu3-j(}8>XVr$IgH>cZ>SdOq zljggkFSyEAhTgDDWXOF3+rRo`%x->dTis)pJ)>`FJH%ap0F?TxY!QI)^hH1sWc`043K57BH8s^ z)n+_-G%Lv+0JdQjb59Gr)ur65dzc^nBU)9YzDsc*_AWp>MZ&fAitg35FHEjmPKQmG z_Cu_x6l_iFd)LF-7?%BxlNFSsaT#_fln5!fR*z8pHo!-ahAq0@flj&{jsssYbV-D; zbzR*y!bOH#U2cJ16*oryO7*7OX#>Y*rJY8GCNPH$O9XZxtTqeZqx*xhVQEfl0cvh= zP^!JKD#qeTjJr?kK{I_;(o#>&>-$c{OPi-&q>aK+Y8yr?z!h`Cg4Ip_Kq2iPt}7Opn(kW$^fF26p7#DZzD}1-%lj9 zZSvlWpXr$Fq>3HiHbSNX{JNyRDZHZ1bTep4cXg^a^0si!XA$8qsvj5IO&q@mcNXJ# z5W^P@vy86cDy#vvjap+Z$73IEuGXRGBL4GFTpr66?!^TsZQ@f9)!U56s0;->3*x~J z+8v`O1Q7Q?x9{y5CyzreJ zjN4#!As-e|jqRiB#pk6Uq?&!Y@_vPNR(TUVX+6&T64i}t)571u^(iD_7~=iQIzjBM zp&4#x!W<^`@!3lSksg2Hne zABZ`A9NNfF@`}g$5#<(mDH-(L5Y(SGL9-)&`s^B}`k7NG)4Ht1`4ha1W$%{{2^8?CcI=n8wHbKUKVGk=~{5>_D_m{_*aygHKWk6KY4f+X8 zLs%eoaK3@|k3c`DQhK@X3(=p$?Px9oiLI@^TFO$Z3>3f1M@Ie^SvvZuIwOxr zZ}VHr)Z_C{%l*NdF7F6U06k8N{q*WG`3wErRp1PEgr^>W>wg%aooOpD6pCCl0*cjy zAUBrJO;^7x^lLZ@B0TebEp{&JoS&9=YaQ**XH; zv-o81ZDxQE3&5DjY~lj|g=o|T$2*+b_p5`n&&1} zy7t8(AIn`d_XR$rxy+ZlyHvH+9hLWwW3s+*EC?Hvof~r~N^i{>i2MXBO{AM)&G zow}w<7+czym~fB_Ooxvjf2Su2s3F!uHZs&3iyx{?87*YLn>YFD__SFs(WjwXn!eXm zS^wwqu#5hL$l@ie<83Q74I<|6a@`vR;U-ECWr4T8m;5;F7 z(;xNAnH{=nC-l>`02WXL1pxKPR=(Q!b;=*l^%3R>R$Hm}A{1kLCecOb8k< zJ(@Y_^6oUe`n-4hA=ha(t@?C4XHhvofO2Or{=!B%fPYi_H>J>~Z(PlSVCiD1xfjQ( zhMu=o}EEa&@Tp zbMTWEuHJKx6%?^t|A6ZJWWtZHO*Jc;K+=v`oZHzn>6wtQIyO(?^cd5buID@0L@(&q zqp*~FVs$3Rd6}={vB8FgAyh$aF_4UnZ^Vc_L3mu7U*4mC_h{Ucr7IXX*>8F!;7QWq_rS4-xOQky3dy(W z`qcqW<&#Vb%dQGoQk?_44u9mf*j>*^27OlCQcrc`U0|F>Klc$sTAWj^DbiQAaB)4E zcit2>C)koEBJjrKH#TYBr_BJ?9X&Mn(t3*T5zNHt}G#RnDM|hlo)2Et&`pP zP6mE_*$V0UN(6MIL#X$Q-e0ebDCThjBmqoSctg+4_QS zE2cT7IQz*JsJ|h=MO&10?kQp=cOK6lAvxJ>8>TGJ;GpJ(AJpv;@$=U0my(OCmk~O0 zsJ#44m9n4MP;Odv=jg1G&j?ucGKp1gAGI4vcKmwdukAB-jrT&HVc%j+nfW+>H?>^#1-!9;#{2L1&&7)-`%6V^Z$V> z52HcfkDIWC$*8Tw&%@q#v^_m#M`zyC(EW8Bq$+q$wU#*L3W>4u=M4MVO6cz1-b^9) z2ex;dwIvY_IDian<=-)Ow;dt+OpR`K8!&oeLSX9QcgG69`~<7x1|@I%3cLj(ce01y zL!*H<34{lH7DR&Pu;9(>!^h@C%rbQ4I%LREO#{?=MF;@g<=+)32HiiDQw6gDzmC^C zD%ycIkLNlIE;pG*x~y@Z>Wgm^eYPq7Bw(n|8S!>RSS^j$dCEN;Ys-NhZhxql=Z!EX zmMOk+uonxXtB4*4O4RR4n=YlL8}X0%+C-v9D|a%&p#Mx~WD!MD&HOTa%0ZN-c1a)-1|BLPPrFERJV;>#wi>`p1N-bl$J6*N74edVF8pL zp`;fQpo&Kq>|1D_UJtu|y?|>dVf^DHJ*)LpDo@-a1kEz3L z2|#i8$)Z1A}+1fBoEKoC^Er8wbq>G)Ib>r*H8D!n~InY(XZ z@k9KtGY`gI*yglL((XkeR(dRkCf-Ne5zSq9o@&Hbs1apHvPyl5;4c z0{;NK3}5X2%&ju$w4U~uMP;b4vBmL#yUpydD1yOkHrXnFB+EW4l@2A3MFnr7jnsgKEx<;06f52&p@0G|=N3wzS9}KN z^sK_)-FhabdLLBXrsou=UNr3DH9Jnse?cdd&dOeAXj1cOzt<6Gdkdx0UkJ_=vYXDk zK3V!$DwasO*)gs*v-GifmjO}WFtHYxflgh1-8>QGrbS~lig z$`(9Q*qR;AV=IfZ<}KpYPtY4bv>#NOx?xEL7^_}WT_(duUhfMHg*de-dCj#0?GMZZ zY;%KTtyQWF*jyoO7mqLmGL`iqNs^-v422AOnrZ%i@w?{>Tbt$Cjj*1jHYYR~aVb42 zJpvLh5I7io0q=h>1?Px6B@Z>4d*~Nv-FSh_PEF5A{sA#@zQR~e^nq%ER;Ya(mOjyL zt>)L;hT;a9qS<|scLB+3}@i+}qbopJ0cH=`itdTU-QW%ziIUnhXvD`Nv zitQej?&i`&y487aoAlUhAee@4dxJ?ZU#>hPY4li&ge^8)+>tNtdoj$IuW|>rYV$#A z8?SHo0q9SVc6F4UZ@zoXDbB5XY7zk-0<8#!V z&z$uV)?u1s;-Yg??WT~>61D`&)47k|oAt{7gcSo|$5Xs)_Atf=tF6-iR#H8D^OvG+ zAz?j$oAab|3^T&l-E>AN+;r3G^6%l{^uWnTbAB6#O=0n~#h`Hs(@G!m9hN{Pp(S)A zVf}nB1WkOK;;UrIf(=NLwq>%N=4rHN1l zj^5=Jj!SN1A^B*_{u}}Q{Da^L+n&SjNy9VgUd{HcJqe&zL1NtML*K1<0SQm44(&N0 zG~vy1@Kew>C8 zY5YA(*HaGqSy4Qddwp_6fTtRwnjGf&qh;{?G0NmEQK<7>cs&zez&V;9MDw^O@ZLhz zmU-obL=<1A^Ff5hI200edDSyR*R(<7C=s+_Ni^KtO-F+j3o1k;57#f#Y-KKWlv%&t&seM3F(cjthwB#C=Z z5Gh~3NfbFVRpqK`^|&6^*UW7qURz7g&&Q7k71(}+&7cMeH=B*F-Y>4&3*_H z%YLYDIP|!wLE(-UV}8wto^@Z%3nX$*hWwbWJyN2z$hhFFfg#kc9NI%C902i#g%k7l z_Bk9nHJ4}IY7Vr`fYl#xBv(uNrw!{3yA9h7ht6RMtn{NHq67df&^NzBmZpfLTtj|= zQGUT(enCclAyj@LIJ^>xs|&!<1>*dW#a9Z&j|m#N5KzHKDQxyg-D@vBJVq(*BXcaT zN}TUTm&xtBvi;E5oj_Mm9%R4u*6sfU*7O0%c!g~N&fZ{dPQxM|TF^(LsIr{^%Ryw% Obf{sn$7Pzb-~R)BkGysO diff --git a/matlab/examples/gtsamExamples.m b/matlab/examples/gtsamExamples.m index b3862088a..014885eea 100644 --- a/matlab/examples/gtsamExamples.m +++ b/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');