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

Fixed "import gtsam.*" gluttony

release/4.3a0
Frank Dellaert 2012-08-05 19:31:27 +00:00
parent abdf46d494
commit 695523a497
24 changed files with 93 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
matlab/gtsam_examples/LocalizationExample.m
View File

@ -10,23 +10,23 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - Robot poses are facing along the X axis (horizontal, to the right in 2D) % - Robot poses are facing along the X axis (horizontal, to the right in 2D)
% - The robot moves 2 meters each step % - The robot moves 2 meters each step
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = gtsam.NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add two odometry factors %% Add two odometry factors
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
%% Add three "GPS" measurements %% Add three "GPS" measurements
import gtsam.*
% We use Pose2 Priors here with high variance on theta % We use Pose2 Priors here with high variance on theta
priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
graph.add(PriorFactorPose2(1, Pose2(0.0, 0.0, 0.0), priorNoise)); graph.add(PriorFactorPose2(1, Pose2(0.0, 0.0, 0.0), priorNoise));
@ -37,7 +37,6 @@ graph.add(PriorFactorPose2(3, Pose2(4.0, 0.0, 0.0), priorNoise));
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
@ -45,17 +44,15 @@ initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
result.print(sprintf('\nFinal result:\n ')); result.print(sprintf('\nFinal result:\n '));
%% Plot trajectory and covariance ellipses %% Plot trajectory and covariance ellipses
import gtsam.*
cla; cla;
hold on; hold on;
gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); plot2DTrajectory(result, [], Marginals(graph, result));
axis([-0.6 4.8 -1 1]) axis([-0.6 4.8 -1 1])
axis equal axis equal

10
matlab/gtsam_examples/OdometryExample.m
View File

@ -10,22 +10,22 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - Robot poses are facing along the X axis (horizontal, to the right in 2D) % - Robot poses are facing along the X axis (horizontal, to the right in 2D)
% - The robot moves 2 meters each step % - The robot moves 2 meters each step
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = gtsam.NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
import gtsam.*
priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add two odometry factors %% Add two odometry factors
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
@ -35,7 +35,6 @@ graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
@ -48,11 +47,10 @@ result = optimizer.optimizeSafely();
result.print(sprintf('\nFinal result:\n ')); result.print(sprintf('\nFinal result:\n '));
%% Plot trajectory and covariance ellipses %% Plot trajectory and covariance ellipses
import gtsam.*
cla; cla;
hold on; hold on;
gtsam.plot2DTrajectory(result, [], Marginals(graph, result)); plot2DTrajectory(result, [], Marginals(graph, result));
axis([-0.6 4.8 -1 1]) axis([-0.6 4.8 -1 1])
axis equal axis equal

