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

View File

@ -20,7 +20,7 @@ for j=1:nPoints
end
%% Create camera poses on a circle around the triangle
nCameras = 30;
nCameras = 10;
height = 10;
r = 30;
poses = {};
@ -48,6 +48,9 @@ for i=1:nCameras
% Prior for the first pose or odometry for subsequent poses
if (i==1)
newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise);
% for j=1:nPoints
% newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
% end
else
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, poseNoise);
end
@ -67,9 +70,10 @@ for i=1:nCameras
initialEstimates.insertPoint(symbol('l',j), points{j});
end
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
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));
end
@ -82,7 +86,7 @@ for i=1:nCameras
result = isam.estimate();
% Plot first result
figure(1);clf
h=figure(1);clf
hold on;
for j=1:size(points,2)
P = isam.marginalCovariance(symbol('l',j));
@ -97,6 +101,8 @@ for i=1:nCameras
plotPose3(pose_ii,P,10);
end
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
% update