diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/examples/VisualISAMDemo.m index 626b3a944..df83ab32c 100644 --- a/matlab/examples/VisualISAMDemo.m +++ b/matlab/examples/VisualISAMDemo.m @@ -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 \ No newline at end of file diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m index 633c20cb5..136adb3ed 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/matlab/examples/VisualISAMExample.m @@ -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; diff --git a/matlab/examples/VisualISAMExample_triangle.m b/matlab/examples/VisualISAMExample_triangle.m index c0dabec72..f6b030d1d 100644 --- a/matlab/examples/VisualISAMExample_triangle.m +++ b/matlab/examples/VisualISAMExample_triangle.m @@ -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 - - % 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 +%% Generate data and initialize iSAM with the first pose and points +VisualISAMGenerateData +VisualISAMInitialize +figure; +VisualISAMPlot - % 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 \ No newline at end of file diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m index 03222f3cf..638bdd90b 100644 --- a/matlab/examples/VisualISAMGenerateData.m +++ b/matlab/examples/VisualISAMGenerateData.m @@ -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 = {}; diff --git a/matlab/examples/VisualISAMGlobalVars.m b/matlab/examples/VisualISAMGlobalVars.m deleted file mode 100644 index 2438a9ea0..000000000 --- a/matlab/examples/VisualISAMGlobalVars.m +++ /dev/null @@ -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 diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index c8f341096..8fd23aeaa 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/matlab/examples/VisualISAMInitialize.m @@ -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; \ No newline at end of file diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m index d7f5c55b0..4e7ef07da 100644 --- a/matlab/examples/VisualISAMPlot.m +++ b/matlab/examples/VisualISAMPlot.m @@ -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 diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index f5e1a96b6..ab417d3d2 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/matlab/examples/VisualISAMStep.m @@ -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; \ No newline at end of file diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m index d13e45e3f..e1de7701e 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/matlab/examples/VisualISAM_gui.m @@ -1,37 +1,20 @@ 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 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... - 'gui_Singleton', gui_Singleton, ... - 'gui_OpeningFcn', @VisualISAM_gui_OpeningFcn, ... - 'gui_OutputFcn', @VisualISAM_gui_OutputFcn, ... - 'gui_LayoutFcn', [] , ... - 'gui_Callback', []); + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @VisualISAM_gui_OpeningFcn, ... + 'gui_OutputFcn', @VisualISAM_gui_OutputFcn, ... + 'gui_LayoutFcn', [] , ... + 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end @@ -55,11 +38,8 @@ 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) +function varargout = VisualISAM_gui_OutputFcn(hObject, ~, handles) % varargout cell array for returning output args (see VARARGOUT); % Get default command line output from handles structure varargout{1} = handles.output; @@ -69,49 +49,54 @@ varargout{1} = handles.output; % Convenient functions %---------------------------------------------------------- function showFramei(hObject, handles) - VisualISAMGlobalVars - set(handles.frameStatus, 'String', sprintf('Frame: %d',frame_i)); - drawnow - guidata(hObject, handles); - +global frame_i +set(handles.frameStatus, 'String', sprintf('Frame: %d',frame_i)); +drawnow +guidata(hObject, handles); + function showWaiting(handles, status) - set(handles.waitingStatus,'String', status); - drawnow - guidata(handles.waitingStatus, handles); - +set(handles.waitingStatus,'String', status); +drawnow +guidata(handles.waitingStatus, handles); + function triangle = chooseDataset(handles) - str = cellstr(get(handles.dataset,'String')); - sel = get(handles.dataset,'Value'); - switch str{sel} - case 'triangle' - triangle = true; - case 'cube' - triangle = false; - end - +str = cellstr(get(handles.dataset,'String')); +sel = get(handles.dataset,'Value'); +switch str{sel} + case 'triangle' + triangle = true; + case 'cube' + triangle = false; +end + function initOptions(handles) - VisualISAMGlobalVars - % Setting data options - 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') +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'); + +% 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'); + +% 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'); - % 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') - %---------------------------------------------------------- % Callback functions for GUI elements @@ -130,7 +115,7 @@ end function dataset_Callback(hObject, ~, handles) % Hints: contents = cellstr(get(hObject,'String')) returns dataset contents as cell array % contents{get(hObject,'Value')} returns selected item from dataset - + % --- Executes during object creation, after setting all properties. function numCamEdit_CreateFcn(hObject, ~, handles) @@ -161,13 +146,13 @@ function pointPriorsCB_Callback(hObject, ~, handles) % --- Executes during object creation, after setting all properties. function batchInitCB_CreateFcn(hObject, eventdata, handles) - set(hObject,'Value',1); - +set(hObject,'Value',1); + % --- Executes on button press in batchInitCB. function batchInitCB_Callback(hObject, ~, handles) % Hint: get(hObject,'Value') returns toggle state of batchInitCB - + % --- Executes on button press in alwaysRelinearizeCB. function alwaysRelinearizeCB_Callback(hObject, ~, handles) % Hint: get(hObject,'Value') returns toggle state of alwaysRelinearizeCB @@ -242,48 +227,45 @@ function saveGraphsCB_Callback(hObject, ~, handles) % --- Executes on button press in intializeButton. function intializeButton_Callback(hObject, ~, handles) - VisualISAMGlobalVars - initOptions(handles) - VisualISAMGenerateData - VisualISAMInitialize - VisualISAMPlot - showFramei(hObject, handles) - - +initOptions(handles) +VisualISAMGenerateData +VisualISAMInitialize +VisualISAMPlot +showFramei(hObject, handles) + + % --- Executes on button press in runButton. function runButton_Callback(hObject, ~, handles) - VisualISAMGlobalVars - while (frame_i