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

Changed the data interface for the VisualISAM demos

release/4.3a0
Alex Cunningham 2012-06-13 14:58:15 +00:00
parent e21696b473
commit 1cbc827512
5 changed files with 32 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
matlab/VisualISAMInitialize.m
View File

@ -1,4 +1,4 @@
function [noiseModels,isam,result] = VisualInitialize(data,truth,options) function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options)
% VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
% Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
@ -17,7 +17,7 @@ newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues; initialEstimates = visualSLAMValues;
for i=1:2 for i=1:2
ii = symbol('x',i); ii = symbol('x',i);
if i==1 & options.hardConstraint % add hard constraint if i==1 && options.hardConstraint % add hard constraint
newFactors.addPoseConstraint(ii,truth.cameras{1}.pose); newFactors.addPoseConstraint(ii,truth.cameras{1}.pose);
else else
newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose); newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose);
@ -28,11 +28,10 @@ end
%% Add visual measurement factors from two first poses and initialize observed landmarks %% Add visual measurement factors from two first poses and initialize observed landmarks
for i=1:2 for i=1:2
ii = symbol('x',i); ii = symbol('x',i);
for j=1:size(data.z,2) for k=1:length(data.Z{i})
jj = symbol('l',j); j = data.J{i}{k};
% Must check whether a landmark was actually observed jj = symbol('l',data.J{i}{k});
if ~isempty(data.z{i,j}) newFactors.addMeasurement(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K);
newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K);
% TODO: initial estimates should not be from ground truth! % TODO: initial estimates should not be from ground truth!
if ~initialEstimates.exists(jj) if ~initialEstimates.exists(jj)
initialEstimates.insertPoint(jj, truth.points{j}); initialEstimates.insertPoint(jj, truth.points{j});
@ -41,7 +40,6 @@ for i=1:2
newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point); newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
end end
end end
end
end end
%% Update ISAM %% Update ISAM

16
matlab/VisualISAMStep.m
View File

@ -1,4 +1,4 @@
function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,options); function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, options)
% VisualISAMStep: execute one update step of visualSLAM::iSAM object % VisualISAMStep: execute one update step of visualSLAM::iSAM object
% Authors: Duy Nguyen Ta and Frank Dellaert % Authors: Duy Nguyen Ta and Frank Dellaert
@ -12,9 +12,17 @@ i = result.nrPoses+1;
odometry = data.odometry{i-1}; odometry = data.odometry{i-1};
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
%% Add visual measurement factors %% Add visual measurement factors and initializations as necessary
for j=1:size(data.z,2) for k=1:length(data.Z{i})
newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, symbol('x',i), symbol('l',j), data.K); zij = data.Z{i}{k};
j = data.J{i}{k};
jj = symbol('l', j);
newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), jj, data.K);
% TODO: initialize with something other than truth
if ~result.exists(jj) && ~initialEstimates.exists(jj)
lmInit = truth.points{j};
initialEstimates.insertPoint(jj, lmInit);
end
end end
%% Initial estimates for the new pose. %% Initial estimates for the new pose.

7
matlab/examples/SFMExample.m
View File

@ -33,9 +33,10 @@ graph = visualSLAMGraph;
%% Add factors for all measurements %% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma); measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
for i=1:size(data.z,1) for i=1:length(data.Z)
for j=1:size(data.z,2) for k=1:length(data.Z{i})
graph.addMeasurement(data.z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), data.K); j = data.J{i}{k};
graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('l',j), data.K);
end end
end end

2
matlab/examples/VisualISAMExample.m
View File

@ -41,7 +41,7 @@ VisualISAMPlot(truth, data, isam, result, options)
%% Main loop for iSAM: stepping through all poses %% Main loop for iSAM: stepping through all poses
for frame_i=3:options.nrCameras for frame_i=3:options.nrCameras
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,options); [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth,options);
if mod(frame_i,options.drawInterval)==0 if mod(frame_i,options.drawInterval)==0
VisualISAMPlot(truth, data, isam, result, options) VisualISAMPlot(truth, data, isam, result, options)
end end

4
matlab/examples/VisualISAMGenerateData.m
View File

@ -32,7 +32,9 @@ for i=1:options.nrCameras
truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K); truth.cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
% Create measurements % Create measurements
for j=1:nrPoints for j=1:nrPoints
data.z{i,j} = truth.cameras{i}.project(truth.points{j}); % All landmarks seen in every frame
data.Z{i}{j} = truth.cameras{i}.project(truth.points{j});
data.J{i}{j} = j;
end end
end end