% % % clear all;close all;clc
% % %
% % %
% % % % mat_filename = '/vol/vssp/datasets/vid+depth01/S3A_XEF_02_2015/20150923_130335_00_headPoses.mat';
% % % mat_filename = 'MKrectangleKinect_headPose.mat';
% % % load(mat_filename);
% % % % allTimeStamp contains index of each frame for depth recordings
% % % % headPoses has the format of [time_stamp person_ID good_head x y z  Qr Qx
% % % % Qy Qz], of which are are interested in [time_stamp person_ID x y z]
% % %
% % %
% % % % first display ALL Kinect data detection
% % % % Indices of poses that are far enough form the kinect sensor:
% % % % dist2kinect = sqrt(sum(headPoses(:,4:6).^2,2));
% % % % badPoses = (dist2kinect<.1); % if the head is less than 10cm from the sensor, it's not good.
% % %
% % % peopleIDs = unique(headPoses(:,2));
% % % peopleIDs = peopleIDs(2:end);
% % % nPeople = length(peopleIDs);
% % % colours = [0,0,0; jet(nPeople-1)];
% % %
% % % % plot the useful detected point
% % % speakerID = 0:5;
% % % H = figure('Position',[800 500 600 400]);
% % % hold on
% % % % plot the spearker
% % % for p = 1:length(speakerID),
% % %     currPersonIdx = (headPoses(:,2)==speakerID(p));
% % %     if any(currPersonIdx)
% % %         Pos_currPersonIdx = headPoses(currPersonIdx,[4 5 6]);
% % %
% % %
% % %         hh = plot3(Pos_currPersonIdx(:,1), Pos_currPersonIdx(:,2), Pos_currPersonIdx(:,3), '*','color', colours(p,:));
% % %     end
% % % end
% % % axislim = [-2 2, -0.5 1.5, 0 5];
% % % % axislim = [-2 2, 0 1, 0 4];
% % % axis(axislim);grid minor
% % % xlabel('X','fontsize',14);ylabel('Y','fontsize',14);zlabel('Z','fontsize',14);
% % % % view([180,-90]);
% % % % view([-37.5,30]);
% % % view(-65,45);
% % %
% % % cameratoolbar('Show');
% % % cameratoolbar('SetMode', 'orbit');
% % % cameratoolbar('SetCoordSys','y');
% % % drawnow
% % %
% % %
% % % headPoses = headPoses(:,[4 5 6]);
% % %
% % % outlierIndex = headPoses(:,2)<0.1 | (headPoses(:,3)>4 & headPoses(:,2)<0.85);
% % %
% % % % 407 out of 12455
% % %
% % %
% % % load RectangleHeadPoses21Sept.mat
% % % % plot the useful detected point
% % % HH = figure('Position',[800 500 600 400]);
% % % headPoses2 = cell2mat(NewHeadPoses)';
% % % outlierIndex = headPoses2(:,2)<0.1 | (headPoses2(:,3)>4 & headPoses2(:,2)<0.85);
% % % % 24 out of 11243



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



clear all;close all;clc


mat_filename = '/vol/vssp/datasets/vid+depth01/S3A_XEF_02_2015/20150923_130335_00_headPoses.mat';
% mat_filename = 'MKrectangleKinect_headPose.mat';
load(mat_filename);
% allTimeStamp contains index of each frame for depth recordings
% headPoses has the format of [time_stamp person_ID good_head x y z  Qr Qx
% Qy Qz], of which are are interested in [time_stamp person_ID x y z]


% first display ALL Kinect data detection
% Indices of poses that are far enough form the kinect sensor:
% dist2kinect = sqrt(sum(headPoses(:,4:6).^2,2));
% badPoses = (dist2kinect<.1); % if the head is less than 10cm from the sensor, it's not good.

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

for k = 1:200%700:length(allTimeStamp)
    
    if ~exist('currentIndex','var')
        currentIndex = 1; % An auxilary parameter to synchronise the times stamp and the kinect detection
        tempTimeStamp = headPoses(currentIndex,1);
        currentTimeStamp = allTimeStamp(k);
        while tempTimeStamp~=currentTimeStamp,
            currentIndex = currentIndex+1;
            tempTimeStamp = headPoses(currentIndex,1);
        end
        axislim = [-1.5 1.5, 0 1, 0 4];
        
        HfigureResults = figure; axis(axislim); %axis equal;
        set(HfigureResults,'Position',[800 40 900 600]);grid on;
        xlabel('x (height, m)');ylabel('y (height, m)');zlabel('z (depth, m)');
        view([-37.5,30]);
        cameratoolbar('Show');
        cameratoolbar('SetMode', 'orbit');
        cameratoolbar('SetCoordSys','y');

    end
    
    Z = [];
    currentTimeStamp = allTimeStamp(k);
    tempTimeStamp = headPoses(currentIndex,1);
    while tempTimeStamp==currentTimeStamp,
        Z = [Z;headPoses(currentIndex,:)];
        currentIndex = currentIndex+1;
        tempTimeStamp = headPoses(currentIndex,1);
    end
    Z = Z(:,[4 5 6 2]); Z = Z'; % [x,y,z ,id]
    
    currentHeadpose = Z;
    try
        set(0, 'CurrentFigure', HfigureResults);
    catch
        figure(HfigureResults);
    end
    
    hold on;
    % if size(currentHeadpose,2)>0 % at least one estimated target
    for i = 1:size(currentHeadpose,2),
        id = currentHeadpose(4,i);
        if id>=0,
            plot3(currentHeadpose(1,i), currentHeadpose(2,i), currentHeadpose(3,i), '*','color', colours(id+1,:));
        end
    end
    
    pause(0.2)
    hold off;
    drawnow
end



%%%%%%%%%%%%%%%%%%%%%%%%

clear all;close all;clc


load RectangleHeadPoses24Sept.mat
% allTimeStamp contains index of each frame for depth recordings
% headPoses has the format of [time_stamp person_ID good_head x y z  Qr Qx
% Qy Qz], of which are are interested in [time_stamp person_ID x y z]


% first display ALL Kinect data detection
% Indices of poses that are far enough form the kinect sensor:
% dist2kinect = sqrt(sum(headPoses(:,4:6).^2,2));
% badPoses = (dist2kinect<.1); % if the head is less than 10cm from the sensor, it's not good.

NewHeadPoses 
colours = [0,0,0; jet(6)];
        lear all;close all;clc

for k = 750:length(NewHeadPoses)
    
    if ~exist('currentIndex','var')
        axislim = [-1.5 1.5, 0 1, 0 4];
        currentIndex = 1;
        
        HfigureResults = figure; axis(axislim); %axis equal;
        set(HfigureResults,'Position',[800 40 900 600]);grid on;
        xlabel('x (height, m)');ylabel('y (height, m)');zlabel('z (depth, m)');
        view([-37.5,30]);
        cameratoolbar('Show');
        cameratoolbar('SetMode', 'orbit');
        cameratoolbar('SetCoordSys','y');
        
    end
    
    currentHeadpose = NewHeadPoses{k};
    try
        set(0, 'CurrentFigure', HfigureResults);
    catch
        figure(HfigureResults);
    end
    
    hold on;
    % if size(currentHeadpose,2)>0 % at least one estimated target
    for i = 1:size(currentHeadpose,2),
        id = currentHeadpose(4,i);
        if id>=0,
            plot3(currentHeadpose(1,i), currentHeadpose(2,i), currentHeadpose(3,i), '*','color', colours(id+1,:));
        end
    end
    
    pause(0.2)
    hold off;
    drawnow
end



