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

18
matlab/VisualISAMInitialize.m
View File

@ -1,6 +1,6 @@
function [noiseModels,isam,result] = VisualInitialize(data,truth,options)
% 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
isam = visualSLAMISAM(options.reorderInterval);
@ -25,23 +25,23 @@ for i=1:2
initialEstimates.insertPose(ii,truth.cameras{i}.pose);
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
ii = symbol('x',i);
for j=1:size(data.z,2)
jj = symbol('l',j);
% 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
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
end
end
%% Update ISAM

3
matlab/VisualISAMPlot.m
View File

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