12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove visualISAM_gui folders

release/4.3a0
Duy-Nguyen Ta 2012-06-08 20:09:30 +00:00
parent 96e71bb10e
commit 017b591f2c
14 changed files with 0 additions and 995 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
matlab/examples/VisualISAM_gui/VisualISAMData_cube.m
View File

@ -1,34 +0,0 @@
function [ data ] = VisualISAMData_cube()
%VISUALISAMDATA_TRIANGLE Generate data for visual ISAM triangle example.
% Landmarks include 3 points around the world's origin on the z=0 plane.
% Cameras are on a circle at a certain height, looking at the origin.
%% Create a triangle target, just 3 points on a plane
nPoints = 8;
data.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]')};
%% Create camera cameras on a circle around the triangle
nCameras = 10;
height = 0;
r = 30;
data.cameras = {};
data.K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K);
end
data.posePriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]');
data.odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]');
data.pointPriorNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
data.measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
end

32
matlab/examples/VisualISAM_gui/VisualISAMData_triangle.m
View File

@ -1,32 +0,0 @@
function [ data ] = VisualISAMData_triangle()
%VISUALISAMDATA_TRIANGLE Generate data for visual ISAM triangle example.
% Landmarks include 3 points around the world's origin on the z=0 plane.
% Cameras are on a circle at a certain height, looking at the origin.
%% Create a triangle target, just 3 points on a plane
nPoints = 3;
r = 10;
data.points = {};
for j=1:nPoints
theta = (j-1)*2*pi/nPoints;
data.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
%% Create camera cameras on a circle around the triangle
nCameras = 10;
height = 10;
r = 30;
data.cameras = {};
data.K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K);
end
data.posePriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]');
data.odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]');
data.pointPriorNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
data.measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
end

22
matlab/examples/VisualISAM_gui/VisualISAMExample_triangle2.m
View File

@ -1,22 +0,0 @@
%% data
%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=2;
%% All steps
while (frame_i<size(data.cameras,2))
%% one step
frame_i = frame_i+1;
[isam results] = VisualISAMStep(frame_i, isam, data, results);
results
figure(1); clf;
VisualISAMPlot(results, data);
end

41
matlab/examples/VisualISAM_gui/VisualISAMInitialize.m
View File

@ -1,41 +0,0 @@
function [ isam, results ] = VisualISAMInitialize( data, reorderInterval )
%VISUALISAMINITIALIZE Initialize the first two poses and update ISAM
if (nargin<2), reorderInterval = 1; end
isam = visualSLAMISAM(reorderInterval);
%% Add new factors
newFactors = visualSLAMGraph;
newFactors.addPosePrior(symbol('x',1), data.cameras{1}.pose, data.posePriorNoise);
newFactors.addPointPrior(symbol('l',1), data.points{1}, data.pointPriorNoise);
odometry = data.cameras{1}.pose().between(data.cameras{2}.pose());
newFactors.addOdometry(symbol('x',1), symbol('x',2), odometry, data.odometryNoise);
for i=1:2
for j=1:size(data.points,2)
zij = data.cameras{i}.project(data.points{j});
newFactors.addMeasurement(zij, data.measurementNoise, symbol('x',i), symbol('l',j), data.K);
end
end
%% Initial estimats for new variables
initials = visualSLAMValues;
initials.insertPose(symbol('x',1), data.cameras{1}.pose);
initials.insertPose(symbol('x',2), data.cameras{2}.pose);
for j=1:size(data.points,2)
initials.insertPoint(symbol('l',j), data.points{j});
end
%% Update ISAM
isam.update(newFactors, initials);
results.frame_i = 2;
results.estimates = isam.estimate();
for i=1:2
results.Pposes{i} = isam.marginalCovariance(symbol('x',i));
end
for j=1:size(data.points,2)
results.Ppoints{j} = isam.marginalCovariance(symbol('l',j));
end
end

20
matlab/examples/VisualISAM_gui/VisualISAMPlot.m
View File

