options structure all the way

matlab/examples/VisualISAMDemo.m

@@ -3,9 +3,6 @@
 
 % Make sure global variables are visible on command prompt
 % so you can examine how they change as you step through
-global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
-global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
-global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING
 global data
 global poseNoise pointNoise odometryNoise measurementNoise
 global frame_i isam result

matlab/examples/VisualISAMExample.m

@@ -11,43 +11,42 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global data isam frame_i result
+global options data isam result frame_i
 global poseNoise odometryNoise pointNoise measurementNoise
-global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
-global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
-global SAVE_FIGURES SAVE_GRAPHS 
 
-%% iSAM Options
+% Data Options
-HARD_CONSTRAINT = false;
-POINT_PRIORS = false;
-BATCH_INIT = true;
-REORDER_INTERVAL = 10;
-ALWAYS_RELINEARIZE = false;
-
-%% Display Options
-SAVE_GRAPH = false;
-PRINT_STATS = false;
-DRAW_INTERVAL = 5;
-CAMERA_INTERVAL = 1;
-DRAW_TRUE_POSES = false;
-SAVE_FIGURES = false;
-SAVE_GRAPHS = false;
-
-%% Generate data
 options.triangle = false;
 options.nrCameras = 20;
-showImages = false;
+options.showImages = false;
-data = VisualISAMGenerateData(options,showImages);
+
+% iSAM Options
+options.hardConstraint = false;
+options.pointPriors = false;
+options.batchInitialization = true;
+options.reorderInterval = 10;
+options.alwaysRelinearize = false;
+
+% Display Options
+options.saveDotFile = false;
+options.printStats = false;
+options.drawInterval = 5;
+options.cameraInterval = 1;
+options.drawTruePoses = false;
+options.saveFigures = false;
+options.saveDotFiles = false;
+
+%% Generate data
+data = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-VisualISAMInitialize
+VisualISAMInitialize(options)
-figure;
+figure(1);
-VisualISAMPlot
+VisualISAMPlot(data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
     VisualISAMStep
-    if mod(frame_i,DRAW_INTERVAL)==0
+    if mod(frame_i,options.drawInterval)==0
-        VisualISAMPlot
+        VisualISAMPlot(data, isam, result, options)
     end
 end

matlab/examples/VisualISAMExample_triangle.m

@@ -11,43 +11,42 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global data isam frame_i result
+global options data isam result frame_i
 global poseNoise odometryNoise pointNoise measurementNoise
-global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
-global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
-global SAVE_FIGURES SAVE_GRAPHS 
 
-%% iSAM Options
+% Data Options
-HARD_CONSTRAINT = false;
-POINT_PRIORS = false;
-BATCH_INIT = true;
-REORDER_INTERVAL = 10;
-ALWAYS_RELINEARIZE = false;
-
-%% Display Options
-SAVE_GRAPH = false;
-PRINT_STATS = false;
-DRAW_INTERVAL = 5;
-CAMERA_INTERVAL = 1;
-DRAW_TRUE_POSES = false;
-SAVE_FIGURES = false;
-SAVE_GRAPHS = false;
-
-%% Generate data
 options.triangle = true;
 options.nrCameras = 10;
-showImages = false;
+options.showImages = false;
-data = VisualISAMGenerateData(options,showImages);
+
+% iSAM Options
+options.hardConstraint = false;
+options.pointPriors = false;
+options.batchInitialization = true;
+options.reorderInterval = 10;
+options.alwaysRelinearize = false;
+
+% Display Options
+options.saveDotFile = false;
+options.printStats = false;
+options.drawInterval = 5;
+options.cameraInterval = 1;
+options.drawTruePoses = false;
+options.saveFigures = false;
+options.saveDotFiles = false;
+
+%% Generate data
+data = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-VisualISAMInitialize
+VisualISAMInitialize(options)
-figure;
+figure(1);
-VisualISAMPlot
+VisualISAMPlot(data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
     VisualISAMStep
-    if mod(frame_i,DRAW_INTERVAL)==0
+    if mod(frame_i,options.drawInterval)==0
-        VisualISAMPlot
+        VisualISAMPlot(data, isam, result, options)
     end
 end

matlab/examples/VisualISAMGenerateData.m

@@ -1,7 +1,6 @@
-function data = VisualISAMGenerateData(options, showImages)
+function data = VisualISAMGenerateData(options)
 % VisualISAMGenerateData: create data for viusalSLAM::iSAM examples
 % Authors: Duy Nguyen Ta and Frank Dellaert
-if nargin<2, showImages=false; end
 
 %% Generate simulated data
 data.points = {};
@@ -35,7 +34,7 @@ for i=1:options.nrCameras
 end
 
 %% show images if asked
-if showImages
+if options.showImages
     gui = gcf;
     for i=1:options.nrCameras
         figure(2+i);clf;hold on

matlab/examples/VisualISAMInitialize.m

@@ -1,10 +1,7 @@
-function VisualInitialize
+function VisualInitialize(options)
 % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
-% options
-global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS BATCH_INIT ALWAYS_RELINEARIZE
-
 % global variables, input
 global data
 
@@ -13,7 +10,7 @@ global isam frame_i result
 global poseNoise odometryNoise pointNoise measurementNoise
 
 %% Initialize iSAM
-isam = visualSLAMISAM(REORDER_INTERVAL);
+isam = visualSLAMISAM(options.reorderInterval);
 
 %% Set Noise parameters
 poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
@@ -26,7 +23,7 @@ newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 for frame_i=1:2
     ii = symbol('x',frame_i);
-    if frame_i==1 & HARD_CONSTRAINT % add hard constraint
+    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);
@@ -48,14 +45,14 @@ end
 %% Initialize points, possibly add priors on them
 for j=1:size(data.points,2)
     jj = symbol('l',j);
-    if POINT_PRIORS % add point priors
+    if options.pointPriors % add point priors
         newFactors.addPointPrior(jj, data.points{j}, pointNoise);
     end
     initialEstimates.insertPoint(jj, data.points{j});  % TODO: should not be from ground truth!
 end
 
 %% Update ISAM
-if BATCH_INIT % Do a full optimize for first two poses
+if options.batchInitialization % Do a full optimize for first two poses
     fullyOptimized = newFactors.optimize(initialEstimates);
     isam.update(newFactors, fullyOptimized);
 else
@@ -66,7 +63,7 @@ end
 result = isam.estimate();
 % t=toc; plot(frame_i,t,'g.');
 
-if ALWAYS_RELINEARIZE % re-linearize
+if options.alwaysRelinearize % re-linearize
     isam.reorder_relinearize();
 end
 

matlab/examples/VisualISAMPlot.m

@@ -1,58 +1,62 @@
+function VisualISAMPlot(data, isam, result, options)
 % VisualISAMPlot: plot current state of visualSLAM::iSAM object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
-% global variables, input
+M = double(result.nrPoses);
-global data frame_i isam result
+N = double(result.nrPoints);
 
-% options
-global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
-global SAVE_GRAPH PRINT_STATS
-
-%% Plot results
 h=gca;
 cla(h);
 hold on;
-for j=1:size(data.points,2)
+
-    point_j = result.point(symbol('l',j));
+%% Plot points
+for j=1:N
+    jj = symbol('l',j);
+    point_j = result.point(jj);
     plot3(point_j.x, point_j.y, point_j.z,'marker','o');
-    if (frame_i>1) 
+    P = isam.marginalCovariance(jj);
-        P = isam.marginalCovariance(symbol('l',j));
+    covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
-        covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); 
+end
-    end
+
-end
+%% Plot cameras
-for ii=1:CAMERA_INTERVAL:frame_i
+for i=1:options.cameraInterval:M
-    pose_ii = result.pose(symbol('x',ii));
+    ii = symbol('x',i);
-    if (frame_i>1)
+    pose_i = result.pose(ii);
-        P = isam.marginalCovariance(symbol('x',ii));
+    if options.hardConstraint & (i==1)
-    else 
+        plotPose3(pose_i,[],10);
-        P = [];
+    else
-    end
+        P = isam.marginalCovariance(ii);
-    plotPose3(pose_ii,P,10);
+        plotPose3(pose_i,P,10);
-    if DRAW_TRUE_POSES % show ground truth
+    end
-        plotPose3(data.cameras{ii}.pose,0.001*eye(6),10);
+    if options.drawTruePoses % show ground truth
+        plotPose3(data.cameras{i}.pose,[],10);
     end
 end
