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

I got rid of GlobalVars, added comments, and made "step" plot every time

release/4.3a0
Frank Dellaert 2012-06-10 03:31:09 +00:00
parent 80e2179a8d
commit 44530b2291
9 changed files with 203 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
matlab/examples/VisualISAMDemo.m
View File

@ -1,2 +1,15 @@
VisualISAMGlobalVars
% VisualISAMDemo: runs VisualSLAM iSAM demo in a GUI
% Authors: Duy Nguyen Ta
% 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 poseNoise pointNoise odometryNoise measurementNoise
global frame_i isam newFactors initialEstimates result
% Start GUI
VisualISAM_gui

12
matlab/examples/VisualISAMExample.m
View File

@ -10,12 +10,14 @@
% @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
%% Global variables used in VisualISAMExample
VisualISAMGlobalVars
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 = true;
TRIANGLE = false;
NCAMERAS = 20;
SHOW_IMAGES = false;
@ -28,8 +30,8 @@ ALWAYS_RELINEARIZE = false;
%% Display Options
SAVE_GRAPH = false;
PRINT_STATS = true;
DRAW_INTERVAL = 4;
PRINT_STATS = false;
DRAW_INTERVAL = 5;
CAMERA_INTERVAL = 1;
DRAW_TRUE_POSES = false;
SAVE_FIGURES = false;

123
matlab/examples/VisualISAMExample_triangle.m
View File

@ -10,100 +10,43 @@
% @author Duy-Nguyen Ta
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a triangle target, just 3 points on a plane
nPoints = 3;
r = 10;
points = {};
for j=1:nPoints
theta = (j-1)*2*pi/nPoints;
points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
%% Global variables used in VisualISAMExample
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
%% Create camera cameras on a circle around the triangle
nCameras = 10;
height = 10;
r = 30;
cameras = {};
K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
end
odometry = cameras{1}.pose.between(cameras{2}.pose);
%% Setting data options
TRIANGLE = true;
NCAMERAS = 10;
SHOW_IMAGES = false;
posepriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]');
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]');
pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
%% iSAM Options
HARD_CONSTRAINT = false;
POINT_PRIORS = false;
BATCH_INIT = true;
REORDER_INTERVAL = 10;
ALWAYS_RELINEARIZE = false;
%% Create an ISAM object for inference
isam = visualSLAMISAM(2);
%% Display Options
SAVE_GRAPH = false;
PRINT_STATS = false;
DRAW_INTERVAL = 5;
CAMERA_INTERVAL = 1;
DRAW_TRUE_POSES = false;
SAVE_FIGURES = false;
SAVE_GRAPHS = false;
%% Update ISAM
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
figure(1); clf;
for i=1:nCameras
%% Generate data and initialize iSAM with the first pose and points
VisualISAMGenerateData
VisualISAMInitialize
figure;
VisualISAMPlot
% Prior for the first pose or odometry for subsequent cameras
if (i==1)
newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, posepriorNoise);
newFactors.addPointPrior(symbol('l',1), points{1}, pointNoise);
else
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, odometryNoise);
end
% Visual measurement factors
for j=1:nPoints
zij = cameras{i}.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
% Initial estimates for the new pose. Also initialize points while in
% the first frame.
if (i==1)
initialEstimates.insertPose(symbol('x',i), cameras{i}.pose);
for j=1:nPoints
initialEstimates.insertPoint(symbol('l',j), points{j});
end
else
%TODO: this might be suboptimal since "result" is not the fully
%optimized result
if (i==2), prevPose = cameras{1}.pose;
else prevPose = result.pose(symbol('x',i-1)); end
initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
end
% Update ISAM, only update for the second frame onward
% Update the first frame will cause error, since it's under constrained
if (i>=2)
isam.update(newFactors, initialEstimates);
result = isam.estimate();
% Plot results
h=figure(1); clf;
hold on;
for j=1:nPoints
P = isam.marginalCovariance(symbol('l',j));
point_j = result.point(symbol('l',j));
plot3(point_j.x, point_j.y, point_j.z,'marker','o');
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end
for ii=1:i
P = isam.marginalCovariance(symbol('x',ii));
pose_ii = result.pose(symbol('x',ii));
plotPose3(pose_ii,P,10);
end
axis([-35 35 -35 35 -35 35])
view([36 34])
colormap('hot')
% print(h,'-dpng',sprintf('vISAM_%03d.png',i));
% Reset newFactors and initialEstimates to prepare for the next
% update
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
%% Main loop for iSAM: stepping through all poses
for frame_i=2:NCAMERAS
VisualISAMStep
if mod(frame_i,DRAW_INTERVAL)==0
VisualISAMPlot
end
end