@ -1,20 +0,0 @@
function VisualISAMPlot( results, data )
%VISUALISAMPLOT Plot results of a step in visual ISAM
hold on;
for i=1:results.frame_i
pose_ii = results.estimates.pose(symbol('x',i));
plotPose3(pose_ii,results.Pposes{i},10);
end
for j=1:size(data.points,2)
point_j = results.estimates.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],results.Ppoints{j});
end
axis([-35 35 -35 35 -15 15])
hold off;
end

31
matlab/examples/VisualISAM_gui/VisualISAMStep.m
View File

@ -1,31 +0,0 @@
function [ isam, results ] = VisualISAMStep( frame_i, isam, data, prevResults )
%VISUALISAMSTEP Summary of this function goes here
% Detailed explanation goes here
%% Add new factors
newFactors = visualSLAMGraph;
frame_i
odometry = data.cameras{frame_i-1}.pose().between(data.cameras{frame_i}.pose());
newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, data.odometryNoise);
for j=1:size(data.points,2)
zij = data.cameras{frame_i}.project(data.points{j});
newFactors.addMeasurement(zij, data.measurementNoise, symbol('x',frame_i), symbol('l',j), data.K);
end
%% Initial estimates for new variables
initials = visualSLAMValues;
prevPose = prevResults.estimates.pose(symbol('x',frame_i-1));
initials.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
isam.update(newFactors, initials);
results.frame_i = frame_i;
results.estimates = isam.estimate();
for i=1:frame_i
results.Pposes{i} = isam.marginalCovariance(symbol('x',i));
end
for j=1:size(data.points,2)
results.Ppoints{j} = isam.marginalCovariance(symbol('l',j));
end
end

BIN
matlab/examples/VisualISAM_gui/VisualISAM_gui.fig
View File

Binary file not shown.

401
matlab/examples/VisualISAM_gui/VisualISAM_gui.m
View File

