I got rid of GlobalVars, added comments, and made "step" plot every time

matlab/examples/VisualISAMDemo.m

@@ -1,2 +1,15 @@
-VisualISAMGlobalVars
+% VisualISAMDemo: runs VisualSLAM iSAM demo in a GUI
+% Authors: Duy Nguyen Ta
+
+% 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 poseNoise pointNoise odometryNoise measurementNoise
+global frame_i isam newFactors initialEstimates result
+
+% Start GUI
 VisualISAM_gui

matlab/examples/VisualISAMExample.m

@@ -10,12 +10,14 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-clear
 %% Global variables used in VisualISAMExample
-VisualISAMGlobalVars
+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 
 
 %% Setting data options
-TRIANGLE = true;
+TRIANGLE = false;
 NCAMERAS = 20;
 SHOW_IMAGES = false;
 
@@ -28,8 +30,8 @@ ALWAYS_RELINEARIZE = false;
 
 %% Display Options
 SAVE_GRAPH = false;
-PRINT_STATS = true;
+PRINT_STATS = false;
-DRAW_INTERVAL = 4;
+DRAW_INTERVAL = 5;
 CAMERA_INTERVAL = 1;
 DRAW_TRUE_POSES = false;
 SAVE_FIGURES = false;

matlab/examples/VisualISAMExample_triangle.m

@@ -10,100 +10,43 @@
 % @author Duy-Nguyen Ta
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-%% Create a triangle target, just 3 points on a plane
+%% Global variables used in VisualISAMExample
-nPoints = 3;
+global TRIANGLE NCAMERAS SHOW_IMAGES 
-r = 10;
+global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE 
-points = {};
+global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES 
-for j=1:nPoints
+global SAVE_FIGURES SAVE_GRAPHS 
-    theta = (j-1)*2*pi/nPoints;
-    points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
-end
 
-%% Create camera cameras on a circle around the triangle
+%% Setting data options
-nCameras = 10;
+TRIANGLE = true;
-height = 10;
+NCAMERAS = 10;
-r = 30;
+SHOW_IMAGES = false;
-cameras = {};
-K = gtsamCal3_S2(500,500,0,640/2,480/2);
-for i=1:nCameras
-    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);
-end
-odometry = cameras{1}.pose.between(cameras{2}.pose);
 
-posepriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]');
+%% iSAM Options
-odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]');
+HARD_CONSTRAINT = false;
-pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
+POINT_PRIORS = false;
-measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
+BATCH_INIT = true;
+REORDER_INTERVAL = 10;
+ALWAYS_RELINEARIZE = false;
 
-%% Create an ISAM object for inference
+%% Display Options
-isam = visualSLAMISAM(2);
+SAVE_GRAPH = false;
+PRINT_STATS = false;
+DRAW_INTERVAL = 5;
+CAMERA_INTERVAL = 1;
+DRAW_TRUE_POSES = false;
+SAVE_FIGURES = false;
+SAVE_GRAPHS = false;
 
-%% Update ISAM
+%% Generate data and initialize iSAM with the first pose and points
-newFactors = visualSLAMGraph;
+VisualISAMGenerateData
-initialEstimates = visualSLAMValues;
+VisualISAMInitialize
-figure(1); clf;
+figure;
-for i=1:nCameras
+VisualISAMPlot
 
