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

VisualiSAM, going crazy. MATLAB is amazing!

release/4.3a0
Frank Dellaert 2012-06-07 13:16:28 +00:00
parent ce71979c8c
commit 8644f81dc8
3 changed files with 225 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
examples/matlab/VisualISAMExample.m
View File

@ -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
figure(NCAMERAS+1);clf
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);
if SAVE_GRAPH
isam.saveGraph(sprintf('VisualiSAM.dot',i));
end
for ii=1: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);
if mod(i,DRAW_INTERVAL)==0
%% Plot results
tic
h=figure(2);clf
set(1,'NumberTitle','off','Name','Visual iSAM');
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;

BIN
examples/matlab/iSAMgui.fig Normal file
View File

Binary file not shown.

171
examples/matlab/iSAMgui.m Normal file
View File

@ -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)