clear all;close all;clc


% mat_filename = '/vol/vssp/datasets/vid+depth01/S3A_XEF_02_2015/20150923_130335_00_headPoses.mat'; axislim = [-2 2, 0 1, 0 4];
mat_filename = 'MKrectangleKinect_headPose.mat'; axislim = [-2 2, -0.5 1.5, 0 5];
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]);
        
        %         temp = diff(Pos_currPersonIdx);
        %         dis = sqrt(sum(temp.^2,2));
        %         dis(dis>0.15) = [];
        %         std(dis)
        %         figure;hist(dis,100);
        %         figure(H)
        %         hold on
        
        hh = plot3(Pos_currPersonIdx(:,1), Pos_currPersonIdx(:,2), Pos_currPersonIdx(:,3), '*','color', colours(p,:));
    end
end

axis(axislim);grid on;
xlabel('X');ylabel('Y');zlabel('Z');
% view([180,-90]);
view([-37.5,30]);
cameratoolbar('Show');
cameratoolbar('SetMode', 'orbit');
cameratoolbar('SetCoordSys','y');
drawnow

% test the proposed algorithm
NewHeadPoses = IDcorrection(headPoses,allTimeStamp,axislim);

disp(':)')


