Fixed "import gtsam.*" gluttony

matlab/gtsam_examples/LocalizationExample.m

@@ -10,23 +10,23 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - Robot poses are facing along the X axis (horizontal, to the right in 2D)
 %  - The robot moves 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)
-graph = gtsam.NonlinearFactorGraph;
+graph = NonlinearFactorGraph;
 
 %% 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)
 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(2, 3, odometry, odometryNoise));
 
 %% Add three "GPS" measurements
-import gtsam.*
 % We use Pose2 Priors here with high variance on theta
 priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
 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'));
 
 %% Initialize to noisy points
-import gtsam.*
 initialEstimate = Values;
 initialEstimate.insert(1, Pose2(0.5, 0.0, 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  '));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimizeSafely();
 result.print(sprintf('\nFinal result:\n  '));
 
 %% Plot trajectory and covariance ellipses
-import gtsam.*
 cla;
 hold on;
 
-gtsam.plot2DTrajectory(result, [], Marginals(graph, result));
+plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/gtsam_examples/OdometryExample.m

@@ -10,22 +10,22 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - Robot poses are facing along the X axis (horizontal, to the right in 2D)
 %  - The robot moves 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)
-graph = gtsam.NonlinearFactorGraph;
+graph = NonlinearFactorGraph;
 
 %% 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.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
 
 %% 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)
 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));
@@ -35,7 +35,6 @@ graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-import gtsam.*
 initialEstimate = Values;
 initialEstimate.insert(1, Pose2(0.5, 0.0, 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  '));
 
 %% Plot trajectory and covariance ellipses
-import gtsam.*
 cla;
 hold on;
 
-gtsam.plot2DTrajectory(result, [], Marginals(graph, result));
+plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/gtsam_examples/PlanarSLAMExample.m

@@ -11,6 +11,8 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - All values are axis aligned
 %  - 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
 
 %% Create keys for variables
-import gtsam.*
 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 = gtsam.NonlinearFactorGraph;
+graph = 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.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.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.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'));
 
 %% Initialize to noisy points
-import gtsam.*
 initialEstimate = Values;
 initialEstimate.insert(i1, Pose2(0.5, 0.0, 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'));
 
 %% Plot Covariance Ellipses
-import gtsam.*
 cla;hold on
 
 marginals = Marginals(graph, result);
-gtsam.plot2DTrajectory(result, [], marginals);
-gtsam.plot2DPoints(result, [], marginals);
+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-');

matlab/gtsam_examples/PlanarSLAMExample_sampling.m

@@ -11,8 +11,9 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Create the same factor graph as in PlanarSLAMExample
 import gtsam.*
+
+%% Create the same factor graph as in PlanarSLAMExample
 i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
 graph = NonlinearFactorGraph;
 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));
 
 %% Except, for measurements we offer a choice
-import gtsam.*
 j1 = symbol('l',1); j2 = symbol('l',2);
 degrees = pi/180;
 brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
@@ -39,7 +39,6 @@ end
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
 
 %% Initialize MCMC sampler with ground truth
-import gtsam.*
 sample = Values;
 sample.insert(i1, Pose2(0,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));
 
 %% Calculate and plot Covariance Ellipses
-import gtsam.*
 cla;hold on
 marginals = Marginals(graph, sample);
 
-gtsam.plot2DTrajectory(sample, [], marginals);
-gtsam.plot2DPoints(sample, [], marginals);
+plot2DTrajectory(sample, [], marginals);
+plot2DPoints(sample, [], marginals);
 
 for j=1:2
     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
 
 %% Do Sampling on point 2
-import gtsam.*
 N=1000;
 for s=1:N
     delta = S{2}*randn(2,1);
     proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
-    gtsam.plotPoint2(proposedPoint,'k.')
+    plotPoint2(proposedPoint,'k.')
 end

matlab/gtsam_examples/Pose2SLAMExample.m

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

matlab/gtsam_examples/Pose2SLAMExample_advanced.m

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

matlab/gtsam_examples/Pose2SLAMExample_circle.m

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

matlab/gtsam_examples/Pose2SLAMExample_graph.m

@@ -10,45 +10,42 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Find data file
-datafile = gtsam.findExampleDataFile('w100-odom.graph');
+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]);
 maxID = 0;
 addNoise = false;
 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'));
 
 %% 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); % prior mean is at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
 graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph
 
 %% Plot Initial Estimate
