No more globals (except in gui and demo)

matlab/examples/VisualISAMDemo.m

@@ -3,9 +3,7 @@
 
 % Make sure global variables are visible on command prompt
 % so you can examine how they change as you step through
-global data
+global frame_i data noiseModels isam result options
-global poseNoise pointNoise odometryNoise measurementNoise
-global frame_i isam result
 
 % Start GUI
 VisualISAM_gui

matlab/examples/VisualISAMExample.m

@@ -10,10 +10,6 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Global variables used in VisualISAMExample
-global options data isam result frame_i
-global poseNoise odometryNoise pointNoise measurementNoise
-
 % Data Options
 options.triangle = false;
 options.nrCameras = 20;
@@ -39,13 +35,13 @@ options.saveDotFiles = false;
 data = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-VisualISAMInitialize(options)
+[noiseModels,isam,result] = VisualISAMInitialize(data,options);
 figure(1);
 VisualISAMPlot(data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
-    VisualISAMStep
+    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
         VisualISAMPlot(data, isam, result, options)
     end

matlab/examples/VisualISAMExample_triangle.m

@@ -10,10 +10,6 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Global variables used in VisualISAMExample
-global options data isam result frame_i
-global poseNoise odometryNoise pointNoise measurementNoise
-
 % Data Options
 options.triangle = true;
 options.nrCameras = 10;
@@ -39,13 +35,13 @@ options.saveDotFiles = false;
 data = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-VisualISAMInitialize(options)
+[noiseModels,isam,result] = VisualISAMInitialize(data,options);
 figure(1);
 VisualISAMPlot(data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
-    VisualISAMStep
+    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
         VisualISAMPlot(data, isam, result, options)
     end

matlab/examples/VisualISAMInitialize.m

@@ -1,22 +1,15 @@
-function VisualInitialize(options)
+function [noiseModels,isam,result] = VisualInitialize(data,options)
 % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
-% global variables, input
-global data
-
-% global variables, output
-global isam frame_i result
-global poseNoise odometryNoise pointNoise measurementNoise
-
 %% Initialize iSAM
 isam = visualSLAMISAM(options.reorderInterval);
 
 %% Set Noise parameters
-poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.pose = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.odometry = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
+noiseModels.point = gtsamSharedNoiseModel_Sigma(3, 0.1);
-measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
+noiseModels.measurement = gtsamSharedNoiseModel_Sigma(2, 1.0);
 
 %% Add constraints/priors
 newFactors = visualSLAMGraph;
@@ -26,7 +19,7 @@ for frame_i=1:2
     if frame_i==1 & options.hardConstraint % add hard constraint
         newFactors.addPoseConstraint(ii,data.cameras{1}.pose);
     else
-        newFactors.addPosePrior(ii,data.cameras{frame_i}.pose, poseNoise);
+        newFactors.addPosePrior(ii,data.cameras{frame_i}.pose, noiseModels.pose);
     end
     % TODO: init should not be from ground truth!
     initialEstimates.insertPose(ii,data.cameras{frame_i}.pose);
@@ -38,7 +31,7 @@ for frame_i=1:2
     for j=1:size(data.points,2)
         jj = symbol('l',j);
         zij = data.cameras{frame_i}.project(data.points{j});
-        newFactors.addMeasurement(zij, measurementNoise, ii, jj, data.K);
+        newFactors.addMeasurement(zij, noiseModels.measurement, ii, jj, data.K);
     end
 end
 
@@ -46,7 +39,7 @@ end
 for j=1:size(data.points,2)
     jj = symbol('l',j);
     if options.pointPriors % add point priors
-        newFactors.addPointPrior(jj, data.points{j}, pointNoise);
+        newFactors.addPointPrior(jj, data.points{j}, noiseModels.point);
     end
     initialEstimates.insertPoint(jj, data.points{j});  % TODO: should not be from ground truth!
 end

matlab/examples/VisualISAMStep.m

@@ -1,33 +1,25 @@
-function VisualISAMStep
+function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
 % VisualISAMStep: execute one update step of visualSLAM::iSAM object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
-% global variables, input
-global options data odometryNoise measurementNoise frame_i
-
-% global variables, input/output
-global isam
-
-% global variables, output
-global result
-
 % iSAM expects us to give it a new set of factors 
 % along with initial estimates for any new variables introduced.
 newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 
 %% Add odometry
-newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), data.odometry, odometryNoise);
+i = double(result.nrPoses)+1;
+newFactors.addOdometry(symbol('x',i-1), symbol('x',i), data.odometry, noiseModels.odometry);
 
 %% Add visual measurement factors
 for j=1:size(data.points,2)
-    zij = data.cameras{frame_i}.project(data.points{j});
+    zij = data.cameras{i}.project(data.points{j});
-    newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), data.K);
+    newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K);
 end
 
 %% Initial estimates for the new pose.
-prevPose = result.pose(symbol('x',frame_i-1));
+prevPose = result.pose(symbol('x',i-1));
-initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(data.odometry));
+initialEstimates.insertPose(symbol('x',i), prevPose.compose(data.odometry));
 
 %% Update ISAM
 % figure(1);tic;

matlab/examples/VisualISAM_gui.m

@@ -224,28 +224,28 @@ function saveGraphsCB_Callback(hObject, ~, handles)
 % --- Executes on button press in intializeButton.
 function intializeButton_Callback(hObject, ~, handles)
 
-global options data isam result
+global frame_i data noiseModels isam result options
 
 % initialize global options
-global options
 initOptions(handles)
 
 % Generate Data
 data = VisualISAMGenerateData(options);
 
 % Initialize and plot
-VisualISAMInitialize(options)
+[noiseModels,isam,result] = VisualISAMInitialize(data,options);
 VisualISAMPlot(data, isam, result, options)
+frame_i = 2;
 showFramei(hObject, handles)
 
 
 % --- Executes on button press in runButton.
 function runButton_Callback(hObject, ~, handles)
-global options data frame_i isam result
+global frame_i data noiseModels isam result options
 while (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
-    VisualISAMStep
+    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
         showWaiting(handles, 'Computing marginals...');
         VisualISAMPlot(data, isam, result, options)
@@ -256,11 +256,11 @@ end
 
 % --- Executes on button press in stepButton.
 function stepButton_Callback(hObject, ~, handles)
-global options data frame_i isam result
+global frame_i data noiseModels isam result options
 if (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
-    VisualISAMStep
+    [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     showWaiting(handles, 'Computing marginals...');
     VisualISAMPlot(data, isam, result, options)
     showWaiting(handles, '');
@@ -269,7 +269,8 @@ end
 % --- Executes on button press in plotButton.
 function plotButton_Callback(hObject, ~, handles)
 showWaiting(handles, 'Computing marginals...');
-VisualISAMPlot;
+global options data isam result
+VisualISAMPlot(data, isam, result, options);
 showWaiting(handles, '');