new prototype for VisualISAM_gui from Frank's example with many options

matlab/examples/VisualISAM_gui/VisualISAMExample_triangle2.m

@@ -1,16 +1,22 @@
 %% data
-data = VisualISAMData_triangle();
+%data = VisualISAMData_triangle();
+data = VisualISAMData_cube();
 
 %% init
 [isam results] = VisualISAMInitialize(data);
 sprintf('Frame 1,2:')
 results
+figure(1); clf;
+VisualISAMPlot(results, data);
 
 %% Next frame index
-frame_i=3; 
+frame_i=2; 
 %% All steps
 while (frame_i<size(data.cameras,2))
     %% one step
-    [isam results] = VisualISAMStep(frame_i, isam, data, results); frame_i = frame_i+1;
+    frame_i = frame_i+1;
+    [isam results] = VisualISAMStep(frame_i, isam, data, results); 
     results
+    figure(1); clf;
+    VisualISAMPlot(results, data);
 end

matlab/examples/VisualISAM_gui2/VisualISAM_gui.m

@@ -0,0 +1,175 @@
+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,eventData,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
+
+% Last Modified by GUIDE v2.5 08-Jun-2012 12:35:14
+
+% 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',   []);
+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 VisualISAM_gui is made visible.
+function VisualISAM_gui_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 VisualISAM_gui (see VARARGIN)
+
+% Choose default command line output for VisualISAM_gui
+%% Data Options
+handles.TRIANGLE = true;
+handles.NCAMERAS = 20;
+handles.SHOW_IMAGES = false;
+
+%% iSAM Options
+handles.HARD_CONSTRAINT = false;
+handles.POINT_PRIORS = false;
+handles.BATCH_INIT = true;
+handles.REORDER_INTERVAL=10;
+handles.ALWAYS_RELINEARIZE = false;
+
+%% Display Options
+handles.SAVE_GRAPH = false;
+handles.PRINT_STATS = true;
+handles.DRAW_INTERVAL = 1;
+handles.CAMERA_INTERVAL = 1;
+handles.DRAW_TRUE_POSES = false;
+handles.SAVE_FIGURES = false;
+handles.SAVE_GRAPHS = false;
+
+handles = vData(handles);
+handles = vInit(handles);
+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, 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;
+
+function handles = initialize(handles)
+    handles=vData(handles);
+    handles=vInit(handles);
+    handles.result = {};
+    cla;
+    handles
+    
+% --- Executes on button press in intializeButton.
+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)
+    guidata(hObject,handles)
+
+% --- Executes on button press in stepButton.
+function stepButton_Callback(hObject, eventdata, handles)
+% hObject    handle to stepButton (see GCBO)
+% 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)
+        sprintf('Frame %d:', handles.frame_i)
+        handles = vStep(handles);
+        guidata(hObject,handles)
+    end
+
+% --- 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 = cellstr(get(hObject,'String')) returns popupmenu1 contents as cell array
+%        contents{get(hObject,'Value')} returns selected item from popupmenu1
+    str = cellstr(get(hObject,'String'));
+    sel = get(hObject,'Value');
+    switch str{sel}
+        case 'triangle'
+            handles.TRIANGLE = true
+        case 'cube'
+            handles.TRIANGLE = false
+    end
+    handles = initialize(handles);
+    guidata(hObject, handles);
+
+% --- 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
+
+
+% --- Executes on button press in backButton.
+function backButton_Callback(hObject, eventdata, handles)
+% hObject    handle to backButton (see GCBO)
+% 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 runButton.
+function runButton_Callback(hObject, eventdata, handles)
+% hObject    handle to runButton (see GCBO)
+% 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 stopButton.
+function stopButton_Callback(hObject, eventdata, handles)
+% hObject    handle to stopButton (see GCBO)
+% eventdata  reserved - to be defined in a future version of MATLAB
+% handles    structure with handles and user data (see GUIDATA)
+    sprintf('Not yet implemented')

matlab/examples/VisualISAM_gui2/vData.m

