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 = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n ')); 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 %% Plot Covariance Ellipses
hold on figure(1);clf;
for i=1:3 plot(result.xs(),result.ys(),'k*-'); hold on
pose_i = result.pose(i); marginals = graph.marginals(result);
covarianceEllipse([pose_i.x;pose_i.y],P{i},'g') for i=1:result.size()
pose_i = result.pose(i);
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'g',P{i})
end 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 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate); result = graph.optimize(initialEstimate);
result.print(sprintf('\nFinal result:\n')); 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

34
examples/matlab/covarianceEllipse.m
View File

@ -2,7 +2,7 @@ function covarianceEllipse(x,P,color)
% covarianceEllipse: plot a Gaussian as an uncertainty ellipse % covarianceEllipse: plot a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366 % Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability % k=2.296 corresponds to 1 std, 68.26% of all probability
% k=11.82 corresponds to 3 std, 99.74% of all probability % k=11.82 corresponds to 3 std, 99.74% of all probability
% %
% covarianceEllipse(x,P,color) % covarianceEllipse(x,P,color)
% it is assumed x and y are the first two components of state x % it is assumed x and y are the first two components of state x
@ -14,21 +14,21 @@ k = 2.296;
[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
line(ex,ey,'color',color); line(ex,ey,'color',color);
function [x,y] = ellipse(a,b,c); function [x,y] = ellipse(a,b,c);
% ellipse: return the x and y coordinates for an ellipse % ellipse: return the x and y coordinates for an ellipse
% [x,y] = ellipse(a,b,c); % [x,y] = ellipse(a,b,c);
% a, and b are the axes. c is the center % a, and b are the axes. c is the center
global ellipse_x ellipse_y global ellipse_x ellipse_y
if ~exist('elipse_x') if ~exist('elipse_x')
q =0:2*pi/25:2*pi; q =0:2*pi/25:2*pi;
ellipse_x = cos(q); ellipse_x = cos(q);
ellipse_y = sin(q); ellipse_y = sin(q);
end end
points = a*ellipse_x + b*ellipse_y; points = a*ellipse_x + b*ellipse_y;
x = c(1) + points(1,:); x = c(1) + points(1,:);
y = c(2) + points(2,:); y = c(2) + points(2,:);
end end
end end