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 on;
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


load RectangleHeadPoses21Sept.mat
% plot the useful detected point
HH = figure('Position',[800 500 600 400]);
NewHeadPoses = cell2mat(NewHeadPoses);
hold on
for p = 1:length(speakerID),
    currPersonIdx = (NewHeadPoses(4,:)==speakerID(p));
    if any(currPersonIdx)
        Pos_currPersonIdx = (NewHeadPoses([1 2 3],currPersonIdx))';
        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 on;
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



clear all;close all;clc


mat_filename = '/vol/vssp/datasets/vid+depth01/S3A_XEF_02_2015/20150923_130335_00_headPoses.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]
headPoses = headPoses(1:600,:);

peopleIDs = unique(headPoses(:,2));
peopleIDs = peopleIDs(2:end);
nPeople = length(peopleIDs);
colours = [0,0,0; jet(6-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]);
        dis = sum(abs(diff(Pos_currPersonIdx,1,1)),2);
        
        for j=1:length(dis),
            if dis(j)<0.3,
                plot3(Pos_currPersonIdx([j j+1],1), Pos_currPersonIdx([j j+1],2), Pos_currPersonIdx([j j+1],3) ,'color', colours(p,:),'linewidth',2)
            else
                if j<length(dis)
                    if dis(j+1)>=0.3
                        plot3(Pos_currPersonIdx(j+1,1), Pos_currPersonIdx(j+1,2), Pos_currPersonIdx(j+1,3), '*' ,'color', colours(p,:))
                    end
                end
            end
        end
        
        
        %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 = [-1.5 1.5, -0.5 1, 1 4];
axis(axislim);grid on;
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


load RectangleHeadPoses24Sept.mat
% plot the useful detected point
HH = figure('Position',[800 500 600 400]);
% NewHeadPoses = NewHeadPoses(1:430);
ttt = cell2mat(NewHeadPoses);
hold on;
for p = 1:length(speakerID),
    currPersonIdx = (ttt(4,:)==speakerID(p));
    if any(currPersonIdx)
        Pos_currPersonIdx = (ttt([1 2 3],currPersonIdx))';
        dis = sum(abs(diff(Pos_currPersonIdx,1,1)),2);
        for j=1:length(dis),
            if dis(j)<1,
                plot3(Pos_currPersonIdx([j j+1],1), Pos_currPersonIdx([j j+1],2), Pos_currPersonIdx([j j+1],3) ,'color', colours(p,:),'linewidth',2)
            else
                if j<length(dis)
                    if dis(j+1)>=1
                        plot3(Pos_currPersonIdx(j+1,1), Pos_currPersonIdx(j+1,2), Pos_currPersonIdx(j+1,3), '*' ,'color', colours(p,:))
                    end
                end
            end
        end
  
    end
end

%axislim = [-2 2, -0.5 1.5, 0 5];
axislim = [-1.5 1.5, -0.5 1, 1 4];
axis(axislim);grid on;
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


