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

VisualISAMgui with figure for 2 datasets.

release/4.3a0
Duy-Nguyen Ta 2012-06-08 08:32:07 +00:00
parent 050cab36d4
commit cafac0b7a1
6 changed files with 318 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
matlab/examples/VisualISAMData_cube.m Normal file
View File

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

10
matlab/examples/VisualISAMInitialize.m
View File

@ -28,6 +28,14 @@ function [ isam, results ] = VisualISAMInitialize( data, reorderInterval )
%% Update ISAM
isam.update(newFactors, initials);
results = isam.estimate();
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

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

@ -0,0 +1,22 @@
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 -35 35])
view([36 34])
colormap('hot')
hold off;
end

13
matlab/examples/VisualISAMStep.m
View File

@ -14,11 +14,18 @@ function [ isam, results ] = VisualISAMStep( frame_i, isam, data, prevResults )
%% Initial estimates for new variables
initials = visualSLAMValues;
prevPose = prevResults.pose(symbol('x',frame_i-1));
prevPose = prevResults.estimates.pose(symbol('x',frame_i-1));
initials.insertPose(symbol('x',frame_i), prevPose.compose(odometry));
isam.update(newFactors, initials);
results = isam.estimate();
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.fig
View File

Binary file not shown.

264
matlab/examples/VisualISAM_gui.m
View File

@ -22,7 +22,7 @@ function varargout = VisualISAM_gui(varargin)
% Edit the above text to modify the response to help VisualISAM_gui
% Last Modified by GUIDE v2.5 07-Jun-2012 23:43:22
% Last Modified by GUIDE v2.5 08-Jun-2012 03:28:25
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
@ -58,13 +58,15 @@ function VisualISAM_gui_OpeningFcn(hObject, eventdata, handles, varargin)
% 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.figure1);
% uiwait(handles.Dataset);
% --- Outputs from this function are returned to the command line.
@ -86,16 +88,30 @@ function generateButton_Callback(hObject, eventdata, handles)
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)
% --- 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
handles=initialize(handles)
guidata(hObject,handles)
@ -104,26 +120,24 @@ 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
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
% --- 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
@ -154,7 +168,11 @@ function popupmenu1_Callback(hObject, eventdata, handles)
% 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)
@ -170,7 +188,211 @@ end
% --- Executes during object creation, after setting all properties.
function figure1_CreateFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
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