+
+%% draw
 axis([-40 40 -40 40 -10 20]);axis equal
 view(3)
 colormap('hot')
+drawnow
 
-if SAVE_FIGURES
+%% do various optional things
+
+if options.saveFigures
     fig2 = figure('visible','off');
     newax = copyobj(h,fig2);
     colormap(fig2,'hot');
     set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]);
-    print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',frame_i));
+    print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M));
-end
-if SAVE_GRAPHS && (frame_i>1)
-    isam.saveGraph(sprintf('VisualiSAM%03d.dot',frame_i));
 end
 
-if SAVE_GRAPH
+if options.saveDotFiles
-    isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
+    isam.saveGraph(sprintf('VisualiSAM%03d.dot',M));
 end
 
-if PRINT_STATS
+if options.saveDotFile
+    isam.saveGraph(sprintf('VisualiSAM.dot'));
+end
+
+if options.printStats
     isam.printStats();
 end
-
-drawnow

matlab/examples/VisualISAMStep.m

@@ -3,8 +3,7 @@ function VisualISAMStep
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 % global variables, input
-global frame_i odometryNoise measurementNoise
+global options data odometryNoise measurementNoise frame_i
-global data
 
 % global variables, input/output
 global isam
@@ -12,9 +11,6 @@ global isam
 % global variables, output
 global result
 