@@ -0,0 +1,51 @@
+function [ handles ] = vData( handles )
+%VDATA Summary of this function goes here
+%   Detailed explanation goes here
+%% Generate simulated data
+if handles.TRIANGLE % Create a triangle target, just 3 points on a plane
+    handles.nPoints = 3;
+    r = 10;
+    for j=1:handles.nPoints
+        theta = (j-1)*2*pi/handles.nPoints;
+        handles.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
+    end
+else % 3D landmarks as vertices of a cube
+    handles.nPoints = 8;
+    handles.points = {gtsamPoint3([10 10 10]'),...
+        gtsamPoint3([-10 10 10]'),...
+        gtsamPoint3([-10 -10 10]'),...
+        gtsamPoint3([10 -10 10]'),...
+        gtsamPoint3([10 10 -10]'),...
+        gtsamPoint3([-10 10 -10]'),...
+        gtsamPoint3([-10 -10 -10]'),...
+        gtsamPoint3([10 -10 -10]')};
+end
+
+%% Create camera cameras on a circle around the triangle
+height = 10; r = 40;
+handles.K = gtsamCal3_S2(500,500,0,640/2,480/2);
+for i=1:handles.NCAMERAS
+    theta = (i-1)*2*pi/handles.NCAMERAS;
+    t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
+    handles.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), handles.K);
+%     if handles.SHOW_IMAGES % show images
+%         figure(2+i);clf;hold on
+%         set(2+i,'NumberTitle','off','Name',sprintf('Camera %d',i));
+%         for j=1:nPoints
+%             zij = handles.cameras{i}.project(handles.points{j});
+%             plot(zij.x,zij.y,'*');
+%             axis([1 640 1 480]);
+%         end
+%     end
+end
+handles.odometry = handles.cameras{1}.pose.between(handles.cameras{2}.pose);
+
+
+%% Set Noise parameters
+handles.poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+handles.odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+handles.pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
+handles.measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
+
+end
+

matlab/examples/VisualISAM_gui2/vInit.m

@@ -0,0 +1,31 @@
+function [ handles ] = vInit( handles )
+%VINIT Summary of this function goes here
+%   Detailed explanation goes here
+%% Initialize iSAM
+handles.isam = visualSLAMISAM(handles.REORDER_INTERVAL);
+handles.newFactors = visualSLAMGraph;
+handles.initialEstimates = visualSLAMValues;
+i1 = symbol('x',1);
+camera1 = handles.cameras{1};
+pose1 = camera1.pose;
+if handles.HARD_CONSTRAINT % add hard constraint
+    handles.newFactors.addPoseConstraint(i1,pose1);
+else
+    handles.newFactors.addPosePrior(i1,pose1, handles.poseNoise);
+end
+handles.initialEstimates.insertPose(i1,pose1);
+% Add visual measurement factors from first pose
+for j=1:handles.nPoints
+    jj = symbol('l',j);
+    if handles.POINT_PRIORS % add point priors
+        handles.newFactors.addPointPrior(jj, handles.points{j}, handles.pointNoise);
+    end
+    zij = camera1.project(handles.points{j});
+    handles.newFactors.addMeasurement(zij, handles.measurementNoise, i1, jj, handles.K);
+    handles.initialEstimates.insertPoint(jj, handles.points{j});
+end
+
+handles.frame_i = 2;
+
+end
+

matlab/examples/VisualISAM_gui2/vStep.m

@@ -0,0 +1,85 @@
+function [ handles ] = vStep( handles )
+%VSTEP Summary of this function goes here
+%   Detailed explanation goes here
+
+    %% Add odometry
+    handles.newFactors.addOdometry(symbol('x',handles.frame_i-1), symbol('x',handles.frame_i), handles.odometry, handles.odometryNoise);
+    
+    %% Add visual measurement factors
+    for j=1:handles.nPoints
+        zij = handles.cameras{handles.frame_i}.project(handles.points{j});
+        handles.newFactors.addMeasurement(zij, handles.measurementNoise, symbol('x',handles.frame_i), symbol('l',j), handles.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
+    handles.frame_i
+    if (handles.frame_i==2), prevPose = handles.cameras{1}.pose;
+    else, prevPose = handles.result.pose(symbol('x',handles.frame_i-1)); end
+    handles.initialEstimates.insertPose(symbol('x',handles.frame_i), prevPose.compose(handles.odometry));
+    
+    %% Update ISAM
+    if handles.BATCH_INIT & (handles.frame_i==2) % Do a full optimize for first two poses
+        handles.initialEstimates
+        fullyOptimized = handles.newFactors.optimize(handles.initialEstimates)
+        handles.initialEstimates = fullyOptimized;
+    end
+%     figure(1);tic;
+    handles.isam.update(handles.newFactors, handles.initialEstimates);
+%     t=toc; plot(handles.frame_i,t,'r.'); tic
+    handles.result = handles.isam.estimate();
+%     t=toc; plot(handles.frame_i,t,'g.');
+    if handles.ALWAYS_RELINEARIZE % re-linearize
+        handles.isam.reorder_relinearize();
+    end
+    
+    if handles.SAVE_GRAPH
+        handles.isam.saveGraph(sprintf('VisualiSAM.dot',handles.frame_i));
+    end
+    if handles.PRINT_STATS
+        handles.isam.printStats();
+    end
+    handles.frame_i
+    handles.DRAW_INTERVAL
+    if mod(handles.frame_i,handles.DRAW_INTERVAL)==0
+        sprintf('Plotting')
+        %% 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
+