-    % Prior for the first pose or odometry for subsequent cameras
+%% Main loop for iSAM: stepping through all poses
-    if (i==1)
+for frame_i=2:NCAMERAS
-        newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, posepriorNoise);
+    VisualISAMStep
-        newFactors.addPointPrior(symbol('l',1), points{1}, pointNoise);
+    if mod(frame_i,DRAW_INTERVAL)==0
-    else
+        VisualISAMPlot
-        newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, odometryNoise);
-    end
-
-    % Visual measurement factors
-    for j=1:nPoints
-        zij = cameras{i}.project(points{j});
-        newFactors.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
-    end
-    
-    % Initial estimates for the new pose. Also initialize points while in 
-    % the first frame.
-    if (i==1)
-        initialEstimates.insertPose(symbol('x',i), cameras{i}.pose);
-        for j=1:nPoints
-            initialEstimates.insertPoint(symbol('l',j), points{j});
-        end
-    else
-        %TODO: this might be suboptimal since "result" is not the fully
-        %optimized result
-        if (i==2), prevPose = cameras{1}.pose;
-        else prevPose = result.pose(symbol('x',i-1)); end
-        initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
-    end
-
-    % Update ISAM, only update for the second frame onward
-    % Update the first frame will cause error, since it's under constrained
-    if (i>=2)
-        isam.update(newFactors, initialEstimates);
-        result = isam.estimate();
-
-        % Plot results
-        h=figure(1); clf;
-        hold on; 
-        for j=1:nPoints
-            P = isam.marginalCovariance(symbol('l',j));
-            point_j = result.point(symbol('l',j));
-            plot3(point_j.x, point_j.y, point_j.z,'marker','o');
-            covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
-        end
-
-        for ii=1:i
-            P = isam.marginalCovariance(symbol('x',ii));
-            pose_ii = result.pose(symbol('x',ii));
-            plotPose3(pose_ii,P,10);
-        end
-        axis([-35 35 -35 35 -35 35])
-        view([36 34])
-        colormap('hot')
-%         print(h,'-dpng',sprintf('vISAM_%03d.png',i));
-        
-        % Reset newFactors and initialEstimates to prepare for the next 
-        % update
-        newFactors = visualSLAMGraph;
-        initialEstimates = visualSLAMValues;
     end
 end

matlab/examples/VisualISAMGenerateData.m

@@ -1,4 +1,12 @@
-VisualISAMGlobalVars
+% VisualISAMGenerateData: create data for viusalSLAM::iSAM examples
+% Authors: Duy Nguyen Ta and Frank Dellaert
+
+% options
+global TRIANGLE NCAMERAS SHOW_IMAGES
+
+% global outputs
+global points cameras K odometry
+global poseNoise odometryNoise pointNoise measurementNoise % data ?
 
 %% Generate simulated data
 points = {};

matlab/examples/VisualISAMGlobalVars.m

@@ -1,7 +0,0 @@
-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 nPoints points K cameras odometry 
-global poseNoise pointNoise odometryNoise measurementNoise
-global frame_i isam newFactors initialEstimates result

matlab/examples/VisualISAMInitialize.m

@@ -1,4 +1,15 @@
-VisualISAMGlobalVars
+function VisualISAMStep
+% VisualISAMStep: execute one update step of visualSLAM::iSAM object
+% Authors: Duy Nguyen Ta and Frank Dellaert
+
+% global variables, input
+global cameras points pointNoise poseNoise measurementNoise K
+
+% global variables, output
+global isam newFactors initialEstimates frame_i result
+
+% options
+global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS
 
 %% Initialize iSAM
 isam = visualSLAMISAM(REORDER_INTERVAL);
@@ -14,7 +25,7 @@ else
 end
 initialEstimates.insertPose(i1,pose1);
 % Add visual measurement factors from first pose
-for j=1:nPoints
+for j=1:size(points,2)
     jj = symbol('l',j);
     if POINT_PRIORS % add point priors
         newFactors.addPointPrior(jj, points{j}, pointNoise);
@@ -24,6 +35,6 @@ for j=1:nPoints
     initialEstimates.insertPoint(jj, points{j});
 end
 
-frame_i = 1
+frame_i = 1;
 result = initialEstimates;
 cla;

matlab/examples/VisualISAMPlot.m

@@ -1,13 +1,17 @@
-VisualISAMGlobalVars
+% VisualISAMPlot: plot current state of visualSLAM::iSAM object
+% Authors: Duy Nguyen Ta and Frank Dellaert
+
+% global variables, input
+global points cameras frame_i isam result
+
+% options
+global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
 
 %% Plot results
 tic
-%     h=figure(2);clf
-%     set(1,'NumberTitle','off','Name','Visual iSAM');
 h=gca;
 cla(h);
 hold on;
-sprintf('Computing marginals and plotting. Please wait...')
 for j=1:size(points,2)
     point_j = result.point(symbol('l',j));
     plot3(point_j.x, point_j.y, point_j.z,'marker','o');
@@ -21,7 +25,7 @@ for ii=1:CAMERA_INTERVAL:frame_i
     if (frame_i>1)
         P = isam.marginalCovariance(symbol('x',ii));
     else 
-        P = []
+        P = [];
     end
     plotPose3(pose_ii,P,10);
     if DRAW_TRUE_POSES % show ground truth
@@ -31,9 +35,7 @@ end
 axis([-40 40 -40 40 -10 20]);axis equal
 view(3)
 colormap('hot')
