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

Visual ISAM gui with global vars

release/4.3a0
Duy-Nguyen Ta 2012-06-08 20:08:53 +00:00
parent 533a7548dd
commit 9b71080625
8 changed files with 588 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
matlab/examples/VisualISAMGenerateData.m Normal file
View File

@ -0,0 +1,50 @@
VisualISAMGlobalVars
sprintf('vData...')
%% Generate simulated data
points = {};
if TRIANGLE % Create a triangle target, just 3 points on a plane
nPoints = 3;
r = 10;
for j=1:nPoints
theta = (j-1)*2*pi/nPoints;
points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
else % 3D landmarks as vertices of a cube
nPoints = 8;
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;
K = gtsamCal3_S2(500,500,0,640/2,480/2);
cameras = {};
for i=1:NCAMERAS
theta = (i-1)*2*pi/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(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,'*');
axis([1 640 1 480]);
end
end
end
odometry = cameras{1}.pose.between(cameras{2}.pose);
%% Set Noise parameters
poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);

7
matlab/examples/VisualISAMGlobalVars.m Normal file
View File

@ -0,0 +1,7 @@
global TRIANGLE NCAMERAS SHOW_IMAGES
global HARD_CONSTRAINT POINT_PRIORS BATCH_INIT REORDER_INTERVAL ALWAYS_RELINEARIZE
global SAVE_GRAPH PRINT_STATS DRAW_INTERVAL CAMERA_INTERVAL DRAW_TRUE_POSES
global SAVE_FIGURES SAVE_GRAPHS
global nPoints points K cameras odometry
global poseNoise pointNoise odometryNoise measurementNoise
global frame_i isam newFactors initialEstimates result

22
matlab/examples/VisualISAMInitOptions.m Normal file
View File

@ -0,0 +1,22 @@
VisualISAMGlobalVars
%% Data Options
TRIANGLE = true;
NCAMERAS = 20;
SHOW_IMAGES = false;
%% iSAM Options
HARD_CONSTRAINT = false;
POINT_PRIORS = false;
BATCH_INIT = true;
REORDER_INTERVAL=10;
ALWAYS_RELINEARIZE = false;
%% Display Options
SAVE_GRAPH = false;
PRINT_STATS = true;
DRAW_INTERVAL = 4;
CAMERA_INTERVAL = 1;
DRAW_TRUE_POSES = false;
SAVE_FIGURES = false;
SAVE_GRAPHS = false;

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

@ -0,0 +1,28 @@
VisualISAMGlobalVars
%% Initialize iSAM
isam = visualSLAMISAM(REORDER_INTERVAL);
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
i1 = symbol('x',1);
camera1 = cameras{1};
pose1 = camera1.pose;
if HARD_CONSTRAINT % add hard constraint
newFactors.addPoseConstraint(i1,pose1);
else
newFactors.addPosePrior(i1,pose1, poseNoise);
end
initialEstimates.insertPose(i1,pose1);
% Add visual measurement factors from first pose
for j=1:nPoints
jj = symbol('l',j);
if POINT_PRIORS % add point priors
newFactors.addPointPrior(jj, points{j}, pointNoise);
end
zij = camera1.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, i1, jj, K);
initialEstimates.insertPoint(jj, points{j});
end
frame_i = 1
cla;

42
matlab/examples/VisualISAMPlot.m Normal file
View File

@ -0,0 +1,42 @@
VisualISAMGlobalVars
if (frame_i<2)
sprintf('Cannot plot the first frame')
return
end
%% Plot results
tic
% h=figure(2);clf
% set(1,'NumberTitle','off','Name','Visual iSAM');
cla;
hold on;
sprintf('Computing marginals and plotting. Please wait...')
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:frame_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')
sprintf('Done!')
% figure(2);
t=toc;
if DRAW_INTERVAL~=NCAMERAS, plot(frame_i,t,'b.'); end
if SAVE_FIGURES
print(h,'-dpng',sprintf('VisualiSAM%03d.png',frame_i));
end
if SAVE_GRAPHS
isam.saveGraph(sprintf('VisualiSAM%03d.dot',frame_i));
end

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

@ -0,0 +1,46 @@
VisualISAMGlobalVars
%% Add odometry
newFactors.addOdometry(symbol('x',frame_i-1), symbol('x',frame_i), odometry, odometryNoise);
%% Add visual measurement factors
for j=1:nPoints
zij = cameras{frame_i}.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, symbol('x',frame_i), symbol('l',j), 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
if (frame_i==2), prevPose = cameras{1}.pose;
else, prevPose = result.pose(symbol('x',frame_i-1)); end
initialEstimates.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
%% Update ISAM
if BATCH_INIT & (frame_i==2) % Do a full optimize for first two poses
initialEstimates
fullyOptimized = newFactors.optimize(initialEstimates)
initialEstimates = fullyOptimized;
end
% figure(1);tic;
isam.update(newFactors, initialEstimates);
% t=toc; plot(frame_i,t,'r.'); tic
result = isam.estimate();
% t=toc; plot(frame_i,t,'g.');
if ALWAYS_RELINEARIZE % re-linearize
isam.reorder_relinearize();
end
if SAVE_GRAPH
isam.saveGraph(sprintf('VisualiSAM.dot',frame_i));
end
if PRINT_STATS
isam.printStats();
end
if mod(frame_i,DRAW_INTERVAL)==0
VisualISAMPlot
end
%% Reset newFactors and initialEstimates to prepare for the next update
newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;

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