14
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -11,6 +11,8 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -20,28 +22,24 @@
% - Landmarks are 2 meters away from the robot trajectory % - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables %% Create keys for variables
import gtsam.*
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2); j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph container and add factors to it %% Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add prior %% Add prior
import gtsam.*
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
%% Add bearing/range measurement factors %% Add bearing/range measurement factors
import gtsam.*
degrees = pi/180; degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
@ -52,7 +50,6 @@ graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
graph.print(sprintf('\nFull graph:\n')); graph.print(sprintf('\nFull graph:\n'));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2));
@ -68,12 +65,11 @@ result = optimizer.optimizeSafely();
result.print(sprintf('\nFinal result:\n')); result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
import gtsam.*
cla;hold on cla;hold on
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
gtsam.plot2DTrajectory(result, [], marginals); plot2DTrajectory(result, [], marginals);
gtsam.plot2DPoints(result, [], marginals); plot2DPoints(result, [], marginals);
plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); plot([result.at(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(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');

13
matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -11,8 +11,9 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create the same factor graph as in PlanarSLAMExample
import gtsam.* import gtsam.*
%% Create the same factor graph as in PlanarSLAMExample
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
@ -24,7 +25,6 @@ graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
%% Except, for measurements we offer a choice %% Except, for measurements we offer a choice
import gtsam.*
j1 = symbol('l',1); j2 = symbol('l',2); j1 = symbol('l',1); j2 = symbol('l',2);
degrees = pi/180; degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
@ -39,7 +39,6 @@ end
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
%% Initialize MCMC sampler with ground truth %% Initialize MCMC sampler with ground truth
import gtsam.*
sample = Values; sample = Values;
sample.insert(i1, Pose2(0,0,0)); sample.insert(i1, Pose2(0,0,0));
sample.insert(i2, Pose2(2,0,0)); sample.insert(i2, Pose2(2,0,0));
@ -48,12 +47,11 @@ sample.insert(j1, Point2(2,2));
sample.insert(j2, Point2(4,2)); sample.insert(j2, Point2(4,2));
%% Calculate and plot Covariance Ellipses %% Calculate and plot Covariance Ellipses
import gtsam.*
cla;hold on cla;hold on
marginals = Marginals(graph, sample); marginals = Marginals(graph, sample);
gtsam.plot2DTrajectory(sample, [], marginals); plot2DTrajectory(sample, [], marginals);
gtsam.plot2DPoints(sample, [], marginals); plot2DPoints(sample, [], marginals);
for j=1:2 for j=1:2
key = symbol('l',j); key = symbol('l',j);
@ -68,10 +66,9 @@ plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'
view(2); axis auto; axis equal view(2); axis auto; axis equal
%% Do Sampling on point 2 %% Do Sampling on point 2
import gtsam.*
N=1000; N=1000;
for s=1:N for s=1:N
delta = S{2}*randn(2,1); delta = S{2}*randn(2,1);
proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2)); proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
gtsam.plotPoint2(proposedPoint,'k.') plotPoint2(proposedPoint,'k.')
end end

6
matlab/gtsam_examples/Pose2SLAMExample.m
View File

@ -12,14 +12,14 @@
% @author Chris Beall % @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
% - We have full odometry for measurements % - We have full odometry for measurements
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
import gtsam.*
%% Create graph container and add factors to it %% Create graph container and add factors to it
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
@ -60,7 +60,7 @@ hold on
plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-'); plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
gtsam.plot2DTrajectory(result, [], marginals); plot2DTrajectory(result, [], marginals);
for i=1:5,marginals.marginalCovariance(i),end for i=1:5,marginals.marginalCovariance(i),end
axis equal axis equal
axis tight axis tight

6
matlab/gtsam_examples/Pose2SLAMExample_advanced.m
View File

@ -14,6 +14,8 @@
% @author Can Erdogan % @author Can Erdogan
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -24,14 +26,12 @@
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add prior %% Add prior
import gtsam.*
% gaussian for prior % gaussian for prior
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
import gtsam.*
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); 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) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
@ -39,7 +39,6 @@ graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
%% Add measurements %% Add measurements
import gtsam.*
% general noisemodel for measurements % general noisemodel for measurements
measurementNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); measurementNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
@ -47,7 +46,6 @@ measurementNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.print('full graph'); graph.print('full graph');
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));

14
matlab/gtsam_examples/Pose2SLAMExample_circle.m
View File

@ -10,14 +10,14 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
import gtsam.* import gtsam.*
hexagon = gtsam.circlePose2(6,1.0);
%% Create a hexagon of poses
hexagon = circlePose2(6,1.0);
p0 = hexagon.at(0); p0 = hexagon.at(0);
p1 = hexagon.at(1); p1 = hexagon.at(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
import gtsam.*
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose2(0, p0)); fg.add(NonlinearEqualityPose2(0, p0));
delta = p0.between(p1); delta = p0.between(p1);
@ -30,7 +30,6 @@ fg.add(BetweenFactorPose2(4,5, delta, covariance));
fg.add(BetweenFactorPose2(5,0, delta, covariance)); fg.add(BetweenFactorPose2(5,0, delta, covariance));
%% Create initial config %% Create initial config
import gtsam.*
initial = Values; initial = Values;
initial.insert(0, p0); initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
@ -40,18 +39,15 @@ initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate %% Plot Initial Estimate
import gtsam.*
cla cla
gtsam.plot2DTrajectory(initial, 'g*-'); axis equal plot2DTrajectory(initial, 'g*-'); axis equal
%% optimize %% optimize
import gtsam.*
optimizer = DoglegOptimizer(fg, initial); optimizer = DoglegOptimizer(fg, initial);
result = optimizer.optimizeSafely; result = optimizer.optimizeSafely;
%% Show Result %% Show Result
import gtsam.* hold on; plot2DTrajectory(result, 'b*-');
hold on; gtsam.plot2DTrajectory(result, 'b*-');
view(2); view(2);
axis([-1.5 1.5 -1.5 1.5]); axis([-1.5 1.5 -1.5 1.5]);
result.print(sprintf('\nFinal result:\n')); result.print(sprintf('\nFinal result:\n'));

