From 6b23258b9739f916790093c9cd85944e6594ff96 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 10 Jun 2012 15:26:59 +0000 Subject: [PATCH] options structure all the way --- matlab/examples/VisualISAMDemo.m | 3 - matlab/examples/VisualISAMExample.m | 55 ++++++++------- matlab/examples/VisualISAMExample_triangle.m | 55 ++++++++------- matlab/examples/VisualISAMGenerateData.m | 5 +- matlab/examples/VisualISAMInitialize.m | 15 ++-- matlab/examples/VisualISAMPlot.m | 72 +++++++++++--------- matlab/examples/VisualISAMStep.m | 8 +-- matlab/examples/VisualISAM_gui.m | 56 +++++++-------- 8 files changed, 130 insertions(+), 139 deletions(-) diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/examples/VisualISAMDemo.m index a6850fb7d..f4b975ad5 100644 --- a/matlab/examples/VisualISAMDemo.m +++ b/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 diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m index f84ee89c7..f7114b652 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/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 -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 +% Data Options options.triangle = false; options.nrCameras = 20; -showImages = false; -data = VisualISAMGenerateData(options,showImages); +options.showImages = false; + +% 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 -figure; -VisualISAMPlot +VisualISAMInitialize(options) +figure(1); +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 - VisualISAMPlot + if mod(frame_i,options.drawInterval)==0 + VisualISAMPlot(data, isam, result, options) end end \ No newline at end of file diff --git a/matlab/examples/VisualISAMExample_triangle.m b/matlab/examples/VisualISAMExample_triangle.m index 536dbf9c1..2fdb35599 100644 --- a/matlab/examples/VisualISAMExample_triangle.m +++ b/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 -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 +% Data Options options.triangle = true; options.nrCameras = 10; -showImages = false; -data = VisualISAMGenerateData(options,showImages); +options.showImages = false; + +% 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 -figure; -VisualISAMPlot +VisualISAMInitialize(options) +figure(1); +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 - VisualISAMPlot + if mod(frame_i,options.drawInterval)==0 + VisualISAMPlot(data, isam, result, options) end end \ No newline at end of file diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m index 0eb504d57..adb0b9243 100644 --- a/matlab/examples/VisualISAMGenerateData.m +++ b/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 diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index 73d512d3d..d7489eb67 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/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 diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m index fc4436332..42407ec82 100644 --- a/matlab/examples/VisualISAMPlot.m +++ b/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 -global data frame_i isam result +M = double(result.nrPoses); +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(symbol('l',j)); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); - end -end -for ii=1:CAMERA_INTERVAL:frame_i - pose_ii = result.pose(symbol('x',ii)); - if (frame_i>1) - P = isam.marginalCovariance(symbol('x',ii)); - else - P = []; - end - plotPose3(pose_ii,P,10); - if DRAW_TRUE_POSES % show ground truth - plotPose3(data.cameras{ii}.pose,0.001*eye(6),10); + P = isam.marginalCovariance(jj); + covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); +end + +%% Plot cameras +for i=1:options.cameraInterval:M + ii = symbol('x',i); + pose_i = result.pose(ii); + if options.hardConstraint & (i==1) + plotPose3(pose_i,[],10); + else + P = isam.marginalCovariance(ii); + plotPose3(pose_i,P,10); + end + 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)); -end -if SAVE_GRAPHS && (frame_i>1) - isam.saveGraph(sprintf('VisualiSAM%03d.dot',frame_i)); + print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M)); end -if SAVE_GRAPH - isam.saveGraph(sprintf('VisualiSAM.dot',frame_i)); +if options.saveDotFiles + 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 \ No newline at end of file diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index 9bf8f93ac..30b0dfcce 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/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 data +global options data odometryNoise measurementNoise frame_i % 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 diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m index a1f9a8a54..3adeb4b1c 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/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 SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES -global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING +global options + +% 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'); -POINT_PRIORS = get(handles.pointPriorsCB,'Value'); -BATCH_INIT = get(handles.batchInitCB,'Value'); -REORDER_INTERVAL = str2num(get(handles.reorderIntervalEdit,'String')); -ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value'); +options.hardConstraint = get(handles.hardConstraintCB,'Value'); +options.pointPriors = get(handles.pointPriorsCB,'Value'); +options.batchInitialization = get(handles.batchInitCB,'Value'); +options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String')); +options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value'); % Display Options -SAVE_GRAPH = get(handles.saveGraphCB,'Value'); -PRINT_STATS = get(handles.printStatsCB,'Value'); -DRAW_INTERVAL = str2num(get(handles.drawInterval,'String')); -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; +options.saveDotFile = get(handles.saveGraphCB,'Value'); +options.printStats = get(handles.printStatsCB,'Value'); +options.drawInterval = str2num(get(handles.drawInterval,'String')); +options.cameraInterval = str2num(get(handles.cameraIntervalEdit,'String')); +options.drawTruePoses = get(handles.drawTruePosesCB,'Value'); +options.saveFigures = get(handles.saveFiguresCB,'Value'); +options.saveDotFiles = get(handles.saveGraphsCB,'Value'); %---------------------------------------------------------- % 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); -options.nrCameras = str2num(get(handles.numCamEdit,'String')); -showImages = get(handles.showImagesCB,'Value'); -data = VisualISAMGenerateData(options, showImages); +data = VisualISAMGenerateData(options); % Initialize and plot -VisualISAMInitialize -VisualISAMPlot +VisualISAMInitialize(options) +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