%% Tracking the closest player using Kinect(R) for Windows(R) from Image Acquisition Toolbox(TM)
% This example shows how to obtain the data available from the Kinect for
% Windows sensor using Image Acquisition Toolbox and to use it to track the
% player closest to the sensor.

% Copyright 2012-2013 The MathWorks, Inc.

%% Introduction
% The Kinect(R) for Windows(R) sensor has the capability to track a specified 
% player in its field of view. This is achieved by setting the
% TrackingID of that particular player. One of the applications of this
% feature is to track the player closest to the sensor.
%
% This example shows the steps to track the player closest to the Kinect
% for Windows sensor. We will visualize this by over-laying the skeleton of
% this player on the corresponding color image.
%
% To achieve this we will need to retrieve the following data from the Kinect Sensor:
%
% 1. Color Frame to visualize the overlayed skeleton.
%
% 2. Position Coodinates of the detected skeletons to determine the closest
% player.
%
% 3. Skeleton Tracking IDs for each detected player to set the ID to be
% tracked.
%
% 4. Skeleton Joint data to draw the skeleton.
%
% We can then find the closest player from the position coordinates of the
% detected skeletons. To track the closest player, the SkeletonsToTrack
% property is set to the TrackingID of the closest player.
%
% The Color frame can be retrieved from the Color device. The remaining Skeletal information
% is retrieved from the metadata attached to the depth frame.

%% Add Utility Function to the MATLAB Path
% In order the keep this example as simple as possible, some utility functions 
% for working with the Kinect for Windows metadata have been created.  These 
% functions are not on the MATLAB path by default.  In order to use them, 
% the directory containing the utility functions must be added to the MATLAB path.
% This path includes the skeletalViewer function which accepts the skeleton data, 
% color image and number of skeletons as inputs and displays the skeleton 
% overlaid on the color image 
utilpath = fullfile(matlabroot, 'toolbox', 'imaq', 'imaqdemos', ...
    'html', 'KinectForWindows');
addpath(utilpath);

%% Create Objects
% In order to retrieve the color images, we will need to create an object for 
% the color device.

% Create the VIDEOINPUT object for the color device
colorVid = videoinput('kinect', 1);

%%
% The skeletal data can be retrieved from the metadata on the depth frame.
% So, create an object for the depth device.

% Create the VIDEOINPUT object for the depth device
depthVid = videoinput('kinect', 2);

%% Configure acquisition parameters
% Set the FramesPerTrigger property to '1', so that, we can acquire 1 frame 
% for live-processing on each trigger.

set([colorVid depthVid], 'FramesPerTrigger', 1);

%%
% Set the TriggerRepeat to the number of frames we want to process.
% We choose to set this to 450 in order to get 15 seconds of data (at 30fps).

triggerRepeat = 450;
set([colorVid depthVid], 'TriggerRepeat', triggerRepeat);

%%
% Set the triggering mode to 'manual', so that we can request frames as
% we need them.

triggerconfig([colorVid depthVid],'manual');

%% Configure tracking parameters
%Configure the tracking properties to track skeletons in standing posture.

src = getselectedsource(depthVid);
src.DepthMode = 'Default';
src.TrackingMode = 'Skeleton';
src.BodyPosture = 'Standing';

%% Start the device
% Start the color and depth devices. This begins acquisition, but does not
% start logging of acquired data because the Trigger mode is set to
% 'manual'.

start([colorVid depthVid]);

%%
% The following variable caches the Tracking ID of the closest player in 
% order to detect a change in the closest player. This is initialized to 0
% since no player is tracked yet.
closestTrackingID_prev = 0;
%% Processing Loop
% The fragment below is a Processing loop to find the closest player and 
% draw its skeleton on the corresponding color frame.
for i = 1:triggerRepeat+1

%% 
% First, we trigger the devices to acquire 1 frame.
    trigger([colorVid depthVid]);

%% 
% Retrieve the required data from the color and depth device objects.
    framesColor = getdata(colorVid);
    [~, ~, metaDataDepth] = getdata(depthVid);
%%      
% Find the index of the skeleton closest to the sensor based on the 'z' 
% coordinate (3rd index) in the positionWorldCoordinates metadata.
    
    % Retieve the skeletonDistance (Z coordinate) for each skeleton tracked. 
    skeletonDistance = metaDataDepth.PositionWorldCoordinates(3,metaDataDepth.IsPositionTracked);
    % Get the index of the closest player
    [~, closestPlayerIndex] = min (skeletonDistance);
%%
% Retrieve the TrackingID of the Skeleton closest to the Kinect device.
    closestTrackingID = metaDataDepth.SkeletonTrackingID(closestPlayerIndex);
%%    
% If the closestTrackingID is non-zero and not the same as the cached value,
% set the SkeletonsToTrack property to the closest player's TrackingID.
% Cache this new tracking ID in closestTrackingID_prev.
    if((~isempty(closestTrackingID))&&(closestTrackingID~=0)&&(closestTrackingID~=closestTrackingID_prev))
        src.SkeletonsToTrack = closestTrackingID;
        closestTrackingID_prev = closestTrackingID;
    end
%%
% Calculate the number of Skeletons tracked.
    nSkeletonTracked = sum(metaDataDepth.IsSkeletonTracked);
%%    
% Use the utility function util_skeletonViewer to draw the tracked 
% skeletons over a color image.  
    util_skeletonViewer(metaDataDepth.JointImageIndices(:,:,metaDataDepth.IsSkeletonTracked),framesColor, nSkeletonTracked);
   
%%   
% If all the players leave the view and return, their TrackingIDs change.
% Since the SkeletonsToTrack is set to track a specific TrackingID,
% tracking will fail. To take care of this, if no skeleton is currently 
% tracked, switch the tracking to Default.
    if nSkeletonTracked == 0 
        src.SkeletonsToTrack = [];
    end
end

%% Clean up
% Stop acquisition and delete the videoinput objects.
stop([colorVid depthVid]);
delete([colorVid depthVid]);