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

fix bug in VisualISAM matlab example

release/4.3a0
Duy-Nguyen Ta 2012-06-06 18:08:30 +00:00
parent 87a705468d
commit 31d88649f7
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
examples/matlab/VisualISAMExample.m
View File

@ -20,7 +20,7 @@ for j=1:nPoints
end end
%% Create camera poses on a circle around the triangle %% Create camera poses on a circle around the triangle
nCameras = 30; nCameras = 10;
height = 10; height = 10;
r = 30; r = 30;
poses = {}; poses = {};
@ -48,6 +48,9 @@ for i=1:nCameras
% Prior for the first pose or odometry for subsequent poses % Prior for the first pose or odometry for subsequent poses
if (i==1) if (i==1)
newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise); newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise);
% for j=1:nPoints
% newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
% end
else else
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, poseNoise); newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, poseNoise);
end end
@ -67,9 +70,10 @@ for i=1:nCameras
initialEstimates.insertPoint(symbol('l',j), points{j}); initialEstimates.insertPoint(symbol('l',j), points{j});
end end
else else
%TODO: this might not be suboptimal since "result" is not the fully %TODO: this might be suboptimal since "result" is not the fully
%optimized result %optimized result
prevPose = result.pose(symbol('x',i-1)); if (i==2), prevPose = poses{1};
else, prevPose = result.pose(symbol('x',i-1)); end
initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
end end
@ -82,7 +86,7 @@ for i=1:nCameras
result = isam.estimate(); result = isam.estimate();
% Plot first result % Plot first result
figure(1);clf h=figure(1);clf
hold on; hold on;
for j=1:size(points,2) for j=1:size(points,2)
P = isam.marginalCovariance(symbol('l',j)); P = isam.marginalCovariance(symbol('l',j));
@ -97,6 +101,8 @@ for i=1:nCameras
plotPose3(pose_ii,P,10); plotPose3(pose_ii,P,10);
end end
axis([-50 50 -50 50 -50 50]) axis([-50 50 -50 50 -50 50])
colormap('hot')
print(h,'-dpng',sprintf('VisualISAM_%03d.png',i));
% Reset newFactors and initialEstimates to prepare for the next % Reset newFactors and initialEstimates to prepare for the next
% update % update