10
matlab/examples/VisualISAMGenerateData.m
View File

@ -1,4 +1,12 @@
VisualISAMGlobalVars
% 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 ?
%% Generate simulated data
points = {};

7
matlab/examples/VisualISAMGlobalVars.m
View File

@ -1,7 +0,0 @@
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
global nPoints points K cameras odometry
global poseNoise pointNoise odometryNoise measurementNoise
global frame_i isam newFactors initialEstimates result

17
matlab/examples/VisualISAMInitialize.m
View File

@ -1,4 +1,15 @@
VisualISAMGlobalVars
function VisualISAMStep
% VisualISAMStep: execute one update step of visualSLAM::iSAM object
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global cameras points pointNoise poseNoise measurementNoise K
% global variables, output
global isam newFactors initialEstimates frame_i result
% options
global REORDER_INTERVAL HARD_CONSTRAINT POINT_PRIORS
%% Initialize iSAM
isam = visualSLAMISAM(REORDER_INTERVAL);
@ -14,7 +25,7 @@ else
end
initialEstimates.insertPose(i1,pose1);
% Add visual measurement factors from first pose
for j=1:nPoints
for j=1:size(points,2)
jj = symbol('l',j);
if POINT_PRIORS % add point priors
newFactors.addPointPrior(jj, points{j}, pointNoise);
@ -24,6 +35,6 @@ for j=1:nPoints
initialEstimates.insertPoint(jj, points{j});
end
frame_i = 1
frame_i = 1;
result = initialEstimates;
cla;

16
matlab/examples/VisualISAMPlot.m
View File

@ -1,13 +1,17 @@
VisualISAMGlobalVars
% VisualISAMPlot: plot current state of visualSLAM::iSAM object
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global points cameras frame_i isam result
% options
global CAMERA_INTERVAL DRAW_TRUE_POSES SAVE_FIGURES SAVE_GRAPHS
%% Plot results
tic
% h=figure(2);clf
% set(1,'NumberTitle','off','Name','Visual iSAM');
h=gca;
cla(h);
hold on;
sprintf('Computing marginals and plotting. Please wait...')
for j=1:size(points,2)
point_j = result.point(symbol('l',j));
plot3(point_j.x, point_j.y, point_j.z,'marker','o');
@ -21,7 +25,7 @@ for ii=1:CAMERA_INTERVAL:frame_i
if (frame_i>1)
P = isam.marginalCovariance(symbol('x',ii));
else
P = []
P = [];
end
plotPose3(pose_ii,P,10);
if DRAW_TRUE_POSES % show ground truth
@ -31,9 +35,7 @@ end
axis([-40 40 -40 40 -10 20]);axis equal
view(3)
colormap('hot')
sprintf('Done!')
% figure(2);
t=toc;
% if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end
if SAVE_FIGURES

38
matlab/examples/VisualISAMStep.m
View File

@ -1,24 +1,46 @@
VisualISAMGlobalVars
function VisualISAMStep
% VisualISAMStep: execute one update step of visualSLAM::iSAM object
% Authors: Duy Nguyen Ta and Frank Dellaert
% global variables, input
global frame_i odometry odometryNoise newFactors initialEstimates
global points cameras measurementNoise K
% global variables, input/output
global isam
% global variables, output
global result
% options
global BATCH_INIT SHOW_TIMING ALWAYS_RELINEARIZE
global SAVE_GRAPH PRINT_STATS
% iSAM expects us to give it a new set of factors
% along with initial estimates for any new variables introduced.
% We do not clear in frame 2 so we add to the factors added in Initialize
if frame_i > 2
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
end
%% Add odometry
newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise);
%% Add visual measurement factors
for j=1:nPoints
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);
end
%% Initial estimates for the new pose. Also initialize points while in the first frame.
%TODO: this might be suboptimal since "result" is not the fully optimized result
if (frame_i==2), prevPose = cameras{1}.pose;
else, prevPose = result.pose(symbol('x',frame_i-1)); end
initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
%% Update ISAM
if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses
initialEstimates
fullyOptimized = newFactors.optimize(initialEstimates)
fullyOptimized = newFactors.optimize(initialEstimates);
initialEstimates = fullyOptimized;
end
% figure(1);tic;
@ -30,14 +52,10 @@ if ALWAYS_RELINEARIZE % re-linearize
isam.reorder_relinearize();
end
if SAVE_GRAPH
isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
end
if PRINT_STATS
isam.printStats();
end
%% Reset newFactors and initialEstimates to prepare for the next update
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;