21
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

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

5
matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

@ -10,6 +10,8 @@
% @author Yong-Dian Jian % @author Yong-Dian Jian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -20,14 +22,12 @@
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add prior %% Add prior
import gtsam.*
% gaussian for prior % gaussian for prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
import gtsam.*
% general noisemodel for odometry % general noisemodel for odometry
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
@ -36,7 +36,6 @@ 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)); graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
%% Add pose constraint %% Add pose constraint
import gtsam.*
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));

14
matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -10,14 +10,14 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
import gtsam.* import gtsam.*
hexagon = gtsam.circlePose3(6,1.0);
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0); p0 = hexagon.at(0);
p1 = hexagon.at(1); p1 = hexagon.at(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
import gtsam.*
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose3(0, p0)); fg.add(NonlinearEqualityPose3(0, p0));
delta = p0.between(p1); delta = p0.between(p1);
@ -30,7 +30,6 @@ fg.add(BetweenFactorPose3(4,5, delta, covariance));
fg.add(BetweenFactorPose3(5,0, delta, covariance)); fg.add(BetweenFactorPose3(5,0, delta, covariance));
%% Create initial config %% Create initial config
import gtsam.*
initial = Values; initial = Values;
s = 0.10; s = 0.10;
initial.insert(0, p0); initial.insert(0, p0);
@ -41,18 +40,15 @@ initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
%% Plot Initial Estimate %% Plot Initial Estimate
import gtsam.*
cla cla
gtsam.plot3DTrajectory(initial, 'g-*'); plot3DTrajectory(initial, 'g-*');
%% optimize %% optimize
import gtsam.*
optimizer = DoglegOptimizer(fg, initial); optimizer = DoglegOptimizer(fg, initial);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
%% Show Result %% Show Result
import gtsam.* hold on; plot3DTrajectory(result, 'b-*', true, 0.3);
hold on; gtsam.plot3DTrajectory(result, 'b-*', true, 0.3);
axis([-2 2 -2 2 -1 1]); axis([-2 2 -2 2 -1 1]);
axis equal axis equal
view(-37,40) view(-37,40)

15
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

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

12
matlab/gtsam_examples/SBAExample.m
View File

@ -10,6 +10,8 @@
% @author Yong-Dian Jian % @author Yong-Dian Jian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc... % - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc...
% - Cameras are on a circle around the cube, pointing at the world origin % - Cameras are on a circle around the cube, pointing at the world origin
@ -22,7 +24,7 @@ options.nrCameras = 10;
options.showImages = false; options.showImages = false;
%% Generate data %% Generate data
[data,truth] = gtsam.VisualISAMGenerateData(options); [data,truth] = VisualISAMGenerateData(options);
measurementNoiseSigma = 1.0; measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1; pointNoiseSigma = 0.1;
@ -30,12 +32,10 @@ cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
0.001*ones(1,5)]'; 0.001*ones(1,5)]';
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add factors for all measurements %% Add factors for all measurements
import gtsam.*
measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
@ -45,7 +45,6 @@ for i=1:length(data.Z)
end end
%% Add Gaussian priors for a pose and a landmark to constrain the system %% Add Gaussian priors for a pose and a landmark to constrain the system
import gtsam.*
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise)); graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
@ -58,7 +57,6 @@ graph.print(sprintf('\nFactor graph:\n'));
%% Initialize cameras and points close to ground truth in this example %% Initialize cameras and points close to ground truth in this example
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
@ -72,8 +70,6 @@ end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Fine grain optimization, allowing user to iterate step by step %% Fine grain optimization, allowing user to iterate step by step
import gtsam.*
parameters = LevenbergMarquardtParams; parameters = LevenbergMarquardtParams;
parameters.setlambdaInitial(1.0); parameters.setlambdaInitial(1.0);
parameters.setVerbosityLM('trylambda'); parameters.setVerbosityLM('trylambda');
@ -86,5 +82,3 @@ end
result = optimizer.values(); result = optimizer.values();
result.print(sprintf('\nFinal result:\n ')); result.print(sprintf('\nFinal result:\n '));

15
matlab/gtsam_examples/SFMExample.m
View File

