15 lines
451 B
Matlab
15 lines
451 B
Matlab
|
function plot3DTrajectory(values,style,frames)
|
||
|
|
% plot3DTrajectory
|
||
|
|
if nargin<3,frames=false;end
|
||
|
|
|
||
|
|
plot3(values.xs(),values.ys(),values.zs(),style); hold on
|
||
|
|
if frames
|
||
|
|
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');
|
||
|
|
end
|
||
|
|
end
|