@ -1,401 +0,0 @@
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 03:28:25
% 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)
% handles.data = VisualISAMData_triangle();
% handles.frame_i = 3;
% handles.isam = visualSLAMISAM;
% handles.results = visualSLAMValues;
% Choose default command line output for VisualISAM_gui
handles.selectedDataset = 'triangle';
handles = initialize(handles);
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes VisualISAM_gui wait for user response (see UIRESUME)
% uiwait(handles.Dataset);
% --- 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;
% --- Executes on button press in generateButton.
function generateButton_Callback(hObject, eventdata, handles)
% hObject handle to generateButton (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
handles.data = VisualISAMData_triangle();
guidata(hObject,handles)
% --- Initialize a new dataset
function handles=initialize(handles)
handles.selectedDataset
switch handles.selectedDataset
case 'cube'
handles.data = VisualISAMData_cube();
case 'triangle'
handles.data = VisualISAMData_triangle();
end
handles.data
handles.results = {}
[handles.isam handles.results{2}] = VisualISAMInitialize(handles.data);
handles.frame_i=2;
sprintf('Frame 1,2:')
handles.results{2}.estimates
cla(handles.resultAxes);
VisualISAMPlot(handles.results{handles.frame_i}, handles.data)
view([36 34])
colormap('hot')
% --- 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<size(handles.data.cameras,2))
handles.frame_i = handles.frame_i+1;
sprintf('Frame %d:', handles.frame_i)
if (handles.frame_i > size(handles.results,2))
[handles.isam handles.results{handles.frame_i}] = ...
VisualISAMStep(handles.frame_i, handles.isam, ...
handles.data, handles.results{handles.frame_i-1});
end
handles.results{handles.frame_i}.estimates
cla(handles.resultAxes);
VisualISAMPlot(handles.results{handles.frame_i}, handles.data)
guidata(hObject,handles)
else
sprintf('Frame %d:', handles.frame_i)
sprintf('No more frame!')
end
function edit1_Callback(hObject, eventdata, handles)
% hObject handle to edit1 (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 edit1 as text
% str2double(get(hObject,'String')) returns contents of edit1 as a double
% --- Executes during object creation, after setting all properties.
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit1 (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
% --- 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');
handles.selectedDataset = str{sel}
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 during object creation, after setting all properties.
function Dataset_CreateFcn(hObject, eventdata, handles)
% hObject handle to Dataset (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% --- 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)
if (handles.frame_i>2)
handles.frame_i = handles.frame_i-1;
sprintf('Frame %d:', handles.frame_i)
handles.results{handles.frame_i}.estimates
cla(handles.resultAxes);
VisualISAMPlot(handles.results{handles.frame_i}, handles.data)
guidata(hObject,handles)
else
sprintf('No more frame!');
end
% --- 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)
for i=handles.frame_i+1:size(handles.data.cameras,2)
if (i > size(handles.results,2))
[handles.isam handles.results{i}] = ...
VisualISAMStep(i, handles.isam, ...
handles.data, handles.results{i-1});
end
handles.results{i}.estimates
cla(handles.resultAxes);
VisualISAMPlot(handles.results{i}, handles.data)
end
handles.frame_i = size(handles.data.cameras,2);
sprintf('Frame %d:', handles.frame_i)
guidata(hObject,handles)
% --- 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')
function edit8_Callback(hObject, eventdata, handles)
% hObject handle to edit8 (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 edit8 as text
% str2double(get(hObject,'String')) returns contents of edit8 as a double
% --- Executes during object creation, after setting all properties.
function edit8_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit8 (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 odoTrans_Callback(hObject, eventdata, handles)
% hObject handle to odoTrans (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 odoTrans as text
% str2double(get(hObject,'String')) returns contents of odoTrans as a double
% --- Executes during object creation, after setting all properties.
function odoTrans_CreateFcn(hObject, eventdata, handles)
% hObject handle to odoTrans (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 measNoise_Callback(hObject, eventdata, handles)
% hObject handle to measNoise (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 measNoise as text
% str2double(get(hObject,'String')) returns contents of measNoise as a double
% --- Executes during object creation, after setting all properties.
function measNoise_CreateFcn(hObject, eventdata, handles)
% hObject handle to measNoise (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 posePriorRot_Callback(hObject, eventdata, handles)
% hObject handle to posePriorRot (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 posePriorRot as text
% str2double(get(hObject,'String')) returns contents of posePriorRot as a double
% --- Executes during object creation, after setting all properties.
function posePriorRot_CreateFcn(hObject, eventdata, handles)
% hObject handle to posePriorRot (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 posePriorTrans_Callback(hObject, eventdata, handles)
% hObject handle to posePriorTrans (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 posePriorTrans as text
% str2double(get(hObject,'String')) returns contents of posePriorTrans as a double
% --- Executes during object creation, after setting all properties.
function posePriorTrans_CreateFcn(hObject, eventdata, handles)
% hObject handle to posePriorTrans (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 pointPrior_Callback(hObject, eventdata, handles)
% hObject handle to pointPrior (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 pointPrior as text
% str2double(get(hObject,'String')) returns contents of pointPrior as a double
% --- Executes during object creation, after setting all properties.
function pointPrior_CreateFcn(hObject, eventdata, handles)
% hObject handle to pointPrior (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 odoRot_Callback(hObject, eventdata, handles)
% hObject handle to odoRot (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 odoRot as text
% str2double(get(hObject,'String')) returns contents of odoRot as a double
% --- Executes during object creation, after setting all properties.
function odoRot_CreateFcn(hObject, eventdata, handles)
% hObject handle to odoRot (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

BIN
matlab/examples/VisualISAM_gui2/VisualISAM_gui.fig
View File

Binary file not shown.

235
matlab/examples/VisualISAM_gui2/VisualISAM_gui.m
View File

@ -1,235 +0,0 @@
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 14:00:55
% 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 = 4;
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.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.
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)
handles.frame_i = handles.frame_i+1;
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')
% --- 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

51
matlab/examples/VisualISAM_gui2/vData.m
View File

@ -1,51 +0,0 @@
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

31
matlab/examples/VisualISAM_gui2/vInit.m
View File

@ -1,31 +0,0 @@
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 = 1;
end

45
matlab/examples/VisualISAM_gui2/vPlot.m
View File

@ -1,45 +0,0 @@
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

52
matlab/examples/VisualISAM_gui2/vStep.m
View File

@ -1,52 +0,0 @@
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
vPlot(handles);
end
%% Reset newFactors and initialEstimates to prepare for the next update
handles.newFactors = visualSLAMGraph;
handles.initialEstimates = visualSLAMValues;
end