269 lines
9.8 KiB
Matlab
269 lines
9.8 KiB
Matlab
function varargout = VisualISAM_gui(varargin)
|
|
% 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 13-Jun-2012 23:15:43
|
|
|
|
% 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, ~, handles, varargin)
|
|
% This function has no output args, see OutputFcn.
|
|
% varargin command line arguments to VisualISAM_gui (see VARARGIN)
|
|
|
|
% Choose default command line output for VisualISAM_gui
|
|
handles.output = hObject;
|
|
|
|
% Update handles structure
|
|
guidata(hObject, handles);
|
|
|
|
% --- Outputs from this function are returned to the command line.
|
|
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;
|
|
|
|
|
|
%----------------------------------------------------------
|
|
% Convenient functions
|
|
%----------------------------------------------------------
|
|
function showFramei(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);
|
|
|
|
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
|
|
|
|
function initOptions(handles)
|
|
|
|
global options
|
|
|
|
% Data options
|
|
options.triangle = chooseDataset(handles);
|
|
options.nrCameras = str2num(get(handles.numCamEdit,'String'));
|
|
options.showImages = get(handles.showImagesCB,'Value');
|
|
|
|
% iSAM Options
|
|
options.hardConstraint = get(handles.hardConstraintCB,'Value');
|
|
options.pointPriors = get(handles.pointPriorsCB,'Value');
|
|
options.batchInitialization = get(handles.batchInitCB,'Value');
|
|
%options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
|
|
options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value');
|
|
|
|
% Display Options
|
|
options.saveDotFile = get(handles.saveGraphCB,'Value');
|
|
options.printStats = get(handles.printStatsCB,'Value');
|
|
options.drawInterval = str2num(get(handles.drawInterval,'String'));
|
|
options.cameraInterval = str2num(get(handles.cameraIntervalEdit,'String'));
|
|
options.drawTruePoses = get(handles.drawTruePosesCB,'Value');
|
|
options.saveFigures = get(handles.saveFiguresCB,'Value');
|
|
options.saveDotFiles = get(handles.saveGraphsCB,'Value');
|
|
|
|
%----------------------------------------------------------
|
|
% Callback functions for GUI elements
|
|
%----------------------------------------------------------
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function dataset_CreateFcn(hObject, ~, handles)
|
|
% 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 selection change in dataset.
|
|
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)
|
|
% Hint: edit controls usually have a white background on Windows.
|
|
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
|
|
set(hObject,'BackgroundColor','white');
|
|
end
|
|
|
|
function numCamEdit_Callback(hObject, ~, handles)
|
|
% Hints: get(hObject,'String') returns contents of numCamEdit as text
|
|
% str2double(get(hObject,'String')) returns contents of numCamEdit as a double
|
|
|
|
|
|
% --- Executes on button press in showImagesCB.
|
|
function showImagesCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of showImagesCB
|
|
|
|
|
|
% --- Executes on button press in hardConstraintCB.
|
|
function hardConstraintCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of hardConstraintCB
|
|
|
|
|
|
% --- Executes on button press in pointPriorsCB.
|
|
function pointPriorsCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of pointPriorsCB
|
|
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function batchInitCB_CreateFcn(hObject, eventdata, handles)
|
|
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
|
|
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function reorderIntervalText_CreateFcn(hObject, ~, handles)
|
|
% Hint: edit controls usually have a white background on Windows.
|
|
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
|
|
set(hObject,'BackgroundColor','white');
|
|
end
|
|
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function reorderIntervalEdit_CreateFcn(hObject, ~, handles)
|
|
% Hint: edit controls usually have a white background on Windows.
|
|
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
|
|
set(hObject,'BackgroundColor','white');
|
|
end
|
|
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function drawInterval_CreateFcn(hObject, ~, handles)
|
|
% Hint: edit controls usually have a white background on Windows.
|
|
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
|
|
set(hObject,'BackgroundColor','white');
|
|
end
|
|
|
|
|
|
function drawInterval_Callback(hObject, ~, handles)
|
|
% Hints: get(hObject,'String') returns contents of drawInterval as text
|
|
% str2double(get(hObject,'String')) returns contents of drawInterval as a double
|
|
|
|
|
|
function cameraIntervalEdit_Callback(hObject, ~, handles)
|
|
% Hints: get(hObject,'String') returns contents of cameraIntervalEdit as text
|
|
% str2double(get(hObject,'String')) returns contents of cameraIntervalEdit as a double
|
|
|
|
|
|
% --- Executes during object creation, after setting all properties.
|
|
function cameraIntervalEdit_CreateFcn(hObject, ~, handles)
|
|
% 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
|
|
|
|
|
|
% --- Executes on button press in saveGraphCB.
|
|
function saveGraphCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of saveGraphCB
|
|
|
|
|
|
% --- Executes on button press in printStatsCB.
|
|
function printStatsCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of printStatsCB
|
|
|
|
|
|
% --- Executes on button press in drawTruePosesCB.
|
|
function drawTruePosesCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of drawTruePosesCB
|
|
|
|
|
|
% --- Executes on button press in saveFiguresCB.
|
|
function saveFiguresCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of saveFiguresCB
|
|
|
|
|
|
% --- Executes on button press in saveGraphsCB.
|
|
function saveGraphsCB_Callback(hObject, ~, handles)
|
|
% Hint: get(hObject,'Value') returns toggle state of saveGraphsCB
|
|
|
|
% --- Executes on button press in intializeButton.
|
|
function intializeButton_Callback(hObject, ~, handles)
|
|
|
|
global frame_i truth data noiseModels isam result nextPoseIndex options
|
|
|
|
% initialize global options
|
|
initOptions(handles)
|
|
|
|
% Generate Data
|
|
[data,truth] = gtsam.VisualISAMGenerateData(options);
|
|
|
|
% Initialize and plot
|
|
[noiseModels,isam,result,nextPoseIndex] = gtsam.VisualISAMInitialize(data,truth,options);
|
|
cla
|
|
gtsam.VisualISAMPlot(truth, data, isam, result, options)
|
|
frame_i = 2;
|
|
showFramei(hObject, handles)
|
|
|
|
|
|
% --- Executes on button press in runButton.
|
|
function runButton_Callback(hObject, ~, handles)
|
|
global frame_i truth data noiseModels isam result nextPoseIndex options
|
|
while (frame_i<size(truth.cameras,2))
|
|
frame_i = frame_i+1;
|
|
showFramei(hObject, handles)
|
|
[isam,result,nextPoseIndex] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
|
|
if mod(frame_i,options.drawInterval)==0
|
|
showWaiting(handles, 'Computing marginals...');
|
|
gtsam.VisualISAMPlot(truth, data, isam, result, options)
|
|
showWaiting(handles, '');
|
|
end
|
|
end
|
|
|
|
|
|
% --- Executes on button press in stepButton.
|
|
function stepButton_Callback(hObject, ~, handles)
|
|
global frame_i truth data noiseModels isam result nextPoseIndex options
|
|
if (frame_i<size(truth.cameras,2))
|
|
frame_i = frame_i+1;
|
|
showFramei(hObject, handles)
|
|
[isam,result,nextPoseIndex] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
|
|
showWaiting(handles, 'Computing marginals...');
|
|
gtsam.VisualISAMPlot(truth, data, isam, result, options)
|
|
showWaiting(handles, '');
|
|
end
|