@ -10,6 +10,8 @@
% @author Duy-Nguyen Ta % @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc... % - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc...
% - Cameras are on a circle around the cube, pointing at the world origin % - Cameras are on a circle around the cube, pointing at the world origin
@ -22,18 +24,16 @@ options.nrCameras = 10;
options.showImages = false; options.showImages = false;
%% Generate data %% Generate data
[data,truth] = gtsam.VisualISAMGenerateData(options); [data,truth] = VisualISAMGenerateData(options);
measurementNoiseSigma = 1.0; measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1; pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add factors for all measurements %% Add factors for all measurements
import gtsam.*
measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
@ -43,7 +43,6 @@ for i=1:length(data.Z)
end end
%% Add Gaussian priors for a pose and a landmark to constrain the system %% Add Gaussian priors for a pose and a landmark to constrain the system
import gtsam.*
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
@ -53,7 +52,6 @@ graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
%% Initialize cameras and points close to ground truth in this example %% Initialize cameras and points close to ground truth in this example
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
@ -66,8 +64,6 @@ end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Fine grain optimization, allowing user to iterate step by step %% Fine grain optimization, allowing user to iterate step by step
import gtsam.*
parameters = LevenbergMarquardtParams; parameters = LevenbergMarquardtParams;
parameters.setlambdaInitial(1.0); parameters.setlambdaInitial(1.0);
parameters.setVerbosityLM('trylambda'); parameters.setVerbosityLM('trylambda');
@ -81,13 +77,12 @@ result = optimizer.values();
result.print(sprintf('\nFinal result:\n ')); result.print(sprintf('\nFinal result:\n '));
%% Plot results with covariance ellipses %% Plot results with covariance ellipses
import gtsam.*
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
cla cla
hold on; hold on;
gtsam.plot3DPoints(result, [], marginals); plot3DPoints(result, [], marginals);
gtsam.plot3DTrajectory(result, '*', 1, 8, marginals); plot3DTrajectory(result, '*', 1, 8, marginals);
axis([-40 40 -40 40 -10 20]);axis equal axis([-40 40 -40 40 -10 20]);axis equal
view(3) view(3)

15
matlab/gtsam_examples/StereoVOExample.m
View File

@ -10,6 +10,8 @@
% @author Chris Beall % @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - For simplicity this example is in the camera's coordinate frame % - For simplicity this example is in the camera's coordinate frame
% - X: right, Y: down, Z: forward % - X: right, Y: down, Z: forward
@ -18,27 +20,22 @@
% - No noise on measurements % - No noise on measurements
%% Create keys for variables %% Create keys for variables
import gtsam.*
x1 = symbol('x',1); x2 = symbol('x',2); x1 = symbol('x',1); x2 = symbol('x',2);
l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
%% Create graph container and add factors to it %% Create graph container and add factors to it
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% add a constraint on the starting pose %% add a constraint on the starting pose
import gtsam.*
first_pose = Pose3(); first_pose = Pose3();
graph.add(NonlinearEqualityPose3(x1, first_pose)); graph.add(NonlinearEqualityPose3(x1, first_pose));
%% Create realistic calibration and measurement noise model %% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
import gtsam.*
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
%% Add measurements %% Add measurements
import gtsam.*
% pose 1 % pose 1
graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)); graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K));
graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)); graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K));
@ -51,7 +48,6 @@ graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l
%% Create initial estimate for camera poses and landmarks %% Create initial estimate for camera poses and landmarks
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(x1, first_pose); initialEstimate.insert(x1, first_pose);
% noisy estimate for pose 2 % noisy estimate for pose 2
@ -62,7 +58,6 @@ initialEstimate.insert(l3, Point3( 0,-.5, 5));
%% optimize %% optimize
fprintf(1,'Optimizing\n'); tic fprintf(1,'Optimizing\n'); tic
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
toc toc
@ -75,6 +70,6 @@ axis equal
view(-38,12) view(-38,12)
camup([0;1;0]); camup([0;1;0]);
gtsam.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); plot3DTrajectory(initialEstimate, 'r', 1, 0.3);
gtsam.plot3DTrajectory(result, 'g', 1, 0.3); plot3DTrajectory(result, 'g', 1, 0.3);
gtsam.plot3DPoints(result); plot3DPoints(result);