76
matlab/examples/VisualISAM_gui.m
View File

@ -1,26 +1,9 @@
function varargout = VisualISAM_gui(varargin)
% VISUALISAM_GUI MATLAB code for VisualISAM_gui.fig
% VISUALISAM_GUI, by itself, creates a new VISUALISAM_GUI or raises the existing
% singleton*.
%
% H = VISUALISAM_GUI returns the handle to a new VISUALISAM_GUI or the handle to
% the existing singleton*.
%
% VISUALISAM_GUI('CALLBACK',hObject,~,handles,...) calls the local
% function named CALLBACK in VISUALISAM_GUI.M with the given input arguments.
%
% VISUALISAM_GUI('Property','Value',...) creates a new VISUALISAM_GUI or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before VisualISAM_gui_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to VisualISAM_gui_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help VisualISAM_gui
% VisualISAM_gui: runs VisualSLAM iSAM demo in GUI
% Interface is defined by VisualISAM_gui.fig
% You can run this file directly, but won't have access to globals
% By running ViusalISAMDemo, you see all variables in command prompt
% Authors: Duy Nguyen Ta
% Last Modified by GUIDE v2.5 09-Jun-2012 00:56:47
@ -55,9 +38,6 @@ handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes VisualISAM_gui wait for user response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = VisualISAM_gui_OutputFcn(hObject, ~, handles)
% varargout cell array for returning output args (see VARARGOUT);
@ -69,7 +49,7 @@ varargout{1} = handles.output;
% Convenient functions
%----------------------------------------------------------
function showFramei(hObject, handles)
VisualISAMGlobalVars
global frame_i
set(handles.frameStatus, 'String', sprintf('Frame: %d',frame_i));
drawnow
guidata(hObject, handles);
@ -90,27 +70,32 @@ function triangle = chooseDataset(handles)
end
function initOptions(handles)
VisualISAMGlobalVars
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')
TRIANGLE = chooseDataset(handles);
NCAMERAS = str2num(get(handles.numCamEdit,'String'));
SHOW_IMAGES = 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.numCamEdit,'String'))
ALWAYS_RELINEARIZE = get(handles.alwaysRelinearizeCB,'Value')
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'));
ALWAYS_RELINEARIZE = 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')
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');
%----------------------------------------------------------
@ -242,7 +227,6 @@ function saveGraphsCB_Callback(hObject, ~, handles)
% --- Executes on button press in intializeButton.
function intializeButton_Callback(hObject, ~, handles)
VisualISAMGlobalVars
initOptions(handles)
VisualISAMGenerateData
VisualISAMInitialize
@ -252,7 +236,7 @@ function intializeButton_Callback(hObject, ~, handles)
% --- Executes on button press in runButton.
function runButton_Callback(hObject, ~, handles)
VisualISAMGlobalVars
global frame_i NCAMERAS DRAW_INTERVAL
while (frame_i<NCAMERAS)
frame_i = frame_i+1;
showFramei(hObject, handles)
@ -267,17 +251,15 @@ function runButton_Callback(hObject, ~, handles)
% --- Executes on button press in stepButton.
function stepButton_Callback(hObject, ~, handles)
VisualISAMGlobalVars
global frame_i NCAMERAS DRAW_INTERVAL
if (frame_i<NCAMERAS)
frame_i = frame_i+1;
showFramei(hObject, handles)
VisualISAMStep
if mod(frame_i,DRAW_INTERVAL)==0
showWaiting(handles, 'Computing marginals...');
VisualISAMPlot
showWaiting(handles, '');
end
end
% --- Executes on button press in plotButton.
function plotButton_Callback(hObject, ~, handles)