diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/examples/VisualISAMDemo.m index f4b975ad5..00518293a 100644 --- a/matlab/examples/VisualISAMDemo.m +++ b/matlab/examples/VisualISAMDemo.m @@ -3,9 +3,7 @@ % Make sure global variables are visible on command prompt % so you can examine how they change as you step through -global data -global poseNoise pointNoise odometryNoise measurementNoise -global frame_i isam result +global frame_i data noiseModels isam result options % Start GUI VisualISAM_gui \ No newline at end of file diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m index f7114b652..eadca5d2f 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/matlab/examples/VisualISAMExample.m @@ -10,10 +10,6 @@ % @author Duy-Nguyen Ta %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Global variables used in VisualISAMExample -global options data isam result frame_i -global poseNoise odometryNoise pointNoise measurementNoise - % Data Options options.triangle = false; options.nrCameras = 20; @@ -39,14 +35,14 @@ options.saveDotFiles = false; data = VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -VisualISAMInitialize(options) +[noiseModels,isam,result] = VisualISAMInitialize(data,options); figure(1); VisualISAMPlot(data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - VisualISAMStep + [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options); if mod(frame_i,options.drawInterval)==0 VisualISAMPlot(data, isam, result, options) end -end \ No newline at end of file +end diff --git a/matlab/examples/VisualISAMExample_triangle.m b/matlab/examples/VisualISAMExample_triangle.m index 2fdb35599..c9c416cb2 100644 --- a/matlab/examples/VisualISAMExample_triangle.m +++ b/matlab/examples/VisualISAMExample_triangle.m @@ -10,10 +10,6 @@ % @author Duy-Nguyen Ta %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Global variables used in VisualISAMExample -global options data isam result frame_i -global poseNoise odometryNoise pointNoise measurementNoise - % Data Options options.triangle = true; options.nrCameras = 10; @@ -39,14 +35,14 @@ options.saveDotFiles = false; data = VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -VisualISAMInitialize(options) +[noiseModels,isam,result] = VisualISAMInitialize(data,options); figure(1); VisualISAMPlot(data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - VisualISAMStep + [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options); if mod(frame_i,options.drawInterval)==0 VisualISAMPlot(data, isam, result, options) end -end \ No newline at end of file +end diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index d7489eb67..f79a30a93 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/matlab/examples/VisualISAMInitialize.m @@ -1,22 +1,15 @@ -function VisualInitialize(options) +function [noiseModels,isam,result] = VisualInitialize(data,options) % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % Authors: Duy Nguyen Ta and Frank Dellaert -% global variables, input -global data - -% global variables, output -global isam frame_i result -global poseNoise odometryNoise pointNoise measurementNoise - %% Initialize iSAM isam = visualSLAMISAM(options.reorderInterval); %% 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); +noiseModels.pose = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.odometry = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.point = gtsamSharedNoiseModel_Sigma(3, 0.1); +noiseModels.measurement = gtsamSharedNoiseModel_Sigma(2, 1.0); %% Add constraints/priors newFactors = visualSLAMGraph; @@ -26,7 +19,7 @@ for frame_i=1:2 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); + newFactors.addPosePrior(ii,data.cameras{frame_i}.pose, noiseModels.pose); end % TODO: init should not be from ground truth! initialEstimates.insertPose(ii,data.cameras{frame_i}.pose); @@ -38,7 +31,7 @@ for frame_i=1:2 for j=1:size(data.points,2) jj = symbol('l',j); zij = data.cameras{frame_i}.project(data.points{j}); - newFactors.addMeasurement(zij, measurementNoise, ii, jj, data.K); + newFactors.addMeasurement(zij, noiseModels.measurement, ii, jj, data.K); end end @@ -46,7 +39,7 @@ end for j=1:size(data.points,2) jj = symbol('l',j); if options.pointPriors % add point priors - newFactors.addPointPrior(jj, data.points{j}, pointNoise); + newFactors.addPointPrior(jj, data.points{j}, noiseModels.point); end initialEstimates.insertPoint(jj, data.points{j}); % TODO: should not be from ground truth! end diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index 30b0dfcce..ca951e5e5 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/matlab/examples/VisualISAMStep.m @@ -1,33 +1,25 @@ -function VisualISAMStep +function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options); % VisualISAMStep: execute one update step of visualSLAM::iSAM object % Authors: Duy Nguyen Ta and Frank Dellaert -% global variables, input -global options data odometryNoise measurementNoise frame_i - -% global variables, input/output -global isam - -% global variables, output -global result - % iSAM expects us to give it a new set of factors % along with initial estimates for any new variables introduced. newFactors = visualSLAMGraph; initialEstimates = visualSLAMValues; %% Add odometry -newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), data.odometry, odometryNoise); +i = double(result.nrPoses)+1; +newFactors.addOdometry(symbol('x',i-1), symbol('x',i), data.odometry, noiseModels.odometry); %% Add visual measurement factors 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); + zij = data.cameras{i}.project(data.points{j}); + newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K); end %% Initial estimates for the new pose. -prevPose = result.pose(symbol('x',frame_i-1)); -initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(data.odometry)); +prevPose = result.pose(symbol('x',i-1)); +initialEstimates.insertPose(symbol('x',i), prevPose.compose(data.odometry)); %% Update ISAM % figure(1);tic; diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m index 3adeb4b1c..49b73ab65 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/matlab/examples/VisualISAM_gui.m @@ -224,28 +224,28 @@ function saveGraphsCB_Callback(hObject, ~, handles) % --- Executes on button press in intializeButton. function intializeButton_Callback(hObject, ~, handles) -global options data isam result +global frame_i data noiseModels isam result options % initialize global options -global options initOptions(handles) % Generate Data data = VisualISAMGenerateData(options); % Initialize and plot -VisualISAMInitialize(options) +[noiseModels,isam,result] = VisualISAMInitialize(data,options); VisualISAMPlot(data, isam, result, options) +frame_i = 2; showFramei(hObject, handles) % --- Executes on button press in runButton. function runButton_Callback(hObject, ~, handles) -global options data frame_i isam result +global frame_i data noiseModels isam result options while (frame_i