VisualiSAM, going crazy. MATLAB is amazing!

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
+    if SAVE_GRAPH
-    figure(NCAMERAS+1);clf
+        isam.saveGraph(sprintf('VisualiSAM.dot',i));
-    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:i
+    if mod(i,DRAW_INTERVAL)==0
-        P = isam.marginalCovariance(symbol('x',ii));
+        %% Plot results
-        pose_ii = result.pose(symbol('x',ii));
+        tic
-        plotPose3(pose_ii,P,10);
+        h=figure(2);clf
-        if DRAW_TRUE_POSES % show ground truth
+        set(1,'NumberTitle','off','Name','Visual iSAM');
-            plotPose3(cameras{ii}.pose,0.001*eye(6),10);
+        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;

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)