-sprintf('Done!')
 
-%     figure(2);
 t=toc;
 % if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end
 if SAVE_FIGURES

matlab/examples/VisualISAMStep.m

@@ -1,24 +1,46 @@
-VisualISAMGlobalVars
+function VisualISAMStep
+% VisualISAMStep: execute one update step of visualSLAM::iSAM object
+% Authors: Duy Nguyen Ta and Frank Dellaert
+
+% global variables, input
+global frame_i odometry odometryNoise newFactors initialEstimates
+global points cameras measurementNoise K
+
+% global variables, input/output
+global isam
+
+% global variables, output
+global result
+
+% options
+global BATCH_INIT SHOW_TIMING ALWAYS_RELINEARIZE 
+global SAVE_GRAPH PRINT_STATS
+
+% iSAM expects us to give it a new set of factors 
+% along with initial estimates for any new variables introduced.
+% We do not clear in frame 2 so we add to the factors added in Initialize
+if frame_i > 2
+    newFactors = visualSLAMGraph;
+    initialEstimates = visualSLAMValues;
+end
 
 %% Add odometry
 newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise);
 
 %% Add visual measurement factors
-for j=1:nPoints
+for j=1:size(points,2)
     zij = cameras{frame_i}.project(points{j});
     newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), K);
 end
 
 %% Initial estimates for the new pose. Also initialize points while in the first frame.
-%TODO: this might be suboptimal since "result" is not the fully optimized result
 if (frame_i==2), prevPose = cameras{1}.pose;
 else, prevPose = result.pose(symbol('x',frame_i-1)); end
 initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
 
 %% Update ISAM
 if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses
-    initialEstimates
+    fullyOptimized = newFactors.optimize(initialEstimates);
-    fullyOptimized = newFactors.optimize(initialEstimates)
     initialEstimates = fullyOptimized;
 end
 % figure(1);tic;
@@ -30,14 +52,10 @@ if ALWAYS_RELINEARIZE % re-linearize
     isam.reorder_relinearize();
 end
 
-
 if SAVE_GRAPH
     isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
 end
+
 if PRINT_STATS
     isam.printStats();
 end
-
-%% Reset newFactors and initialEstimates to prepare for the next update
-newFactors = visualSLAMGraph;
-initialEstimates = visualSLAMValues;

matlab/examples/VisualISAM_gui.m

@@ -1,26 +1,9 @@
 function varargout = VisualISAM_gui(varargin)
-% VISUALISAM_GUI MATLAB code for VisualISAM_gui.fig
+% VisualISAM_gui: runs VisualSLAM iSAM demo in GUI
-%      VISUALISAM_GUI, by itself, creates a new VISUALISAM_GUI or raises the existing
+%   Interface is defined by VisualISAM_gui.fig
-%      singleton*.
+%   You can run this file directly, but won't have access to globals
-%
+%   By running ViusalISAMDemo, you see all variables in command prompt
-%      H = VISUALISAM_GUI returns the handle to a new VISUALISAM_GUI or the handle to
+% Authors: Duy Nguyen Ta
-%      the existing singleton*.
-%
-%      VISUALISAM_GUI('CALLBACK',hObject,~,handles,...) calls the local
-%      function named CALLBACK in VISUALISAM_GUI.M with the given input arguments.
-%
-%      VISUALISAM_GUI('Property','Value',...) creates a new VISUALISAM_GUI or raises the
-%      existing singleton*.  Starting from the left, property value pairs are
-%      applied to the GUI before VisualISAM_gui_OpeningFcn gets called.  An
-%      unrecognized property name or invalid value makes property application
-%      stop.  All inputs are passed to VisualISAM_gui_OpeningFcn via varargin.
-%
-%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
-%      instance to run (singleton)".
-%
-% See also: GUIDE, GUIDATA, GUIHANDLES
-
-% Edit the above text to modify the response to help VisualISAM_gui
 
 % Last Modified by GUIDE v2.5 09-Jun-2012 00:56:47
 
@@ -55,9 +38,6 @@ handles.output = hObject;
 % Update handles structure
 guidata(hObject, handles);
 
-% UIWAIT makes VisualISAM_gui wait for user response (see UIRESUME)
-% uiwait(handles.figure1);
-
 % --- Outputs from this function are returned to the command line.
 function varargout = VisualISAM_gui_OutputFcn(hObject, ~, handles)
 % varargout  cell array for returning output args (see VARARGOUT);
