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

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

release/4.3a0
Frank Dellaert 2012-06-10 22:32:24 +00:00
parent 94560fcc45
commit f90ae6feca
7 changed files with 63 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matlab/examples/VisualISAMExample.m
View File

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

8
matlab/examples/VisualISAMExample_triangle.m
View File

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

34
matlab/examples/VisualISAMGenerateData.m
View File

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

32
matlab/examples/VisualISAMInitialize.m
View File

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

4
matlab/examples/VisualISAMPlot.m
View File

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

10
matlab/examples/VisualISAMStep.m
View File

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

24
matlab/examples/VisualISAM_gui.m
View File

@ -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<size(data.cameras,2))
global frame_i truth data noiseModels isam result options
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
if (frame_i<size(data.cameras,2))
global frame_i truth data noiseModels isam result options
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
VisualISAMPlot(data, isam, result, options);
global options data truth isam result
VisualISAMPlot(truth, data, isam, result, options);
showWaiting(handles, '');