GenerateData is now a function, no globals

matlab/examples/VisualISAMDemo.m

@@ -3,11 +3,10 @@
 
 % Make sure global variables are visible on command prompt
 % so you can examine how they change as you step through
-global TRIANGLE NCAMERAS SHOW_IMAGES 
 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 points K cameras odometry 
+global data
 global poseNoise pointNoise odometryNoise measurementNoise
 global frame_i isam newFactors initialEstimates result
 

matlab/examples/VisualISAMExample.m

@@ -11,16 +11,11 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global TRIANGLE NCAMERAS SHOW_IMAGES 
+global data
 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 
 
-%% Setting data options
-TRIANGLE = false;
-NCAMERAS = 20;
-SHOW_IMAGES = false;
-
 %% iSAM Options
 HARD_CONSTRAINT = false;
 POINT_PRIORS = false;
@@ -37,14 +32,19 @@ DRAW_TRUE_POSES = false;
 SAVE_FIGURES = false;
 SAVE_GRAPHS = false;
 
-%% Generate data and initialize iSAM with the first pose and points
+%% Generate data
-VisualISAMGenerateData
+options.triangle = false;
+options.nrCameras = 20;
+showImages = false;
+data = VisualISAMGenerateData(options,showImages);
+
+%% Initialize iSAM with the first pose and points
 VisualISAMInitialize
 figure;
 VisualISAMPlot
 
 %% Main loop for iSAM: stepping through all poses
-for frame_i=2:NCAMERAS
+for frame_i=2:options.nrCameras
     VisualISAMStep
     if mod(frame_i,DRAW_INTERVAL)==0
         VisualISAMPlot

matlab/examples/VisualISAMExample_triangle.m

@@ -11,16 +11,11 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Global variables used in VisualISAMExample
-global TRIANGLE NCAMERAS SHOW_IMAGES 
+global data
 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 
 
-%% Setting data options
-TRIANGLE = true;
-NCAMERAS = 10;
-SHOW_IMAGES = false;
-
 %% iSAM Options
 HARD_CONSTRAINT = false;
 POINT_PRIORS = false;
@@ -37,14 +32,19 @@ DRAW_TRUE_POSES = false;
 SAVE_FIGURES = false;
 SAVE_GRAPHS = false;
 
-%% Generate data and initialize iSAM with the first pose and points
+%% Generate data
-VisualISAMGenerateData
+options.triangle = true;
+options.nrCameras = 10;
+showImages = false;
+data = VisualISAMGenerateData(options,showImages);
+
+%% Initialize iSAM with the first pose and points
 VisualISAMInitialize
 figure;
 VisualISAMPlot
 
 %% Main loop for iSAM: stepping through all poses
-for frame_i=2:NCAMERAS
+for frame_i=2:options.nrCameras
     VisualISAMStep
     if mod(frame_i,DRAW_INTERVAL)==0
         VisualISAMPlot

matlab/examples/VisualISAMGenerateData.m

@@ -1,25 +1,20 @@
+function data = VisualISAMGenerateData(options, showImages)
 % VisualISAMGenerateData: create data for viusalSLAM::iSAM examples
 % Authors: Duy Nguyen Ta and Frank Dellaert
-
+if nargin<2, showImages=false; end
-% options
-global TRIANGLE NCAMERAS SHOW_IMAGES
-
-% global outputs
-global points cameras K odometry
-global poseNoise odometryNoise pointNoise measurementNoise % data ?
 
 %% Generate simulated data
-points = {};
+data.points = {};
-if TRIANGLE % Create a triangle target, just 3 points on a plane
+if options.triangle % Create a triangle target, just 3 points on a plane
     nPoints = 3;
     r = 10;
     for j=1:nPoints
         theta = (j-1)*2*pi/nPoints;
-        points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
+        data.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
     end
 else % 3D landmarks as vertices of a cube
     nPoints = 8;