-% options
-global SHOW_TIMING ALWAYS_RELINEARIZE 
-
 % iSAM expects us to give it a new set of factors 
 % along with initial estimates for any new variables introduced.
 newFactors = visualSLAMGraph;
@@ -40,6 +36,6 @@ isam.update(newFactors, initialEstimates);
 result = isam.estimate();
 % t=toc; plot(frame_i,t,'g.');
 
-if ALWAYS_RELINEARIZE % re-linearize
+if options.alwaysRelinearize % re-linearize
     isam.reorder_relinearize();
 end

matlab/examples/VisualISAM_gui.m

@@ -71,26 +71,28 @@ end
 
 function initOptions(handles)
 
-global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE
+global options
-global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES
+
-global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING
+% Data options
+options.triangle = chooseDataset(handles);
+options.nrCameras = str2num(get(handles.numCamEdit,'String'));
+options.showImages = get(handles.showImagesCB,'Value');
 
 % iSAM Options
-HARD_CONSTRAINT = get(handles.hardConstraintCB,'Value');
+options.hardConstraint = get(handles.hardConstraintCB,'Value');
-POINT_PRIORS = get(handles.pointPriorsCB,'Value');
+options.pointPriors = get(handles.pointPriorsCB,'Value');
-BATCH_INIT = get(handles.batchInitCB,'Value');
+options.batchInitialization = get(handles.batchInitCB,'Value');
-REORDER_INTERVAL = str2num(get(handles.reorderIntervalEdit,'String'));
+options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
-ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value');
+options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value');
 
 % Display Options
-SAVE_GRAPH = get(handles.saveGraphCB,'Value');
+options.saveDotFile = get(handles.saveGraphCB,'Value');
-PRINT_STATS = get(handles.printStatsCB,'Value');
+options.printStats = get(handles.printStatsCB,'Value');
-DRAW_INTERVAL = str2num(get(handles.drawInterval,'String'));
+options.drawInterval = str2num(get(handles.drawInterval,'String'));
-CAMERA_INTERVAL = str2num(get(handles.cameraIntervalEdit,'String'));
+options.cameraInterval = str2num(get(handles.cameraIntervalEdit,'String'));
-DRAW_TRUE_POSES = get(handles.drawTruePosesCB,'Value');
+options.drawTruePoses = get(handles.drawTruePosesCB,'Value');
-SAVE_FIGURES = get(handles.saveFiguresCB,'Value');
+options.saveFigures = get(handles.saveFiguresCB,'Value');
-SAVE_GRAPHS = get(handles.saveGraphsCB,'Value');
+options.saveDotFiles = get(handles.saveGraphsCB,'Value');
-SHOW_TIMING = false;
 
 %----------------------------------------------------------
 % Callback functions for GUI elements
@@ -222,33 +224,31 @@ function saveGraphsCB_Callback(hObject, ~, handles)
 % --- Executes on button press in intializeButton.
 function intializeButton_Callback(hObject, ~, handles)
 
-global data
+global options data isam result
 
 % initialize global options
+global options
 initOptions(handles)
 
 % Generate Data
-options.triangle = chooseDataset(handles);
+data = VisualISAMGenerateData(options);
-options.nrCameras = str2num(get(handles.numCamEdit,'String'));
-showImages = get(handles.showImagesCB,'Value');
-data = VisualISAMGenerateData(options, showImages);
 
 % Initialize and plot
-VisualISAMInitialize
+VisualISAMInitialize(options)
-VisualISAMPlot
+VisualISAMPlot(data, isam, result, options)
 showFramei(hObject, handles)
 
 
 % --- Executes on button press in runButton.
 function runButton_Callback(hObject, ~, handles)
-global frame_i data DRAW_INTERVAL
+global options data frame_i isam result
 while (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     VisualISAMStep
-    if mod(frame_i,DRAW_INTERVAL)==0
+    if mod(frame_i,options.drawInterval)==0
         showWaiting(handles, 'Computing marginals...');
-        VisualISAMPlot
+        VisualISAMPlot(data, isam, result, options)
         showWaiting(handles, '');
     end
 end
@@ -256,13 +256,13 @@ end
 
 % --- Executes on button press in stepButton.
 function stepButton_Callback(hObject, ~, handles)
-global frame_i data DRAW_INTERVAL
+global options data frame_i isam result
 if (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     VisualISAMStep
     showWaiting(handles, 'Computing marginals...');
-    VisualISAMPlot
+    VisualISAMPlot(data, isam, result, options)
     showWaiting(handles, '');
 end