12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

No more globals (except in gui and demo)

release/4.3a0
Frank Dellaert 2012-06-10 18:46:56 +00:00
parent cc829d36ae
commit 653d4ef131
6 changed files with 31 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matlab/examples/VisualISAMDemo.m
View File

@ -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

10
matlab/examples/VisualISAMExample.m
View File

@ -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
end

10
matlab/examples/VisualISAMExample_triangle.m
View File

@ -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
end

23
matlab/examples/VisualISAMInitialize.m
View File

@ -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

22
matlab/examples/VisualISAMStep.m
View File

@ -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;

17
matlab/examples/VisualISAM_gui.m
View File

@ -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<size(data.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
VisualISAMStep
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
if mod(frame_i,options.drawInterval)==0
showWaiting(handles, 'Computing marginals...');
VisualISAMPlot(data, isam, result, options)
@ -256,11 +256,11 @@ end
% --- Executes on button press in stepButton.
function stepButton_Callback(hObject, ~, handles)
global options data frame_i isam result
global frame_i data noiseModels isam result options
if (frame_i<size(data.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
VisualISAMStep
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
showWaiting(handles, 'Computing marginals...');
VisualISAMPlot(data, isam, result, options)
showWaiting(handles, '');
@ -269,7 +269,8 @@ end
% --- Executes on button press in plotButton.
function plotButton_Callback(hObject, ~, handles)
showWaiting(handles, 'Computing marginals...');
VisualISAMPlot;
global options data isam result
VisualISAMPlot(data, isam, result, options);
showWaiting(handles, '');