VisualISAMgui with figure for 2 datasets.

matlab/examples/VisualISAMData_cube.m

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

matlab/examples/VisualISAMInitialize.m

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

matlab/examples/VisualISAMPlot.m

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

matlab/examples/VisualISAMStep.m

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

matlab/examples/VisualISAM_gui.m

@@ -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=initialize(handles)
-    handles.frame_i=3;
-    sprintf('Frame 1,2:')
-    handles.results
     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)
+function Dataset_CreateFcn(hObject, eventdata, handles)
-% hObject    handle to figure1 (see GCBO)
+% 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