From 7eb449c2051c844f589693a41e817f4b281ec383 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 10 Jun 2012 04:25:05 +0000 Subject: [PATCH] GenerateData is now a function, no globals --- matlab/examples/VisualISAMDemo.m | 3 +- matlab/examples/VisualISAMExample.m | 18 ++++---- matlab/examples/VisualISAMExample_triangle.m | 18 ++++---- matlab/examples/VisualISAMGenerateData.m | 48 +++++++++----------- matlab/examples/VisualISAMInitialize.m | 28 ++++++++---- matlab/examples/VisualISAMPlot.m | 9 ++-- matlab/examples/VisualISAMStep.m | 18 ++++---- matlab/examples/VisualISAM_gui.m | 33 ++++++++------ 8 files changed, 90 insertions(+), 85 deletions(-) diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/examples/VisualISAMDemo.m index df83ab32c..cb56a8208 100644 --- a/matlab/examples/VisualISAMDemo.m +++ b/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 diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m index 136adb3ed..c6563aa6e 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/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 -VisualISAMGenerateData +%% Generate data +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 diff --git a/matlab/examples/VisualISAMExample_triangle.m b/matlab/examples/VisualISAMExample_triangle.m index f6b030d1d..8629ae79f 100644 --- a/matlab/examples/VisualISAMExample_triangle.m +++ b/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 -VisualISAMGenerateData +%% Generate data +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 diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m index 638bdd90b..0eb504d57 100644 --- a/matlab/examples/VisualISAMGenerateData.m +++ b/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 - -% options -global TRIANGLE NCAMERAS SHOW_IMAGES - -% global outputs -global points cameras K odometry -global poseNoise odometryNoise pointNoise measurementNoise % data ? +if nargin<2, showImages=false; end %% Generate simulated data -points = {}; -if TRIANGLE % Create a triangle target, just 3 points on a plane +data.points = {}; +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); -cameras = {}; -gui = gcf; -for i=1:NCAMERAS - theta = (i-1)*2*pi/NCAMERAS; +data.K = gtsamCal3_S2(500,500,0,640/2,480/2); +data.cameras = {}; +for i=1:options.nrCameras + theta = (i-1)*2*pi/options.nrCameras; t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); - cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K); - if SHOW_IMAGES % show images + data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K); +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); - -%% 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); \ No newline at end of file +%% Calculate odometry between cameras +data.odometry = data.cameras{1}.pose.between(data.cameras{2}.pose); diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index 8fd23aeaa..eeba8cbf6 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/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); -% Add visual measurement factors from first pose -for j=1:size(points,2) +initialEstimates.insertPose(i1,pose1); % TODO: should not be from ground truth! + +%% 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}); - newFactors.addMeasurement(zij, measurementNoise, i1, jj, K); - initialEstimates.insertPoint(jj, points{j}); + zij = camera1.project(data.points{j}); + newFactors.addMeasurement(zij, measurementNoise, i1, jj, data.K); + initialEstimates.insertPoint(jj, data.points{j}); % TODO: should not be from ground truth! end frame_i = 1; diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m index 4e7ef07da..52eb030b5 100644 --- a/matlab/examples/VisualISAMPlot.m +++ b/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); diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index ab417d3d2..833618a6f 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/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 points cameras measurementNoise K +global frame_i odometryNoise measurementNoise newFactors initialEstimates +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) - zij = cameras{frame_i}.project(points{j}); - newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), K); +for j=1:size(data.points,2) + zij = data.cameras{frame_i}.project(data.points{j}); + 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. -if (frame_i==2), prevPose = cameras{1}.pose; +%% Initial estimates for the new pose. Also initialize data.points while in the first frame. +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 diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m index e1de7701e..a1f9a8a54 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/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 - -% Setting data options -TRIANGLE = chooseDataset(handles); -NCAMERAS = str2num(get(handles.numCamEdit,'String')); -SHOW_IMAGES = get(handles.showImagesCB,'Value'); +global SAVE_FIGURES SAVE_GRAPHS SHOW_TIMING % 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 -while (frame_i