clear, close all, clc
%% Step 1: Detect a Face
% First, you must detect the face. Use the 
% vision.CascadeObjectDetector System object to detect the 
% location of a face in a video frame. The cascade object 
% detector uses the Viola-Jones detection algorithm and a 
% trained classification model for detection. By default, the 
% detector is configured to detect faces, but it can be used to 
% detect other types of objects.

% Create a cascade detector object.
faceDetector = vision.CascadeObjectDetector();

% Read a video frame and run the face detector.
%videoFileReader = vision.VideoFileReader('tilted_face.avi');
videoFileReader = vision.VideoFileReader('visionface.avi');
%videoFileReader = vision.VideoFileReader('varios_rostros_2.mp4');
videoFrame      = step(videoFileReader);
bbox            = step(faceDetector, videoFrame);

% Convert the first box to a polygon.
% This is needed to be able to visualize the rotation of the 
% object.
x = bbox(1, 1); y = bbox(1, 2); w = bbox(1, 3); h = bbox(1, 4);
bboxPolygon = [x, y, x+w, y, x+w, y+h, x, y+h];

% Draw the returned bounding box around the detected face.
videoFrame = insertShape(videoFrame, 'Polygon', bboxPolygon);

figure; imshow(videoFrame); title('Detected face');

%% Step 2: Identify Facial Features To Track
% To track the face over time, this example uses the 
% Kanade-Lucas-Tomasi (KLT) algorithm. While it is possible to 
% use the cascade object detector on every frame, it is 
% computationally expensive. It may also fail to detect the face, 
% when the subject turns or tilts his head. This limitation 
% comes from the type of trained classification model used for 
% detection. The example detects the face only once, and then the 
% KLT algorithm tracks the face across the video frames.
% The KLT algorithm tracks a set of feature points across the 
% video frames. Once the detection locates the face, the next 
% step in the example identifies feature points that can be 
% reliably tracked. This example uses the standard, "good 
% features to track" proposed by Shi and Tomasi.
%
% Detect feature points in the face region.
points = detectMinEigenFeatures(rgb2gray(videoFrame),...
    'ROI', bbox);

% Display the detected points.
figure, imshow(videoFrame), hold on, title('Detected features');
plot(points);

%% Step 3: Track the face
% Initialize a Tracker to Track the Points
%
% With the feature points identified, you can now use the 
% vision.PointTracker System object to track them. For each 
% point in the previous frame, the point tracker attempts to 
% find the corresponding point in the current frame. Then the 
% estimateGeometricTransform function is used to estimate the 
% translation, rotation, and scale between the old points and 
% the new points. This transformation is applied to the bounding 
% box around the face.

% Create a point tracker and enable the bidirectional error 
% constraint to make it more robust in the presence of noise 
% and clutter.
pointTracker = vision.PointTracker('MaxBidirectionalError', 2);

% Initialize the tracker with the initial point locations and 
% the initial video frame.
points = points.Location;
initialize(pointTracker, points, videoFrame);

% Initialize a Video Player to Display the Results

% Create a video player object for displaying video frames.

videoPlayer  = vision.VideoPlayer('Position',...
    [0 0 [size(videoFrame, 2), size(videoFrame, 1)]+200]);
% Track the Face
% Track the points from frame to frame, and use 
% estimateGeometricTransform function to estimate the motion 
% of the face.

% Make a copy of the points to be used for computing the 
% geometric transformation between the points in the previous 
% and the current frames
oldPoints = points;

while ~isDone(videoFileReader)
    % get the next frame
    videoFrame = step(videoFileReader);

    % Track the points. Note that some points may be lost.
    [points, isFound] = step(pointTracker, videoFrame);
    visiblePoints = points(isFound, :);
    oldInliers = oldPoints(isFound, :);

    if size(visiblePoints, 1) >= 2 % need at least 2 points

        % Estimate the geometric transformation between the old 
        % points and the new points and eliminate outliers
        [xform, oldInliers, visiblePoints] = ...
            estimateGeometricTransform(...
            oldInliers, visiblePoints, 'similarity',...
            'MaxDistance', 4);

        % Apply the transformation to the bounding box
        [bboxPolygon(1:2:end), bboxPolygon(2:2:end)] ...
            = transformPointsForward(xform, ...
            bboxPolygon(1:2:end), bboxPolygon(2:2:end));

        % Insert a bounding box around the object being tracked
        videoFrame = insertShape(videoFrame, 'Polygon',...
            bboxPolygon);

        % Display tracked points
        videoFrame = insertMarker(videoFrame, visiblePoints,...
            '+','Color', 'white');

        % Reset the points
        oldPoints = visiblePoints;
        setPoints(pointTracker, oldPoints);
    end

    % Display the annotated video frame using the video player 
    % object
    step(videoPlayer, videoFrame);
end

% Clean up
release(videoFileReader);
release(videoPlayer);
release(pointTracker);