-import gtsam.*
 cla
-gtsam.plot2DTrajectory(initial, 'g-*'); axis equal
+plot2DTrajectory(initial, 'g-*'); axis equal
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = optimizer.optimizeSafely;
-hold on; gtsam.plot2DTrajectory(result, 'b-*');
+hold on; plot2DTrajectory(result, 'b-*');
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
-import gtsam.*
 marginals = Marginals(graph, result);
 P={};
 for i=1:result.size()-1
     pose_i = result.at(i);
     P{i}=marginals.marginalCovariance(i);
-    gtsam.plotPose2(pose_i,'b',P{i})
+    plotPose2(pose_i,'b',P{i})
 end
 view(2)
-axis([-15 10 -15 10]); axis equal;
+axis tight; axis equal;
 fprintf(1,'%.5f %.5f %.5f\n',P{99})

matlab/gtsam_examples/Pose2SLAMwSPCG.m

@@ -10,6 +10,8 @@
 % @author Yong-Dian Jian
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - All values are axis aligned
 %  - Robot poses are facing along the X axis (horizontal, to the right in images)
@@ -20,14 +22,12 @@
 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.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.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));
 
 %% Add pose constraint
-import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 

matlab/gtsam_examples/Pose3SLAMExample.m

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

matlab/gtsam_examples/Pose3SLAMExample_graph.m

@@ -10,33 +10,32 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Find data file
 N = 2500;
 % dataset = 'sphere_smallnoise.graph';
 % dataset = 'sphere2500_groundtruth.txt';
 dataset = 'sphere2500.txt';
 
-datafile = gtsam.findExampleDataFile(dataset);
+datafile = findExampleDataFile(dataset);
 
 %% 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]);
-[graph,initial]=gtsam.load3D(datafile,model,true,N);
+[graph,initial]=load3D(datafile,model,true,N);
 
 %% Plot Initial Estimate
-import gtsam.*
 cla
 first = initial.at(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
-gtsam.plot3DTrajectory(initial,'g-',false);
+plot3DTrajectory(initial,'g-',false);
 drawnow;
 
 %% Read again, now with all constraints, and optimize
-import gtsam.*
-graph = gtsam.load3D(datafile, model, false, N);
+graph = load3D(datafile, model, false, N);
 graph.add(NonlinearEqualityPose3(0, first));
 optimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = optimizer.optimizeSafely();
-gtsam.plot3DTrajectory(result, 'r-', false); axis equal;
+plot3DTrajectory(result, 'r-', false); axis equal;
 
 view(3); axis equal;

matlab/gtsam_examples/SBAExample.m

@@ -10,6 +10,8 @@
 % @author Yong-Dian Jian
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - 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
@@ -22,7 +24,7 @@ options.nrCameras = 10;
 options.showImages = false;
 
 %% Generate data
-[data,truth] = gtsam.VisualISAMGenerateData(options);
+[data,truth] = VisualISAMGenerateData(options);
 
 measurementNoiseSigma = 1.0;
 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)]';
 
 %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
-import gtsam.*
 graph = NonlinearFactorGraph;
 
  
 %% Add factors for all measurements
-import gtsam.*
 measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
@@ -45,7 +45,6 @@ for i=1:length(data.Z)
 end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
-import gtsam.*
 cameraPriorNoise  = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
 firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
 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
-import gtsam.*
 initialEstimate = Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
@@ -72,8 +70,6 @@ end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Fine grain optimization, allowing user to iterate step by step
-
-import gtsam.*
 parameters = LevenbergMarquardtParams;
 parameters.setlambdaInitial(1.0);
 parameters.setVerbosityLM('trylambda');
