VisualISAM_gui2: add 2 options and take plot out of step

matlab/examples/VisualISAM_gui2/VisualISAM_gui.m

@@ -22,7 +22,7 @@ function varargout = VisualISAM_gui(varargin)
 
 % Edit the above text to modify the response to help VisualISAM_gui
 
-% Last Modified by GUIDE v2.5 08-Jun-2012 12:35:14
+% Last Modified by GUIDE v2.5 08-Jun-2012 14:00:55
 
 % Begin initialization code - DO NOT EDIT
 gui_Singleton = 1;
@@ -68,7 +68,7 @@ handles.ALWAYS_RELINEARIZE = false;
 %% Display Options
 handles.SAVE_GRAPH = false;
 handles.PRINT_STATS = true;
-handles.DRAW_INTERVAL = 1;
+handles.DRAW_INTERVAL = 4;
 handles.CAMERA_INTERVAL = 1;
 handles.DRAW_TRUE_POSES = false;
 handles.SAVE_FIGURES = false;
@@ -107,7 +107,9 @@ function intializeButton_Callback(hObject, eventdata, handles)
 % hObject    handle to intializeButton (see GCBO)
 % eventdata  reserved - to be defined in a future version of MATLAB
 % handles    structure with handles and user data (see GUIDATA)
-    handles=initialize(handles)
+    handles.DRAW_INTERVAL = str2num(get(handles.drawInterval,'String')) ;
+    handles.NCAMERAS = str2num(get(handles.numCamEdit,'String')) ;
+    handles = initialize(handles)
     guidata(hObject,handles)
 
 % --- Executes on button press in stepButton.
@@ -116,6 +118,7 @@ function stepButton_Callback(hObject, eventdata, handles)
 % eventdata  reserved - to be defined in a future version of MATLAB
 % handles    structure with handles and user data (see GUIDATA)
     if (handles.frame_i<handles.NCAMERAS)
+        handles.frame_i = handles.frame_i+1;
         sprintf('Frame %d:', handles.frame_i)
         handles = vStep(handles);
         guidata(hObject,handles)
@@ -173,3 +176,60 @@ function stopButton_Callback(hObject, eventdata, handles)
 % eventdata  reserved - to be defined in a future version of MATLAB
 % handles    structure with handles and user data (see GUIDATA)
     sprintf('Not yet implemented')
+
+
+% --- Executes on button press in plotButton.
+function plotButton_Callback(hObject, eventdata, handles)
+% hObject    handle to plotButton (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+    vPlot(handles);
+
+
+function drawInterval_Callback(hObject, eventdata, handles)
+% hObject    handle to drawInterval (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'String') returns contents of drawInterval as text
+%        str2double(get(hObject,'String')) returns contents of drawInterval as a double
+    handles.DRAW_INTERVAL = str2num(get(hObject,'String')) ;
+    handles
+    guidata(hObject,handles);
+
+% --- Executes during object creation, after setting all properties.
+function drawInterval_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to drawInterval (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: edit 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
+
+
+
+function numCamEdit_Callback(hObject, eventdata, handles)
+% hObject    handle to numCamEdit (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+
+% Hints: get(hObject,'String') returns contents of numCamEdit as text
+%        str2double(get(hObject,'String')) returns contents of numCamEdit as a double
+    handles.NCAMERAS = str2num(get(hObject,'String')) ;
+    handles
+    guidata(hObject,handles);
+
+% --- Executes during object creation, after setting all properties.
+function numCamEdit_CreateFcn(hObject, eventdata, handles)
+% hObject    handle to numCamEdit (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    empty - handles not created until after all CreateFcns called
+
+% Hint: edit 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

matlab/examples/VisualISAM_gui2/vInit.m

@@ -25,7 +25,7 @@ for j=1:handles.nPoints
     handles.initialEstimates.insertPoint(jj, handles.points{j});
 end
 
-handles.frame_i = 2;
+handles.frame_i = 1;
 
 end
 

matlab/examples/VisualISAM_gui2/vPlot.m

@@ -0,0 +1,45 @@
+function vPlot( handles )
+%VPLOT Summary of this function goes here
+%   Detailed explanation goes here
+if (handles.frame_i<2)
+    sprintf('Cannot plot the first frame')
+    return
+end
+    
+sprintf('Plotting to frame %d', handles.frame_i)
+cla;
+%% Plot results
+tic
+%         h=figure(2);clf
+%         set(1,'NumberTitle','off','Name','Visual iSAM');
+hold on;
+for j=1:handles.nPoints
+    P = handles.isam.marginalCovariance(symbol('l',j));
+    point_j = handles.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:handles.CAMERA_INTERVAL:handles.frame_i
+    P = handles.isam.marginalCovariance(symbol('x',ii));
+    pose_ii = handles.result.pose(symbol('x',ii));
+    plotPose3(pose_ii,P,10);
+    if handles.DRAW_TRUE_POSES % show ground truth
+        plotPose3(handles.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 handles.DRAW_INTERVAL~=handles.NCAMERAS, plot(handles.frame_i,t,'b.'); end
+if handles.SAVE_FIGURES
+    print(h,'-dpng',sprintf('VisualiSAM%03d.png',handles.frame_i));
+end
+if handles.SAVE_GRAPHS
+    handles.isam.saveGraph(sprintf('VisualiSAM%03d.dot',handles.frame_i));
+end
+hold off;
+
+end
+

matlab/examples/VisualISAM_gui2/vStep.m

@@ -42,44 +42,11 @@ function [ handles ] = vStep( handles )
     handles.frame_i
     handles.DRAW_INTERVAL
     if mod(handles.frame_i,handles.DRAW_INTERVAL)==0
-        sprintf('Plotting')
+        vPlot(handles);
-        %% Plot results
-        tic
-%         h=figure(2);clf
-%         set(1,'NumberTitle','off','Name','Visual iSAM');
-        hold on;
-        for j=1:handles.nPoints
-            P = handles.isam.marginalCovariance(symbol('l',j));
-            point_j = handles.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:handles.CAMERA_INTERVAL:handles.frame_i
-            P = handles.isam.marginalCovariance(symbol('x',ii));
-            pose_ii = handles.result.pose(symbol('x',ii));
-            plotPose3(pose_ii,P,10);
-            if handles.DRAW_TRUE_POSES % show ground truth
-                plotPose3(handles.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 handles.DRAW_INTERVAL~=handles.NCAMERAS, plot(handles.frame_i,t,'b.'); end
-        if handles.SAVE_FIGURES
-            print(h,'-dpng',sprintf('VisualiSAM%03d.png',handles.frame_i));
-        end
-        if handles.SAVE_GRAPHS
-            handles.isam.saveGraph(sprintf('VisualiSAM%03d.dot',handles.frame_i));
-        end
-        hold off;
     end
     
     %% Reset newFactors and initialEstimates to prepare for the next update
     handles.newFactors = visualSLAMGraph;
     handles.initialEstimates = visualSLAMValues;
-	handles.frame_i = handles.frame_i+1;
 end