12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated more matlab examples

release/4.3a0
Richard Roberts 2012-07-23 19:21:05 +00:00
parent 080dd7d57c
commit 62f28bb798
13 changed files with 211 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/examples/LocalizationExample.m
View File

@ -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

2
matlab/examples/OdometryExample.m
View File

@ -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

59
matlab/examples/PlanarSLAMExample.m
View File

@ -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)

61
matlab/examples/PlanarSLAMExample_sampling.m
View File

@ -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;

43
matlab/examples/Pose2SLAMExample.m
View File

@ -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)

33
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -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');

44
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -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'));

22
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -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})

29
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -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'));

41
matlab/examples/Pose3SLAMExample.m
View File

@ -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)

30
matlab/examples/Pose3SLAMExample_graph.m
View File

@ -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;

BIN
matlab/examples/gtsamExamples.fig
View File

Binary file not shown.

34
matlab/examples/gtsamExamples.m
View File

@ -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');