19
matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -10,10 +10,11 @@
% @author Chris Beall % @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Load calibration
import gtsam.* import gtsam.*
%% Load calibration
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
calib = dlmread(gtsam.findExampleDataFile('VO_calibration.txt')); calib = dlmread(findExampleDataFile('VO_calibration.txt'));
K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
@ -24,9 +25,8 @@ initial = Values;
%% load the initial poses from VO %% load the initial poses from VO
% row format: camera_id 4x4 pose (row, major) % row format: camera_id 4x4 pose (row, major)
import gtsam.*
fprintf(1,'Reading data\n'); fprintf(1,'Reading data\n');
cameras = dlmread(gtsam.findExampleDataFile('VO_camera_poses_large.txt')); cameras = dlmread(findExampleDataFile('VO_camera_poses_large.txt'));
for i=1:size(cameras,1) for i=1:size(cameras,1)
pose = Pose3(reshape(cameras(i,2:17),4,4)'); pose = Pose3(reshape(cameras(i,2:17),4,4)');
initial.insert(symbol('x',cameras(i,1)),pose); initial.insert(symbol('x',cameras(i,1)),pose);
@ -34,8 +34,7 @@ end
%% load stereo measurements and initialize landmarks %% load stereo measurements and initialize landmarks
% camera_id landmark_id uL uR v X Y Z % camera_id landmark_id uL uR v X Y Z
import gtsam.* measurements = dlmread(findExampleDataFile('VO_stereo_factors_large.txt'));
measurements = dlmread(gtsam.findExampleDataFile('VO_stereo_factors_large.txt'));
fprintf(1,'Creating Graph\n'); tic fprintf(1,'Creating Graph\n'); tic
for i=1:size(measurements,1) for i=1:size(measurements,1)
@ -54,13 +53,11 @@ end
toc toc
%% add a constraint on the starting pose %% add a constraint on the starting pose
import gtsam.*
key = symbol('x',1); key = symbol('x',1);
first_pose = initial.at(key); first_pose = initial.at(key);
graph.add(NonlinearEqualityPose3(key, first_pose)); graph.add(NonlinearEqualityPose3(key, first_pose));
%% optimize %% optimize
import gtsam.*
fprintf(1,'Optimizing\n'); tic fprintf(1,'Optimizing\n'); tic
optimizer = LevenbergMarquardtOptimizer(graph, initial); optimizer = LevenbergMarquardtOptimizer(graph, initial);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
@ -69,9 +66,9 @@ toc
%% visualize initial trajectory, final trajectory, and final points %% visualize initial trajectory, final trajectory, and final points
cla; hold on; cla; hold on;
gtsam.plot3DTrajectory(initial, 'r', 1, 0.5); plot3DTrajectory(initial, 'r', 1, 0.5);
gtsam.plot3DTrajectory(result, 'g', 1, 0.5); plot3DTrajectory(result, 'g', 1, 0.5);
gtsam.plot3DPoints(result); plot3DPoints(result);
axis([-5 20 -20 20 0 100]); axis([-5 20 -20 20 0 100]);
axis equal axis equal

12
matlab/gtsam_examples/VisualISAMExample.m
View File

