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

29
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
pose_i = result.pose(i);
covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
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);
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

28
examples/matlab/covarianceEllipse.m
View File

@ -14,21 +14,21 @@ k = 2.296;
[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
line(ex,ey,'color',color);
function [x,y] = ellipse(a,b,c);
% ellipse: return the x and y coordinates for an ellipse
% [x,y] = ellipse(a,b,c);
% a, and b are the axes. c is the center
function [x,y] = ellipse(a,b,c);
% ellipse: return the x and y coordinates for an ellipse
% [x,y] = ellipse(a,b,c);
% a, and b are the axes. c is the center
global ellipse_x ellipse_y
if ~exist('elipse_x')
q =0:2*pi/25:2*pi;
ellipse_x = cos(q);
ellipse_y = sin(q);
end
global ellipse_x ellipse_y
if ~exist('elipse_x')
q =0:2*pi/25:2*pi;
ellipse_x = cos(q);
ellipse_y = sin(q);
end
points = a*ellipse_x + b*ellipse_y;
x = c(1) + points(1,:);
y = c(2) + points(2,:);
end
points = a*ellipse_x + b*ellipse_y;
x = c(1) + points(1,:);
y = c(2) + points(2,:);
end
end