@@ -69,7 +49,7 @@ varargout{1} = handles.output;
 % Convenient functions
 %----------------------------------------------------------
 function showFramei(hObject, handles)
-    VisualISAMGlobalVars
+global frame_i
 set(handles.frameStatus, 'String', sprintf('Frame: %d',frame_i));
 drawnow
 guidata(hObject, handles);
@@ -90,27 +70,32 @@ function triangle = chooseDataset(handles)
 end
 
 function initOptions(handles)
-    VisualISAMGlobalVars
+
+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
+
 % Setting data options
-    TRIANGLE = chooseDataset(handles)
+TRIANGLE = chooseDataset(handles);
-    NCAMERAS = str2num(get(handles.numCamEdit,'String')) 
+NCAMERAS = str2num(get(handles.numCamEdit,'String'));
-    SHOW_IMAGES = get(handles.showImagesCB,'Value')
+SHOW_IMAGES = get(handles.showImagesCB,'Value');
 
 % iSAM Options
-    HARD_CONSTRAINT = get(handles.hardConstraintCB,'Value')
+HARD_CONSTRAINT = get(handles.hardConstraintCB,'Value');
-    POINT_PRIORS = get(handles.pointPriorsCB,'Value')
+POINT_PRIORS = get(handles.pointPriorsCB,'Value');
-    BATCH_INIT = get(handles.batchInitCB,'Value')
+BATCH_INIT = get(handles.batchInitCB,'Value');
-    REORDER_INTERVAL = str2num(get(handles.numCamEdit,'String')) 
+REORDER_INTERVAL = str2num(get(handles.numCamEdit,'String'));
-    ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value')
+ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value');
 
 % Display Options
-    SAVE_GRAPH = get(handles.saveGraphCB,'Value')
+SAVE_GRAPH = get(handles.saveGraphCB,'Value');
-    PRINT_STATS = get(handles.printStatsCB,'Value')
+PRINT_STATS = get(handles.printStatsCB,'Value');
-    DRAW_INTERVAL = str2num(get(handles.drawInterval,'String'))
+DRAW_INTERVAL = str2num(get(handles.drawInterval,'String'));
-    CAMERA_INTERVAL = str2num(get(handles.cameraIntervalEdit,'String')) 
+CAMERA_INTERVAL = str2num(get(handles.cameraIntervalEdit,'String'));
-    DRAW_TRUE_POSES = get(handles.drawTruePosesCB,'Value')
+DRAW_TRUE_POSES = get(handles.drawTruePosesCB,'Value');
-    SAVE_FIGURES = get(handles.saveFiguresCB,'Value')
+SAVE_FIGURES = get(handles.saveFiguresCB,'Value');
-    SAVE_GRAPHS = get(handles.saveGraphsCB,'Value')
+SAVE_GRAPHS = get(handles.saveGraphsCB,'Value');
 
 
 %----------------------------------------------------------
@@ -242,7 +227,6 @@ function saveGraphsCB_Callback(hObject, ~, handles)
 
 % --- Executes on button press in intializeButton.
 function intializeButton_Callback(hObject, ~, handles)
-    VisualISAMGlobalVars
 initOptions(handles)
 VisualISAMGenerateData
 VisualISAMInitialize
@@ -252,7 +236,7 @@ function intializeButton_Callback(hObject, ~, handles)
 
 % --- Executes on button press in runButton.
 function runButton_Callback(hObject, ~, handles)
-    VisualISAMGlobalVars
+global frame_i NCAMERAS DRAW_INTERVAL
 while (frame_i<NCAMERAS)
     frame_i = frame_i+1;
     showFramei(hObject, handles)
@@ -267,17 +251,15 @@ function runButton_Callback(hObject, ~, handles)
 
 % --- Executes on button press in stepButton.
 function stepButton_Callback(hObject, ~, handles)
-    VisualISAMGlobalVars
+global frame_i NCAMERAS DRAW_INTERVAL
 if (frame_i<NCAMERAS)
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     VisualISAMStep
-        if mod(frame_i,DRAW_INTERVAL)==0
     showWaiting(handles, 'Computing marginals...');
     VisualISAMPlot
     showWaiting(handles, '');
 end
-    end
 
 % --- Executes on button press in plotButton.
 function plotButton_Callback(hObject, ~, handles)