Binary file not shown.

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

@ -0,0 +1,393 @@
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 16:03:16
% 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
VisualISAMGlobalVars
VisualISAMInitOptions
VisualISAMGenerateData
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 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)
sprintf('init button')
VisualISAMGlobalVars
DRAW_INTERVAL = str2num(get(handles.drawInterval,'String')) ;
NCAMERAS = str2num(get(handles.numCamEdit,'String')) ;
VisualISAMGenerateData
VisualISAMInitialize
% --- 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)
VisualISAMGlobalVars
if (frame_i<NCAMERAS)
frame_i = frame_i+1;
sprintf('Frame %d:', frame_i)
VisualISAMStep
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
global TRIANGLE
str = cellstr(get(hObject,'String'));
sel = get(hObject,'Value');
switch str{sel}
case 'triangle'
TRIANGLE = true
case 'cube'
TRIANGLE = false
end
VisualISAMGenerateData
VisualISAMInitialize
% --- 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)
VisualISAMPlot;
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
VisualISAMGlobalVars
DRAW_INTERVAL = str2num(get(hObject,'String')) ;
% --- 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
VisualISAMGlobalVars
NCAMERAS = str2num(get(hObject,'String')) ;
VisualISAMGenerateData
VisualISAMInitialize
% --- 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
function edit5_Callback(hObject, eventdata, handles)
% hObject handle to edit5 (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 edit5 as text
% str2double(get(hObject,'String')) returns contents of edit5 as a double
% --- Executes during object creation, after setting all properties.
function edit5_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (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 edit6_Callback(hObject, eventdata, handles)
% hObject handle to edit6 (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 edit6 as text
% str2double(get(hObject,'String')) returns contents of edit6 as a double
% --- Executes during object creation, after setting all properties.
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit6 (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 button press in hardConstraintCB.
function hardConstraintCB_Callback(hObject, eventdata, handles)
% hObject handle to hardConstraintCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of hardConstraintCB
VisualISAMGlobalVars
HARD_CONSTRAINT = get(hObject, 'Value')
% --- Executes on button press in pointPriorCB.
function pointPriorCB_Callback(hObject, eventdata, handles)
% hObject handle to pointPriorCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of pointPriorCB
% --- Executes on button press in batchInitCB.
function batchInitCB_Callback(hObject, eventdata, handles)
% hObject handle to batchInitCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of batchInitCB
% --- Executes on button press in alwaysRelinearizeCB.
function alwaysRelinearizeCB_Callback(hObject, eventdata, handles)
% hObject handle to alwaysRelinearizeCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of alwaysRelinearizeCB
function edit11_Callback(hObject, eventdata, handles)
% hObject handle to edit11 (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 edit11 as text
% str2double(get(hObject,'String')) returns contents of edit11 as a double
% --- Executes during object creation, after setting all properties.
function edit11_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit11 (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 cameraIntervalEdit_Callback(hObject, eventdata, handles)
% hObject handle to cameraIntervalEdit (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 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, eventdata, handles)
% hObject handle to cameraIntervalEdit (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 button press in saveGraphCB.
function saveGraphCB_Callback(hObject, eventdata, handles)
% hObject handle to saveGraphCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of saveGraphCB
% --- Executes on button press in printStatsCB.
function printStatsCB_Callback(hObject, eventdata, handles)
% hObject handle to printStatsCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of printStatsCB
% --- Executes on button press in drawTruePosesCB.
function drawTruePosesCB_Callback(hObject, eventdata, handles)
% hObject handle to drawTruePosesCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of drawTruePosesCB
% --- Executes on button press in saveFiguresCB.
function saveFiguresCB_Callback(hObject, eventdata, handles)
% hObject handle to saveFiguresCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of saveFiguresCB
% --- Executes on button press in saveGraphsCB.
function saveGraphsCB_Callback(hObject, eventdata, handles)
% hObject handle to saveGraphsCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of saveGraphsCB
% --- Executes on button press in showImagesCB.
function showImagesCB_Callback(hObject, eventdata, handles)
% hObject handle to showImagesCB (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hint: get(hObject,'Value') returns toggle state of showImagesCB