|
function plotTrajectory(values,color)
|
|
% plotTrajectory: plot the poses in a values object
|
|
% assumes keys are 0 to N-1, where N is values.size()
|
|
X=[];Y=[];
|
|
for i=0:values.size()-1
|
|
pose_i = values.pose(i);
|
|
X=[X;pose_i.x];
|
|
Y=[Y;pose_i.y];
|
|
end
|
|
plot(X,Y,color);
|