Made generateData create ground truth and measurements, and now step only uses the latter.

matlab/examples/VisualISAMExample.m

@@ -32,17 +32,17 @@ options.saveFigures = false;
 options.saveDotFiles = false;
 
 %% Generate data
-data = VisualISAMGenerateData(options);
+[data,truth] = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-[noiseModels,isam,result] = VisualISAMInitialize(data,options);
+[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
 figure(1);
-VisualISAMPlot(data, isam, result, options)
+VisualISAMPlot(truth, data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
     [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
-        VisualISAMPlot(data, isam, result, options)
+        VisualISAMPlot(truth, data, isam, result, options)
     end
 end

matlab/examples/VisualISAMExample_triangle.m

@@ -32,17 +32,17 @@ options.saveFigures = false;
 options.saveDotFiles = false;
 
 %% Generate data
-data = VisualISAMGenerateData(options);
+[data,truth] = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-[noiseModels,isam,result] = VisualISAMInitialize(data,options);
+[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
 figure(1);
-VisualISAMPlot(data, isam, result, options)
+VisualISAMPlot(truth, data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
     [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
-        VisualISAMPlot(data, isam, result, options)
+        VisualISAMPlot(truth, data, isam, result, options)
     end
 end

matlab/examples/VisualISAMGenerateData.m

@@ -1,19 +1,18 @@
-function data = VisualISAMGenerateData(options)
+function [data,truth] = VisualISAMGenerateData(options)
 % VisualISAMGenerateData: create data for viusalSLAM::iSAM examples
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 %% Generate simulated data
-data.points = {};
 if options.triangle % Create a triangle target, just 3 points on a plane
-    nPoints = 3;
+    nrPoints = 3;
     r = 10;
-    for j=1:nPoints
+    for j=1:nrPoints
-        theta = (j-1)*2*pi/nPoints;
+        theta = (j-1)*2*pi/nrPoints;
-        data.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
+        truth.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
     end
 else % 3D landmarks as vertices of a cube
-    nPoints = 8;
+    nrPoints = 8;
-    data.points = {gtsamPoint3([10 10 10]'),...
+    truth.points = {gtsamPoint3([10 10 10]'),...
         gtsamPoint3([-10 10 10]'),...
         gtsamPoint3([-10 -10 10]'),...
         gtsamPoint3([10 -10 10]'),...
@@ -25,12 +24,16 @@ end
 
 %% Create camera cameras on a circle around the triangle
 height = 10; r = 40;
-data.K = gtsamCal3_S2(500,500,0,640/2,480/2);
+truth.K = gtsamCal3_S2(500,500,0,640/2,480/2);
-data.cameras = {};
+data.K = truth.K;
 for i=1:options.nrCameras
     theta = (i-1)*2*pi/options.nrCameras;
     t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
-    data.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), data.K);
+    truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
+    % Create measurements
+    for j=1:nrPoints
+        data.z{i,j} = truth.cameras{i}.project(truth.points{j});
+    end    
 end
 
 %% show images if asked
@@ -39,8 +42,8 @@ if options.showImages
     for i=1:options.nrCameras
         figure(2+i);clf;hold on
         set(2+i,'NumberTitle','off','Name',sprintf('Camera %d',i));
-        for j=1:nPoints
+        for j=1:nrPoints
-            zij = data.cameras{i}.project(data.points{j});
+            zij = truth.cameras{i}.project(truth.points{j});
             plot(zij.x,zij.y,'*');
             axis([1 640 1 480]);
         end
@@ -49,4 +52,7 @@ if options.showImages
 end
 
 %% Calculate odometry between cameras
-data.odometry = data.cameras{1}.pose.between(data.cameras{2}.pose);
+odometry = truth.cameras{1}.pose.between(truth.cameras{2}.pose);
+for i=1:options.nrCameras-1
+    data.odometry{i}=odometry;
+end

matlab/examples/VisualISAMInitialize.m

@@ -1,4 +1,4 @@
-function [noiseModels,isam,result] = VisualInitialize(data,options)
+function [noiseModels,isam,result] = VisualInitialize(data,truth,options)
 % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
@@ -12,36 +12,36 @@ noiseModels.point = gtsamSharedNoiseModel_Sigma(3, 0.1);
 noiseModels.measurement = gtsamSharedNoiseModel_Sigma(2, 1.0);
 
 %% Add constraints/priors
+% TODO: should not be from ground truth!
 newFactors = visualSLAMGraph;
 initialEstimates = visualSLAMValues;
-for frame_i=1:2
+for i=1:2
-    ii = symbol('x',frame_i);
+    ii = symbol('x',i);
-    if frame_i==1 & options.hardConstraint % add hard constraint
+    if i==1 & options.hardConstraint % add hard constraint
-        newFactors.addPoseConstraint(ii,data.cameras{1}.pose);
+        newFactors.addPoseConstraint(ii,truth.cameras{1}.pose);
     else
-        newFactors.addPosePrior(ii,data.cameras{frame_i}.pose, noiseModels.pose);
+        newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose);
     end
-    % TODO: init should not be from ground truth!
+    initialEstimates.insertPose(ii,truth.cameras{i}.pose);
-    initialEstimates.insertPose(ii,data.cameras{frame_i}.pose);
 end
 
 %% Add visual measurement factors from two first poses
-for frame_i=1:2
+for i=1:2
-    ii = symbol('x',frame_i);
+    ii = symbol('x',i);
-    for j=1:size(data.points,2)
+    for j=1:size(data.z,2)
         jj = symbol('l',j);
-        zij = data.cameras{frame_i}.project(data.points{j});
+        newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K);
-        newFactors.addMeasurement(zij, noiseModels.measurement, ii, jj, data.K);
     end
 end
 
 %% Initialize points, possibly add priors on them
-for j=1:size(data.points,2)
+% TODO: should not be from ground truth!
+for j=1:size(data.z,2)
     jj = symbol('l',j);
     if options.pointPriors % add point priors
-        newFactors.addPointPrior(jj, data.points{j}, noiseModels.point);
+        newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
     end
-    initialEstimates.insertPoint(jj, data.points{j});  % TODO: should not be from ground truth!
+    initialEstimates.insertPoint(jj, truth.points{j});
 end
 
 %% Update ISAM

matlab/examples/VisualISAMPlot.m

@@ -1,4 +1,4 @@
-function VisualISAMPlot(data, isam, result, options)
+function VisualISAMPlot(truth, data, isam, result, options)
 % VisualISAMPlot: plot current state of visualSLAM::iSAM object
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
@@ -29,7 +29,7 @@ for i=1:options.cameraInterval:M
         plotPose3(pose_i,P,10);
     end
     if options.drawTruePoses % show ground truth
-        plotPose3(data.cameras{i}.pose,[],10);
+        plotPose3(truth.cameras{i}.pose,[],10);
     end
 end
 

matlab/examples/VisualISAMStep.m

@@ -9,17 +9,17 @@ initialEstimates = visualSLAMValues;
 
 %% Add odometry
 i = double(result.nrPoses)+1;
-newFactors.addOdometry(symbol('x',i-1), symbol('x',i), data.odometry, noiseModels.odometry);
+odometry = data.odometry{i-1};
+newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
 
 %% Add visual measurement factors
-for j=1:size(data.points,2)
+for j=1:size(data.z,2)
-    zij = data.cameras{i}.project(data.points{j});
+    newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K);
-    newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K);
 end
 
 %% Initial estimates for the new pose.
 prevPose = result.pose(symbol('x',i-1));
-initialEstimates.insertPose(symbol('x',i), prevPose.compose(data.odometry));
+initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
 
 %% Update ISAM
 % figure(1);tic;

matlab/examples/VisualISAM_gui.m

@@ -224,31 +224,31 @@ function saveGraphsCB_Callback(hObject, ~, handles)
 % --- Executes on button press in intializeButton.
 function intializeButton_Callback(hObject, ~, handles)
 
-global frame_i data noiseModels isam result options
+global frame_i truth data noiseModels isam result options
 
 % initialize global options
 initOptions(handles)
 
 % Generate Data
-data = VisualISAMGenerateData(options);
+[data,truth] = VisualISAMGenerateData(options);
 
 % Initialize and plot
-[noiseModels,isam,result] = VisualISAMInitialize(data,options);
+[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
-VisualISAMPlot(data, isam, result, options)
+VisualISAMPlot(truth, data, isam, result, options)
 frame_i = 2;
 showFramei(hObject, handles)
 
 
 % --- Executes on button press in runButton.
 function runButton_Callback(hObject, ~, handles)
-global frame_i data noiseModels isam result options
+global frame_i truth data noiseModels isam result options
-while (frame_i<size(data.cameras,2))
+while (frame_i<size(truth.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     if mod(frame_i,options.drawInterval)==0
         showWaiting(handles, 'Computing marginals...');
-        VisualISAMPlot(data, isam, result, options)
+        VisualISAMPlot(truth, data, isam, result, options)
         showWaiting(handles, '');
     end
 end
@@ -256,21 +256,21 @@ end
 
 % --- Executes on button press in stepButton.
 function stepButton_Callback(hObject, ~, handles)
-global frame_i data noiseModels isam result options
+global frame_i truth data noiseModels isam result options
-if (frame_i<size(data.cameras,2))
+if (frame_i<size(truth.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
     [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options);
     showWaiting(handles, 'Computing marginals...');
-    VisualISAMPlot(data, isam, result, options)
+    VisualISAMPlot(truth, data, isam, result, options)
     showWaiting(handles, '');
 end
 
 % --- Executes on button press in plotButton.
 function plotButton_Callback(hObject, ~, handles)
 showWaiting(handles, 'Computing marginals...');
-global options data isam result
+global options data truth isam result
-VisualISAMPlot(data, isam, result, options);
+VisualISAMPlot(truth, data, isam, result, options);
 showWaiting(handles, '');