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

Added checking in VisualISAM to only add measurements/priors/initializations for observed landmarks

release/4.3a0
Alex Cunningham 2012-06-13 02:31:32 +00:00
parent cee80c1938
commit 5b47ab512c
2 changed files with 20 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
matlab/VisualISAMInitialize.m
View File

@ -1,6 +1,6 @@
function [noiseModels,isam,result] = VisualInitialize(data,truth,options) function [noiseModels,isam,result] = VisualInitialize(data,truth,options)
% VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters
% Authors: Duy Nguyen Ta and Frank Dellaert % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
%% Initialize iSAM %% Initialize iSAM
isam = visualSLAMISAM(options.reorderInterval); isam = visualSLAMISAM(options.reorderInterval);
@ -25,25 +25,25 @@ for i=1:2
initialEstimates.insertPose(ii,truth.cameras{i}.pose); initialEstimates.insertPose(ii,truth.cameras{i}.pose);
end end
%% Add visual measurement factors from two first poses %% 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 j=1:size(data.z,2)
jj = symbol('l',j); jj = symbol('l',j);
newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K); % Must check whether a landmark was actually observed
if ~isempty(data.z{i,j})
newFactors.addMeasurement(data.z{i,j}, noiseModels.measurement, ii, jj, data.K);
% TODO: initial estimates should not be from ground truth!
if ~initialEstimates.exists(jj)
initialEstimates.insertPoint(jj, truth.points{j});
end
if options.pointPriors % add point priors
newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point);
end
end
end end
end end
%% Initialize points, possibly add priors on them
% 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, truth.points{j}, noiseModels.point);
end
initialEstimates.insertPoint(jj, truth.points{j});
end
%% Update ISAM %% Update ISAM
if options.batchInitialization % Do a full optimize for first two poses if options.batchInitialization % Do a full optimize for first two poses
fullyOptimized = newFactors.optimize(initialEstimates); fullyOptimized = newFactors.optimize(initialEstimates);

11
matlab/VisualISAMPlot.m
View File

@ -11,11 +11,14 @@ hold on;
%% Plot points %% Plot points
for j=1:N for j=1:N
%% TODO: use the actual set of keys present
jj = symbol('l',j); jj = symbol('l',j);
point_j = result.point(jj); if result.exists(jj)
plot3(point_j.x, point_j.y, point_j.z,'marker','o'); point_j = result.point(jj);
P = isam.marginalCovariance(jj); plot3(point_j.x, point_j.y, point_j.z,'marker','o');
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); P = isam.marginalCovariance(jj);
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end
end end
%% Plot cameras %% Plot cameras