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

small improvements to plotting coordinate frames

release/4.3a0
Chris Beall 2012-06-05 00:02:55 +00:00
parent f34ae951fc
commit 647b38c758
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/matlab/Pose3SLAMExample_circle.m
View File

@ -39,11 +39,11 @@ initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
figure(1);clf
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*');
%% optimize
result = fg.optimize(initial);
%% Show Result
hold on; plot3(result.xs(),result.ys(),result.zs(),'b-*')
hold on; plot3DTrajectory(result,'b-*', true, 0.3); axis equal;
result.print(sprintf('\nFinal result:\n'));

10
examples/matlab/plot3DTrajectory.m
View File

@ -1,15 +1,17 @@
function plot3DTrajectory(values,style,frames)
function plot3DTrajectory(values,style,frames,scale)
% plot3DTrajectory
if nargin<3,frames=false;end
if nargin<4,scale=0;end
plot3(values.xs(),values.ys(),values.zs(),style); hold on
if frames
N=values.size;
for i=0:N-1
pose = values.pose(i);
t = pose.translation;
R = pose.rotation.matrix;
quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),'r');
quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),'g');
quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),'b');
quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),scale,'r');
quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),scale,'g');
quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b');
end
end