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

early prototype for visualISAM_gui. No drawing yet.

release/4.3a0
Duy-Nguyen Ta 2012-06-08 03:54:27 +00:00
parent 70974bd55e
commit 2ebd7a8974
6 changed files with 281 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
matlab/examples/VisualISAMData_triangle.m Normal file
View File

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

16
matlab/examples/VisualISAMExample_triangle2.m Normal file
View File

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

33
matlab/examples/VisualISAMInitialize.m Normal file
View File

@ -0,0 +1,33 @@
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 = isam.estimate();
end

24
matlab/examples/VisualISAMStep.m Normal file
View File

@ -0,0 +1,24 @@
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.pose(symbol('x',frame_i-1));
initials.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
isam.update(newFactors, initials);
results = isam.estimate();
end

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

Binary file not shown.

176
matlab/examples/VisualISAM_gui.m Normal file
View File

@ -0,0 +1,176 @@
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 07-Jun-2012 23:43:22
% 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.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;
% --- 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)
% --- 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.isam handles.results] = VisualISAMInitialize(handles.data);
handles.frame_i=3;
sprintf('Frame 1,2:')
handles.results
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)
handles.frame_i
handles.results
if (handles.frame_i<size(handles.data.cameras,2))
[handles.isam handles.results] = VisualISAMStep(handles.frame_i, handles.isam, handles.data, handles.results);
handles.frame_i = handles.frame_i+1;
handles.results
guidata(hObject,handles)
else
sprintf('No more frame!')
end
% --- Executes on button press in clearButton.
function clearButton_Callback(hObject, eventdata, handles)
% hObject handle to clearButton (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
clear;
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
% --- 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 figure1_CreateFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called