-    points = {gtsamPoint3([10 10 10]'),...
+    data.points = {gtsamPoint3([10 10 10]'),...
         gtsamPoint3([-10 10 10]'),...
         gtsamPoint3([-10 -10 10]'),...
         gtsamPoint3([10 -10 10]'),...
@@ -31,29 +26,28 @@ end
 
 %% Create camera cameras on a circle around the triangle
 height = 10; r = 40;
-K = gtsamCal3_S2(500,500,0,640/2,480/2);
+data.K = gtsamCal3_S2(500,500,0,640/2,480/2);
-cameras = {};
+data.cameras = {};
-gui = gcf;
+for i=1:options.nrCameras
-for i=1:NCAMERAS
+    theta = (i-1)*2*pi/options.nrCameras;
-    theta = (i-1)*2*pi/NCAMERAS;
     t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
-    cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
+    data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K);
-    if SHOW_IMAGES % show images
+end
+
+%% show images if asked
+if showImages
+    gui = gcf;
+    for i=1:options.nrCameras
         figure(2+i);clf;hold on
         set(2+i,'NumberTitle','off','Name',sprintf('Camera %d',i));
         for j=1:nPoints
-            zij = cameras{i}.project(points{j});
+            zij = data.cameras{i}.project(data.points{j});
             plot(zij.x,zij.y,'*');
             axis([1 640 1 480]);
         end
     end
+    figure(gui);
 end
-figure(gui);
-odometry = cameras{1}.pose.between(cameras{2}.pose);
 
-
+%% Calculate odometry between cameras
-%% Set Noise parameters
+data.odometry = data.cameras{1}.pose.between(data.cameras{2}.pose);
-poseNoise = 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]');
-pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
-measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);

matlab/examples/VisualISAMInitialize.m

@@ -3,36 +3,46 @@ function VisualISAMStep
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 % global variables, input
-global cameras points pointNoise poseNoise measurementNoise K
+global data pointNoise poseNoise measurementNoise
 
 % global variables, output
 global isam newFactors initialEstimates frame_i result
+global poseNoise odometryNoise pointNoise measurementNoise
 
 % options
 global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS
 
 %% Initialize iSAM
 isam = visualSLAMISAM(REORDER_INTERVAL);
+
+%% Set Noise parameters
+poseNoise = 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]');
+pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
+measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
+
+%% Add constraints/priors
 newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
 i1 = symbol('x',1);
-camera1 = cameras{1};
+camera1 = data.cameras{1};
 pose1 = camera1.pose;
 if HARD_CONSTRAINT % add hard constraint
     newFactors.addPoseConstraint(i1,pose1);
 else
     newFactors.addPosePrior(i1,pose1, poseNoise);
 end
-initialEstimates.insertPose(i1,pose1);
+initialEstimates.insertPose(i1,pose1); % TODO: should not be from ground truth!
-% Add visual measurement factors from first pose
+
-for j=1:size(points,2)
+%% Add visual measurement factors from first pose
+for j=1:size(data.points,2)
     jj = symbol('l',j);
     if POINT_PRIORS % add point priors
-        newFactors.addPointPrior(jj, points{j}, pointNoise);
+        newFactors.addPointPrior(jj, data.points{j}, pointNoise);
     end
-    zij = camera1.project(points{j});
+    zij = camera1.project(data.points{j});
-    newFactors.addMeasurement(zij, measurementNoise, i1, jj, K);
+    newFactors.addMeasurement(zij, measurementNoise, i1, jj, data.K);
-    initialEstimates.insertPoint(jj, points{j});
+    initialEstimates.insertPoint(jj, data.points{j});  % TODO: should not be from ground truth!
 end
 
 frame_i = 1;

matlab/examples/VisualISAMPlot.m

@@ -2,17 +2,16 @@
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 % global variables, input
-global points cameras frame_i isam result
+global data frame_i isam result
 
 % options
 global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
 
 %% Plot results
-tic
 h=gca;
 cla(h);
 hold on;
-for j=1:size(points,2)
+for j=1:size(data.points,2)
     point_j = result.point(symbol('l',j));
     plot3(point_j.x, point_j.y, point_j.z,'marker','o');
     if (frame_i>1) 
@@ -29,15 +28,13 @@ for ii=1:CAMERA_INTERVAL:frame_i
     end
     plotPose3(pose_ii,P,10);
     if DRAW_TRUE_POSES % show ground truth
-        plotPose3(cameras{ii}.pose,0.001*eye(6),10);
+        plotPose3(data.cameras{ii}.pose,0.001*eye(6),10);
     end
 end
 axis([-40 40 -40 40 -10 20]);axis equal
 view(3)
 colormap('hot')
 
-t=toc;
-% if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end
 if SAVE_FIGURES
     fig2 = figure('visible','off');
     newax = copyobj(h,fig2);

matlab/examples/VisualISAMStep.m

@@ -3,8 +3,8 @@ function VisualISAMStep
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 % global variables, input
-global frame_i odometry odometryNoise newFactors initialEstimates
+global frame_i odometryNoise measurementNoise newFactors initialEstimates
-global points cameras measurementNoise K
+global data
 
 % global variables, input/output
 global isam
@@ -25,18 +25,18 @@ if frame_i > 2
 end
 
 %% Add odometry
-newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise);
+newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), data.odometry, odometryNoise);
 
 %% Add visual measurement factors