@ -10,6 +10,8 @@
% @author Duy-Nguyen Ta % @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
% Data Options % Data Options
options.triangle = false; options.triangle = false;
options.nrCameras = 20; options.nrCameras = 20;
@ -32,17 +34,17 @@ options.saveFigures = false;
options.saveDotFiles = false; options.saveDotFiles = false;
%% Generate data %% Generate data
[data,truth] = gtsam.VisualISAMGenerateData(options); [data,truth] = VisualISAMGenerateData(options);
%% Initialize iSAM with the first pose and points %% Initialize iSAM with the first pose and points
[noiseModels,isam,result,nextPose] = gtsam.VisualISAMInitialize(data,truth,options); [noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options);
cla; cla;
gtsam.VisualISAMPlot(truth, data, isam, result, options) VisualISAMPlot(truth, data, isam, result, options)
%% Main loop for iSAM: stepping through all poses %% Main loop for iSAM: stepping through all poses
for frame_i=3:options.nrCameras for frame_i=3:options.nrCameras
[isam,result,nextPose] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); [isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
if mod(frame_i,options.drawInterval)==0 if mod(frame_i,options.drawInterval)==0
gtsam.VisualISAMPlot(truth, data, isam, result, options) VisualISAMPlot(truth, data, isam, result, options)
end end
end end

8
matlab/gtsam_tests/testLocalizationExample.m
View File

@ -10,12 +10,12 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
import gtsam.* import gtsam.*
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add two odometry factors %% Add two odometry factors
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
@ -23,7 +23,6 @@ graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
%% Add three "GPS" measurements %% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta % We use Pose2 Priors here with high variance on theta
import gtsam.*
groundTruth = Values; groundTruth = Values;
groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
@ -34,19 +33,16 @@ for i=1:3
end end
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
import gtsam.*
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
P={}; P={};
for i=1:result.size() for i=1:result.size()

8
matlab/gtsam_tests/testOdometryExample.m
View File

@ -10,38 +10,34 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
import gtsam.* import gtsam.*
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add a Gaussian prior on pose x_1 %% Add a Gaussian prior on pose x_1
import gtsam.*
priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add two odometry factors %% Add two odometry factors
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
marginals.marginalCovariance(1); marginals.marginalCovariance(1);
%% Check first pose equality %% Check first pose equality
import gtsam.*
pose_1 = result.at(1); pose_1 = result.at(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

10
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -11,6 +11,8 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -20,29 +22,24 @@
% - Landmarks are 2 meters away from the robot trajectory % - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables %% Create keys for variables
import gtsam.*
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2); j1 = symbol('l',1); j2 = symbol('l',2);
%% Create graph container and add factors to it %% Create graph container and add factors to it
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add prior %% Add prior
import gtsam.*
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
import gtsam.*
odometry = Pose2(2.0, 0.0, 0.0); odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
%% Add bearing/range measurement factors %% Add bearing/range measurement factors
import gtsam.*
degrees = pi/180; degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
@ -50,7 +47,6 @@ graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2));
@ -59,13 +55,11 @@ initialEstimate.insert(j1, Point2(1.8, 2.1));
initialEstimate.insert(j2, Point2(4.1, 1.8)); initialEstimate.insert(j2, Point2(4.1, 1.8));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
%% Check first pose and point equality %% Check first pose and point equality
import gtsam.*
pose_1 = result.at(symbol('x',1)); pose_1 = result.at(symbol('x',1));
marginals.marginalCovariance(symbol('x',1)); marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

19
matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -12,6 +12,8 @@
% @author Chris Beall % @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - All values are axis aligned % - All values are axis aligned
% - Robot poses are facing along the X axis (horizontal, to the right in images) % - Robot poses are facing along the X axis (horizontal, to the right in images)
@ -19,11 +21,9 @@
% - The robot is on a grid, moving 2 meters each step % - The robot is on a grid, moving 2 meters each step
%% Create graph container and add factors to it %% Create graph container and add factors to it
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add prior %% Add prior
import gtsam.*
% gaussian for prior % gaussian for prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
@ -31,7 +31,6 @@ graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
%% Add odometry %% Add odometry
% general noisemodel for odometry % general noisemodel for odometry
import gtsam.*
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), 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(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
@ -39,12 +38,10 @@ 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)); graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
%% Add pose constraint %% Add pose constraint
import gtsam.*
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
%% Initialize to noisy points %% Initialize to noisy points
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 ));
@ -53,26 +50,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi ));
initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely(); result = optimizer.optimizeSafely();
%% Optimize using SPCG
%import gtsam.*
%params = LevenbergMarquardtParams;
%params.setLinearSolverType('CONJUGATE_GRADIENT');
%optimizerSPCG = LevenbergMarquardtOptimizer(graph, initialEstimate, params);
%resultSPCG = optimizerSPCG.optimize();
%% Plot Covariance Ellipses %% Plot Covariance Ellipses
import gtsam.*
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1); P = marginals.marginalCovariance(1);
pose_1 = result.at(1); pose_1 = result.at(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
%poseSPCG_1 = resultSPCG.at(1);
%CHECK('poseSPCG_1.equals(Pose2,1e-4)',poseSPCG_1.equals(Pose2,1e-4));

6
matlab/gtsam_tests/testPose3SLAMExample.m
View File

@ -10,14 +10,14 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
import gtsam.* import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0); hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0); p0 = hexagon.at(0);
p1 = hexagon.at(1); p1 = hexagon.at(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
import gtsam.*
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose3(0, p0)); fg.add(NonlinearEqualityPose3(0, p0));
delta = p0.between(p1); delta = p0.between(p1);
@ -30,7 +30,6 @@ fg.add(BetweenFactorPose3(4,5, delta, covariance));
fg.add(BetweenFactorPose3(5,0, delta, covariance)); fg.add(BetweenFactorPose3(5,0, delta, covariance));
%% Create initial config %% Create initial config
import gtsam.*
initial = Values; initial = Values;
s = 0.10; s = 0.10;
initial.insert(0, p0); initial.insert(0, p0);
@ -41,7 +40,6 @@ initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
%% optimize %% optimize
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(fg, initial); optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely; result = optimizer.optimizeSafely;