@@ -86,5 +82,3 @@ end
 
 result = optimizer.values();
 result.print(sprintf('\nFinal result:\n  '));
-
-

matlab/gtsam_examples/SFMExample.m

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

matlab/gtsam_examples/StereoVOExample.m

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

matlab/gtsam_examples/StereoVOExample_large.m

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

matlab/gtsam_examples/VisualISAMExample.m

@@ -10,6 +10,8 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 % Data Options
 options.triangle = false;
 options.nrCameras = 20;
@@ -32,17 +34,17 @@ options.saveFigures = false;
 options.saveDotFiles = false;
 
 %% Generate data
-[data,truth] = gtsam.VisualISAMGenerateData(options);
+[data,truth] = VisualISAMGenerateData(options);
 
 %% 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;
-gtsam.VisualISAMPlot(truth, data, isam, result, options)
+VisualISAMPlot(truth, data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 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
-        gtsam.VisualISAMPlot(truth, data, isam, result, options)
+        VisualISAMPlot(truth, data, isam, result, options)
     end
 end

matlab/gtsam_tests/testLocalizationExample.m

@@ -10,12 +10,12 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
 import gtsam.*
+
+%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
 graph = NonlinearFactorGraph;
 
 %% 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)
 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));
@@ -23,7 +23,6 @@ graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 
 %% Add three "GPS" measurements
 % We use Pose2 Priors here with high variance on theta
-import gtsam.*
 groundTruth = Values;
 groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
 groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
@@ -34,19 +33,16 @@ for i=1:3
 end
 
 %% Initialize to noisy points
-import gtsam.*
 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));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimizeSafely();
 
 %% Plot Covariance Ellipses
-import gtsam.*
 marginals = Marginals(graph, result);
 P={};
 for i=1:result.size()

matlab/gtsam_tests/testOdometryExample.m

@@ -10,38 +10,34 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
 import gtsam.*
+
+%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
 graph = NonlinearFactorGraph;
 
 %% 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.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
 
 %% 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)
 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(2, 3, odometry, odometryNoise));
 
 %% Initialize to noisy points
-import gtsam.*
 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));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimizeSafely();
 marginals = Marginals(graph, result);
 marginals.marginalCovariance(1);
 
 %% Check first pose equality
-import gtsam.*
 pose_1 = result.at(1);
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -11,6 +11,8 @@
 % @author Frank Dellaert
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - All values are axis aligned
 %  - 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
 
 %% Create keys for variables
-import gtsam.*
 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
-import gtsam.*
 graph = 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.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.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.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));
 
 %% Initialize to noisy points
-import gtsam.*
 initialEstimate = Values;
 initialEstimate.insert(i1, Pose2(0.5, 0.0, 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));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 result = optimizer.optimizeSafely();
 marginals = Marginals(graph, result);
 
 %% Check first pose and point equality
-import gtsam.*
 pose_1 = result.at(symbol('x',1));
 marginals.marginalCovariance(symbol('x',1));
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -12,6 +12,8 @@
 % @author Chris Beall
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+import gtsam.*
+
 %% Assumptions
 %  - All values are axis aligned
 %  - 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
 
 %% Create graph container and add factors to it
-import gtsam.*
 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]);
@@ -31,7 +31,6 @@ graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-import gtsam.*
 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(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));
 
 %% Add pose constraint
-import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 %% Initialize to noisy points
-import gtsam.*
 initialEstimate = Values;
 initialEstimate.insert(1, Pose2(0.5, 0.0, 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));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
 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
-import gtsam.*
 marginals = Marginals(graph, result);
 P = marginals.marginalCovariance(1);
 
 pose_1 = result.at(1);
 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));
-
 

matlab/gtsam_tests/testPose3SLAMExample.m

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

matlab/gtsam_tests/testSFMExample.m

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

matlab/gtsam_tests/testStereoVOExample.m

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

matlab/gtsam_tests/testVisualISAMExample.m

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