diff --git a/matlab/examples/VisualISAMExample.m b/matlab/examples/VisualISAMExample.m index eadca5d2f..2dfde60bb 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/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 diff --git a/matlab/examples/VisualISAMExample_triangle.m b/matlab/examples/VisualISAMExample_triangle.m index c9c416cb2..c5cdf288d 100644 --- a/matlab/examples/VisualISAMExample_triangle.m +++ b/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 diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/examples/VisualISAMGenerateData.m index adb0b9243..fab8185f9 100644 --- a/matlab/examples/VisualISAMGenerateData.m +++ b/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 - theta = (j-1)*2*pi/nPoints; - data.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); + for j=1:nrPoints + theta = (j-1)*2*pi/nrPoints; + truth.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]'); end else % 3D landmarks as vertices of a cube - nPoints = 8; - data.points = {gtsamPoint3([10 10 10]'),... + nrPoints = 8; + 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); -data.cameras = {}; +truth.K = gtsamCal3_S2(500,500,0,640/2,480/2); +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 - zij = data.cameras{i}.project(data.points{j}); + for j=1:nrPoints + 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 \ No newline at end of file diff --git a/matlab/examples/VisualISAMInitialize.m b/matlab/examples/VisualISAMInitialize.m index f79a30a93..13a4258c9 100644 --- a/matlab/examples/VisualISAMInitialize.m +++ b/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 - ii = symbol('x',frame_i); - if frame_i==1 & options.hardConstraint % add hard constraint - newFactors.addPoseConstraint(ii,data.cameras{1}.pose); +for i=1:2 + ii = symbol('x',i); + if i==1 & options.hardConstraint % add hard constraint + 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,data.cameras{frame_i}.pose); + initialEstimates.insertPose(ii,truth.cameras{i}.pose); end %% Add visual measurement factors from two first poses -for frame_i=1:2 - ii = symbol('x',frame_i); - for j=1:size(data.points,2) +for i=1:2 + ii = symbol('x',i); + for j=1:size(data.z,2) jj = symbol('l',j); - zij = data.cameras{frame_i}.project(data.points{j}); - newFactors.addMeasurement(zij, noiseModels.measurement, ii, jj, data.K); + newFactors.addMeasurement(data.z{i,j}, 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 diff --git a/matlab/examples/VisualISAMPlot.m b/matlab/examples/VisualISAMPlot.m index 42407ec82..8e8dce6f1 100644 --- a/matlab/examples/VisualISAMPlot.m +++ b/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 diff --git a/matlab/examples/VisualISAMStep.m b/matlab/examples/VisualISAMStep.m index ca951e5e5..491fd9bac 100644 --- a/matlab/examples/VisualISAMStep.m +++ b/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) - zij = data.cameras{i}.project(data.points{j}); - newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K); +for j=1:size(data.z,2) + newFactors.addMeasurement(data.z{i,j}, 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; diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/examples/VisualISAM_gui.m index 49b73ab65..97405a318 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/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); -VisualISAMPlot(data, isam, result, options) +[noiseModels,isam,result] = VisualISAMInitialize(data,truth,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 -while (frame_i