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

options structure all the way

release/4.3a0
Frank Dellaert 2012-06-10 15:26:59 +00:00
parent 4107167633
commit 6b23258b97
8 changed files with 130 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
matlab/examples/VisualISAMDemo.m
View File

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

55
matlab/examples/VisualISAMExample.m
View File

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

55
matlab/examples/VisualISAMExample_triangle.m
View File

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

5
matlab/examples/VisualISAMGenerateData.m
View File

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

15
matlab/examples/VisualISAMInitialize.m
View File

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

64
matlab/examples/VisualISAMPlot.m
View File

@ -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));
P = isam.marginalCovariance(jj);
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));
%% 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 = [];
P = isam.marginalCovariance(ii);
plotPose3(pose_i,P,10);
end
plotPose3(pose_ii,P,10);
if DRAW_TRUE_POSES % show ground truth
plotPose3(data.cameras{ii}.pose,0.001*eye(6),10);
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

8
matlab/examples/VisualISAMStep.m
View File

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

56
matlab/examples/VisualISAM_gui.m
View File

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