-for j=1:size(points,2)
+for j=1:size(data.points,2)
-    zij = cameras{frame_i}.project(points{j});
+    zij = data.cameras{frame_i}.project(data.points{j});
-    newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), K);
+    newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), data.K);
 end
 
-%% Initial estimates for the new pose. Also initialize points while in the first frame.
+%% Initial estimates for the new pose. Also initialize data.points while in the first frame.
-if (frame_i==2), prevPose = cameras{1}.pose;
+if (frame_i==2), prevPose = data.cameras{1}.pose;
 else, prevPose = result.pose(symbol('x',frame_i-1)); end
-initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
+initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(data.odometry));
 
 %% Update ISAM
 if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses

matlab/examples/VisualISAM_gui.m

@@ -71,21 +71,15 @@ end
 
 function initOptions(handles)
 
-global TRIANGLE NCAMERAS SHOW_IMAGES
 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
+global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING
-
-% Setting data options
-TRIANGLE = chooseDataset(handles);
-NCAMERAS = str2num(get(handles.numCamEdit,'String'));
-SHOW_IMAGES = get(handles.showImagesCB,'Value');
 
 % iSAM Options
 HARD_CONSTRAINT = get(handles.hardConstraintCB,'Value');
 POINT_PRIORS = get(handles.pointPriorsCB,'Value');
 BATCH_INIT = get(handles.batchInitCB,'Value');
-REORDER_INTERVAL = str2num(get(handles.numCamEdit,'String'));
+REORDER_INTERVAL = str2num(get(handles.reorderIntervalEdit,'String'));
 ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value');
 
 % Display Options
@@ -96,7 +90,7 @@ CAMERA_INTERVAL = str2num(get(handles.cameraIntervalEdit,'String'));
 DRAW_TRUE_POSES = get(handles.drawTruePosesCB,'Value');
 SAVE_FIGURES = get(handles.saveFiguresCB,'Value');
 SAVE_GRAPHS = get(handles.saveGraphsCB,'Value');
-
+SHOW_TIMING = false;
 
 %----------------------------------------------------------
 % Callback functions for GUI elements
@@ -227,8 +221,19 @@ function saveGraphsCB_Callback(hObject, ~, handles)
 
 % --- Executes on button press in intializeButton.
 function intializeButton_Callback(hObject, ~, handles)
+
+global data
+
+% initialize global options
 initOptions(handles)
-VisualISAMGenerateData
+
+% Generate Data
+options.triangle = chooseDataset(handles);
+options.nrCameras = str2num(get(handles.numCamEdit,'String'));
+showImages = get(handles.showImagesCB,'Value');
+data = VisualISAMGenerateData(options, showImages);
+
+% Initialize and plot
 VisualISAMInitialize
 VisualISAMPlot
 showFramei(hObject, handles)
@@ -236,8 +241,8 @@ showFramei(hObject, handles)
 
 % --- Executes on button press in runButton.
 function runButton_Callback(hObject, ~, handles)
-global frame_i NCAMERAS DRAW_INTERVAL
+global frame_i data DRAW_INTERVAL
-while (frame_i<NCAMERAS)
+while (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     VisualISAMStep
@@ -251,8 +256,8 @@ end
 
 % --- Executes on button press in stepButton.
 function stepButton_Callback(hObject, ~, handles)
-global frame_i NCAMERAS DRAW_INTERVAL
+global frame_i data DRAW_INTERVAL
-if (frame_i<NCAMERAS)
+if (frame_i<size(data.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     VisualISAMStep