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 be72daf9c..d00370278 100644 Binary files a/matlab/examples/gtsamExamples.fig and b/matlab/examples/gtsamExamples.fig differ 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');