5
matlab/gtsam_tests/testSFMExample.m
View File

@ -25,7 +25,6 @@ poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% Add factors for all measurements %% Add factors for all measurements
import gtsam.*
measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
@ -40,7 +39,6 @@ pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise)); graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
%% Initial estimate %% Initial estimate
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose; pose_i = truth.cameras{i}.pose;
@ -52,7 +50,6 @@ for j=1:size(truth.points,2)
end end
%% Optimization %% Optimization
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
for i=1:5 for i=1:5
optimizer.iterate(); optimizer.iterate();
@ -60,13 +57,11 @@ end
result = optimizer.values(); result = optimizer.values();
%% Marginalization %% Marginalization
import gtsam.*
marginals = Marginals(graph, result); marginals = Marginals(graph, result);
marginals.marginalCovariance(symbol('p',1)); marginals.marginalCovariance(symbol('p',1));
marginals.marginalCovariance(symbol('x',1)); marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth %% Check optimized results, should be equal to ground truth
import gtsam.*
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i)); pose_i = result.at(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))

11
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -10,6 +10,8 @@
% @author Chris Beall % @author Chris Beall
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Assumptions %% Assumptions
% - For simplicity this example is in the camera's coordinate frame % - For simplicity this example is in the camera's coordinate frame
% - X: right, Y: down, Z: forward % - X: right, Y: down, Z: forward
@ -18,27 +20,22 @@
% - No noise on measurements % - No noise on measurements
%% Create keys for variables %% Create keys for variables
import gtsam.*
x1 = symbol('x',1); x2 = symbol('x',2); x1 = symbol('x',1); x2 = symbol('x',2);
l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
%% Create graph container and add factors to it %% Create graph container and add factors to it
import gtsam.*
graph = NonlinearFactorGraph; graph = NonlinearFactorGraph;
%% add a constraint on the starting pose %% add a constraint on the starting pose
import gtsam.*
first_pose = Pose3(); first_pose = Pose3();
graph.add(NonlinearEqualityPose3(x1, first_pose)); graph.add(NonlinearEqualityPose3(x1, first_pose));
%% Create realistic calibration and measurement noise model %% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
import gtsam.*
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
%% Add measurements %% Add measurements
import gtsam.*
% pose 1 % pose 1
graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)); graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K));
graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)); graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K));
@ -49,9 +46,7 @@ graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l
graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)); graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K));
graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)); graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K));
%% Create initial estimate for camera poses and landmarks %% Create initial estimate for camera poses and landmarks
import gtsam.*
initialEstimate = Values; initialEstimate = Values;
initialEstimate.insert(x1, first_pose); initialEstimate.insert(x1, first_pose);
% noisy estimate for pose 2 % noisy estimate for pose 2
@ -62,12 +57,10 @@ initialEstimate.insert(l2, Point3(-1, 1, 5));
initialEstimate.insert(l3, Point3( 0,-.5, 5)); initialEstimate.insert(l3, Point3( 0,-.5, 5));
%% optimize %% optimize
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize(); result = optimizer.optimize();
%% check equality for the first pose and point %% check equality for the first pose and point
import gtsam.*
pose_x1 = result.at(x1); pose_x1 = result.at(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4)); CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));

5
matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -10,6 +10,8 @@
% @author Duy-Nguyen Ta % @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
% Data Options % Data Options
options.triangle = false; options.triangle = false;
options.nrCameras = 20; options.nrCameras = 20;
@ -32,15 +34,12 @@ options.saveFigures = false;
options.saveDotFiles = false; options.saveDotFiles = false;
%% Generate data %% Generate data
import gtsam.*
[data,truth] = VisualISAMGenerateData(options); [data,truth] = VisualISAMGenerateData(options);
%% Initialize iSAM with the first pose and points %% Initialize iSAM with the first pose and points
import gtsam.*
[noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options); [noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options);
%% Main loop for iSAM: stepping through all poses %% Main loop for iSAM: stepping through all poses
import gtsam.*
for frame_i=3:options.nrCameras for frame_i=3:options.nrCameras
[isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); [isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
end end