function plot_head_trajectories(headPoses)

peopleIDs = unique(headPoses(:,2));
peopleIDs = peopleIDs(2:end);
nPeople = length(peopleIDs);
colours = [0,0,0; jet(nPeople-1)];

dist2kinect = sqrt(sum(headPoses(:,4:6).^2,2));
badPoses = (dist2kinect<.5); % if the head is less than 10cm from the sensor, it's not good.

for p=1:nPeople,
    currPersonIdx = (headPoses(:,2)==peopleIDs(p));
    currPersonIdx(badPoses) = false;
    plot3(headPoses(currPersonIdx,4), headPoses(currPersonIdx,5), headPoses(currPersonIdx,6), 'color', colours(p,:));
    hold on
end
axis([-2 2, -1.2 1.2, 0 5])
xlabel('X');
ylabel('Y');
zlabel('Z');
view([180,-90]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');

grid on
hold off;

