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

Plotting

release/4.3a0
Frank Dellaert 2012-06-05 03:51:21 +00:00
parent 9a8e083697
commit 9560997bc7
3 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
examples/matlab/LocalizationExample.m
View File

@ -43,26 +43,13 @@ initialEstimate.print(sprintf('\nInitial estimate:\n '));
result = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n '));
%% Query the marginals
marginals = graph.marginals(result);
P{1}=marginals.marginalCovariance(1);
P{2}=marginals.marginalCovariance(2);
P{3}=marginals.marginalCovariance(3);
%% Plot Trajectory
figure(1)
clf
X=[];Y=[];
for i=1:3
pose_i = result.pose(i);
X=[X;pose_i.x];
Y=[Y;pose_i.y];
end
plot(X,Y,'b*-');
%% Plot Covariance Ellipses
hold on
for i=1:3
figure(1);clf;
plot(result.xs(),result.ys(),'k*-'); hold on
marginals = graph.marginals(result);
for i=1:result.size()
pose_i = result.pose(i);
covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'g',P{i})
end
axis equal

13
examples/matlab/Pose2SLAMExample.m
View File

@ -54,3 +54,16 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
figure(1);clf;
plot(result.xs(),result.ys(),'k*-'); hold on
plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-');
marginals = graph.marginals(result);
for i=1:result.size()
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);
fprintf(1,'%.5f %.5f %.5f\n',P{i})
plotPose2(pose_i,'g',P{i})
end
axis equal