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

Fixed logic error, added odometry

release/4.3a0
Frank Dellaert 2012-06-13 17:42:06 +00:00
parent 9d0c88c1dd
commit 87fe727838
1 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
matlab/VisualISAMInitialize.m
View File

@ -17,10 +17,12 @@ newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
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,truth.cameras{i}.pose, noiseModels.pose);
if i==1
if options.hardConstraint % add hard constraint
newFactors.addPoseConstraint(ii,truth.cameras{1}.pose);
else
newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose);
end
end
initialEstimates.insertPose(ii,truth.cameras{i}.pose);
end
@ -42,6 +44,9 @@ for i=1:2
end
end
%% Add odometry between frames 1 and 2
newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry);
%% Update ISAM
if options.batchInitialization % Do a full optimize for first two poses
fullyOptimized = newFactors.optimize(initialEstimates);