diff --git a/examples/matlab/VisualISAMExample.m b/examples/matlab/VisualISAMExample.m index 89947cd40..e1116552d 100644 --- a/examples/matlab/VisualISAMExample.m +++ b/examples/matlab/VisualISAMExample.m @@ -12,15 +12,25 @@ clear -%% Set Options here +%% Data Options TRIANGLE = false; -NCAMERAS = 10; +NCAMERAS = 20; SHOW_IMAGES = false; + +%% iSAM Options HARD_CONSTRAINT = false; POINT_PRIORS = false; BATCH_INIT = true; +REORDER_INTERVAL=10; ALWAYS_RELINEARIZE = false; -DRAW_TRUE_POSES = true; + +%% Display Options +SAVE_GRAPH = false; +DRAW_INTERVAL = 20; +CAMERA_INTERVAL = 1; +DRAW_TRUE_POSES = false; +SAVE_FIGURES = false; +SAVE_GRAPHS = false; %% Generate simulated data if TRIANGLE % Create a triangle target, just 3 points on a plane @@ -50,7 +60,8 @@ for i=1:NCAMERAS t = gtsamPoint3([r*cos(theta), r*sin(theta), height]'); cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K); if SHOW_IMAGES % show images - figure(i);clf;hold on + figure(2+i);clf;hold on + set(2+i,'NumberTitle','off','Name',sprintf('Camera %d',i)); for j=1:nPoints zij = cameras{i}.project(points{j}); plot(zij.x,zij.y,'*'); @@ -68,7 +79,7 @@ pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); %% Initialize iSAM -isam = visualSLAMISAM; +isam = visualSLAMISAM(REORDER_INTERVAL); newFactors = visualSLAMGraph; initialEstimates = visualSLAMValues; i1 = symbol('x',1); @@ -92,6 +103,8 @@ for j=1:nPoints end %% Run iSAM Loop +figure(1);clf;hold on; +set(1,'NumberTitle','off','Name','iSAM timing'); for i=2:NCAMERAS %% Add odometry @@ -115,33 +128,51 @@ for i=2:NCAMERAS fullyOptimized = newFactors.optimize(initialEstimates) initialEstimates = fullyOptimized; end + figure(1);tic; isam.update(newFactors, initialEstimates); + t=toc; plot(i,t,'r.'); tic result = isam.estimate(); + t=toc; plot(i,t,'g.'); if ALWAYS_RELINEARIZE % re-linearize isam.reorder_relinearize(); end - %% Plot results - figure(NCAMERAS+1);clf - hold on; - for j=1:size(points,2) - 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); + if SAVE_GRAPH + isam.saveGraph(sprintf('VisualiSAM.dot',i)); end - for ii=1:i - P = isam.marginalCovariance(symbol('x',ii)); - pose_ii = result.pose(symbol('x',ii)); - plotPose3(pose_ii,P,10); - if DRAW_TRUE_POSES % show ground truth - plotPose3(cameras{ii}.pose,0.001*eye(6),10); + if mod(i,DRAW_INTERVAL)==0 + %% Plot results + tic + h=figure(2);clf + set(1,'NumberTitle','off','Name','Visual iSAM'); + hold on; + for j=1:size(points,2) + 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:CAMERA_INTERVAL:i + P = isam.marginalCovariance(symbol('x',ii)); + pose_ii = result.pose(symbol('x',ii)); + plotPose3(pose_ii,P,10); + if DRAW_TRUE_POSES % show ground truth + plotPose3(cameras{ii}.pose,0.001*eye(6),10); + end + end + axis([-40 40 -40 40 -10 20]);axis equal + view(3) + colormap('hot') + figure(2); + t=toc; + if DRAW_INTERVAL~=NCAMERAS, plot(i,t,'b.'); end + if SAVE_FIGURES + print(h,'-dpng',sprintf('VisualiSAM%03d.png',i)); + end + if SAVE_GRAPHS + isam.saveGraph(sprintf('VisualiSAM%03d.dot',i)); end end - axis([-40 40 -40 40 -10 20]);axis equal - view(2) - colormap('hot') - %print(h,'-dpng',sprintf('VisualISAM_%03d.png',i)); %% Reset newFactors and initialEstimates to prepare for the next update newFactors = visualSLAMGraph; diff --git a/examples/matlab/iSAMgui.fig b/examples/matlab/iSAMgui.fig new file mode 100644 index 000000000..5f6154b49 Binary files /dev/null and b/examples/matlab/iSAMgui.fig differ diff --git a/examples/matlab/iSAMgui.m b/examples/matlab/iSAMgui.m new file mode 100644 index 000000000..71cd53855 --- /dev/null +++ b/examples/matlab/iSAMgui.m @@ -0,0 +1,171 @@ +function varargout = iSAMgui(varargin) +% ISAMGUI MATLAB code for iSAMgui.fig +% ISAMGUI, by itself, creates a new ISAMGUI or raises the existing +% singleton*. +% +% H = ISAMGUI returns the handle to a new ISAMGUI or the handle to +% the existing singleton*. +% +% ISAMGUI('CALLBACK',hObject,eventData,handles,...) calls the local +% function named CALLBACK in ISAMGUI.M with the given input arguments. +% +% ISAMGUI('Property','Value',...) creates a new ISAMGUI or raises the +% existing singleton*. Starting from the left, property value pairs are +% applied to the GUI before iSAMgui_OpeningFcn gets called. An +% unrecognized property name or invalid value makes property application +% stop. All inputs are passed to iSAMgui_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 iSAMgui + +% Last Modified by GUIDE v2.5 07-Jun-2012 01:55:37 + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @iSAMgui_OpeningFcn, ... + 'gui_OutputFcn', @iSAMgui_OutputFcn, ... + 'gui_LayoutFcn', [] , ... + 'gui_Callback', []); +if nargin && ischar(varargin{1}) + gui_State.gui_Callback = str2func(varargin{1}); +end + +if nargout + [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); +else + gui_mainfcn(gui_State, varargin{:}); +end +% End initialization code - DO NOT EDIT + +% --- Executes just before iSAMgui is made visible. +function iSAMgui_OpeningFcn(hObject, eventdata, handles, varargin) +% This function has no output args, see OutputFcn. +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% varargin command line arguments to iSAMgui (see VARARGIN) + +% Choose default command line output for iSAMgui +handles.output = hObject; + +% Update handles structure +guidata(hObject, handles); + +% This sets up the initial plot - only do when we are invisible +% so window can get raised using iSAMgui. +if strcmp(get(hObject,'Visible'),'off') + plot(rand(5)); +end + +% UIWAIT makes iSAMgui wait for user response (see UIRESUME) +% uiwait(handles.figure1); + + +% --- Outputs from this function are returned to the command line. +function varargout = iSAMgui_OutputFcn(hObject, eventdata, handles) +% varargout cell array for returning output args (see VARARGOUT); +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Get default command line output from handles structure +varargout{1} = handles.output; + +% --- Executes on button press in pushbutton1. +function pushbutton1_Callback(hObject, eventdata, handles) +% hObject handle to pushbutton1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +axes(handles.axes1); +cla; + +popup_sel_index = get(handles.popupmenu1, 'Value'); +switch popup_sel_index + case 1 + plot(rand(5)); + case 2 + plot(sin(1:0.01:25.99)); + case 3 + bar(1:.5:10); + case 4 + plot(membrane); + case 5 + surf(peaks); +end + + +% -------------------------------------------------------------------- +function FileMenu_Callback(hObject, eventdata, handles) +% hObject handle to FileMenu (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + + +% -------------------------------------------------------------------- +function OpenMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to OpenMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +file = uigetfile('*.fig'); +if ~isequal(file, 0) + open(file); +end + +% -------------------------------------------------------------------- +function PrintMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to PrintMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +printdlg(handles.figure1) + +% -------------------------------------------------------------------- +function CloseMenuItem_Callback(hObject, eventdata, handles) +% hObject handle to CloseMenuItem (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +selection = questdlg(['Close ' get(handles.figure1,'Name') '?'],... + ['Close ' get(handles.figure1,'Name') '...'],... + 'Yes','No','Yes'); +if strcmp(selection,'No') + return; +end + +delete(handles.figure1) + + +% --- Executes on selection change in popupmenu1. +function popupmenu1_Callback(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Hints: contents = get(hObject,'String') returns popupmenu1 contents as cell array +% contents{get(hObject,'Value')} returns selected item from popupmenu1 + + +% --- Executes during object creation, after setting all properties. +function popupmenu1_CreateFcn(hObject, eventdata, handles) +% hObject handle to popupmenu1 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles empty - handles not created until after all CreateFcns called + +% Hint: popupmenu controls usually have a white background on Windows. +% See ISPC and COMPUTER. +if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) + set(hObject,'BackgroundColor','white'); +end + +set(hObject, 'String', {'plot(rand(5))', 'plot(sin(1:0.01:25))', 'bar(1:.5:10)', 'plot(membrane)', 'surf(peaks)'}); + + +% --- Executes on button press in pushbutton4. +function pushbutton4_Callback(hObject, eventdata, handles) +% hObject handle to pushbutton4 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA)