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

VisualISAM_gui2: add 2 options and take plot out of step

release/4.3a0
Duy-Nguyen Ta 2012-06-08 18:05:58 +00:00
parent 370be58adb
commit c2c27b08d8
5 changed files with 110 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

66
matlab/examples/VisualISAM_gui2/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 08-Jun-2012 12:35:14
% Last Modified by GUIDE v2.5 08-Jun-2012 14:00:55
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
@ -68,7 +68,7 @@ handles.ALWAYS_RELINEARIZE = false;
%% Display Options
handles.SAVE_GRAPH = false;
handles.PRINT_STATS = true;
handles.DRAW_INTERVAL = 1;
handles.DRAW_INTERVAL = 4;
handles.CAMERA_INTERVAL = 1;
handles.DRAW_TRUE_POSES = false;
handles.SAVE_FIGURES = false;
@ -107,7 +107,9 @@ 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)
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.
@ -116,6 +118,7 @@ function stepButton_Callback(hObject, eventdata, handles)
% 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)
@ -173,3 +176,60 @@ function stopButton_Callback(hObject, eventdata, handles)
% 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

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

@ -25,7 +25,7 @@ for j=1:handles.nPoints
handles.initialEstimates.insertPoint(jj, handles.points{j});
end
handles.frame_i = 2;
handles.frame_i = 1;
end

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

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

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

@ -42,44 +42,11 @@ function [ handles ] = vStep( handles )
handles.frame_i
handles.DRAW_INTERVAL
if mod(handles.frame_i,handles.DRAW_INTERVAL)==0
sprintf('Plotting')
%% 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;
vPlot(handles);
end
%% Reset newFactors and initialEstimates to prepare for the next update
handles.newFactors = visualSLAMGraph;
handles.initialEstimates = visualSLAMValues;
handles.frame_i = handles.frame_i+1;
end