align_faces.py

import numpy as np
import cv2
from cv2 import CAP_PROP_FPS, CAP_PROP_FRAME_COUNT
import pandas as pd
import scipy
import scipy.io
import math
from scipy.stats import mode


def rotate(ox, oy, px, py, angle):

    qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
    qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
    return qx, qy


input_tsv = "/home/peternagy/Downloads/world_videos/pair50M_BG3_proc/world.csv"
dataFrame_world = pd.read_csv(input_tsv)
frameIndices_world = dataFrame_world['frame']
numberOfFrames_world = len(frameIndices_world)
xCoordinates_world = np.zeros([numberOfFrames_world, 68])
yCoordinates_world = np.zeros([numberOfFrames_world, 68])
for i in range(68):
    stringToTest = 'x_' + str(i)
    xCoordinates_world[:, i] = dataFrame_world[stringToTest].to_numpy()
    stringToTest = 'y_' + str(i)
    yCoordinates_world[:, i] = dataFrame_world[stringToTest].to_numpy()

input_tsv = "/home/peternagy/Downloads/world_videos/pair50M_BG3_webcam_proc/pair50_Mordor_BG3.csv"
dataFrame_webcam = pd.read_csv(input_tsv)
frameIndices_webcam = dataFrame_webcam['frame']
numberOfFrames_webcam = len(frameIndices_webcam)
xCoordinates_webcam = np.zeros([numberOfFrames_webcam, 68])
yCoordinates_webcam = np.zeros([numberOfFrames_webcam, 68])
for i in range(68):
    stringToTest = 'x_' + str(i)
    xCoordinates_webcam[:, i] = dataFrame_webcam[stringToTest].to_numpy()
    stringToTest = 'y_' + str(i)
    yCoordinates_webcam[:, i] = dataFrame_webcam[stringToTest].to_numpy()

pathToVideo = "/home/peternagy/Downloads/world_videos/pair50M_BG3_proc/world.mp4"
video = cv2.VideoCapture(pathToVideo)
numberOfVideoFrames = video.get(cv2.CAP_PROP_FRAME_COUNT)

# gaze_file = os.path.join(input_dir, "gaze_data_world.tsv")
gaze_file = "/media/peternagy/work_external/50M_BG3/tsv/gaze_data_world.tsv"
gazeTable = pd.read_csv(gaze_file, sep='\t')
timeStampGaze = gazeTable.timestamp.to_numpy()
frameIdxGaze = gazeTable.frame_idx.to_numpy()

videoTimes = scipy.io.loadmat("/media/peternagy/work_external/50M_BG3/pair50_Mordor_BG3_times.mat")
flipTimeStamps = videoTimes["flipTimeStamps"]
flipTimeStamps = flipTimeStamps[:, 0]
flipTimeStamps = flipTimeStamps[~np.isnan(flipTimeStamps)]

ok, frame = video.read()
hsv = np.zeros_like(frame)
hsv[..., 1] = 255
previousAngle = np.zeros([720, 1280])
flipTimeStamps = np.asarray(flipTimeStamps)
frameIndex = 0
while True:
    ok, frame = video.read()
    if ok == 1:
        # for i in range(68):
        #     centrePoint = (int(xCoordinates[frameIndex, i]), int(yCoordinates[frameIndex, i]))
        #     cv2.circle(frame, centrePoint, 5, (255, 0, 0), 2)
        # centrePoint = (int(xCoordinates[frameIndex, 36]), int(yCoordinates[frameIndex, 36]))
        # cv2.circle(frame, centrePoint, 5, (255, 0, 0), 2)
        # centrePoint = (int(xCoordinates[frameIndex, 45]), int(yCoordinates[frameIndex, 45]))
        # cv2.circle(frame, centrePoint, 5, (255, 0, 0), 2)

        indexOfGazeFrame = np.where(frameIdxGaze == frameIndex)
        indexOfGazeFrame = indexOfGazeFrame[0]
        sizeOfIndex = indexOfGazeFrame.size
        if sizeOfIndex > 1:
            indexOfGazeFrame = indexOfGazeFrame[0]
        if sizeOfIndex > 0:
            if indexOfGazeFrame > 0:
                timeStampGazeUnderTest = timeStampGaze[indexOfGazeFrame]
                if timeStampGazeUnderTest > flipTimeStamps[0]:
                    idxWebcamClosestToGazeTimeStamp = (np.abs(flipTimeStamps - timeStampGazeUnderTest)).argmin()
                    flipTimeStampUnderTest = flipTimeStamps[idxWebcamClosestToGazeTimeStamp]
                    xCoordsEye1_webcam = np.zeros([6, 1])
                    yCoordsEye1_webcam = np.zeros([6, 1])
                    for i in range(36, 42):
                        xCoordsEye1_webcam[i-36] = xCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, i]
                        yCoordsEye1_webcam[i-36] = yCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, i]
                    xCoordsEye2_webcam = np.zeros([6, 1])
                    yCoordsEye2_webcam = np.zeros([6, 1])
                    for i in range(42, 48):
                        xCoordsEye2_webcam[i-42] = xCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, i]
                        yCoordsEye2_webcam[i-42] = yCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, i]

                    xCoordsEye1_world = np.zeros([6, 1])
                    yCoordsEye1_world = np.zeros([6, 1])
                    for i in range(36, 42):
                        xCoordsEye1_world[i-36] = xCoordinates_world[frameIndex, i]
                        yCoordsEye1_world[i-36] = yCoordinates_world[frameIndex, i]
                        # cv2.circle(frame, (int(xCoordsEye1_world[i-36]), int(yCoordsEye1_world[i-36])), 5, (255, 0, 0), 2)
                    xCoordsEye2_world = np.zeros([6, 1])
                    yCoordsEye2_world = np.zeros([6, 1])
                    for i in range(42, 48):
                        xCoordsEye2_world[i-42] = xCoordinates_world[frameIndex, i]
                        yCoordsEye2_world[i-42] = yCoordinates_world[frameIndex, i]
                        # cv2.circle(frame, (int(xCoordsEye2_world[i-42]), int(yCoordsEye2_world[i-42])), 5, (255, 0, 0), 2)

                    xCordEye1Middle_webcam = np.mean(xCoordsEye1_webcam)
                    yCordEye1Middle_webcam = np.mean(yCoordsEye1_webcam)
                    xCordEye2Middle_webcam = np.mean(xCoordsEye2_webcam)
                    yCordEye2Middle_webcam = np.mean(yCoordsEye2_webcam)
                    xCordEye1Middle_world = np.mean(xCoordsEye1_world)
                    yCordEye1Middle_world = np.mean(yCoordsEye1_world)
                    xCordEye2Middle_world = np.mean(xCoordsEye2_world)
                    yCordEye2Middle_world = np.mean(yCoordsEye2_world)
                    # xCordEye1Middle_webcam = xCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, 48]
                    # yCordEye1Middle_webcam = yCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, 48]
                    # xCordEye2Middle_webcam = xCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, 54]
                    # yCordEye2Middle_webcam = yCoordinates_webcam[idxWebcamClosestToGazeTimeStamp, 54]
                    # xCordEye1Middle_world = xCoordinates_world[frameIndex, 48]
                    # yCordEye1Middle_world = yCoordinates_world[frameIndex, 48]
                    # xCordEye2Middle_world = xCoordinates_world[frameIndex, 54]
                    # yCordEye2Middle_world = yCoordinates_world[frameIndex, 54]

                    # cv2.circle(frame, (int(xCordEye1Middle_world), int(yCordEye1Middle_world)), 5, (255, 0, 0), 2)
                    # cv2.circle(frame, (int(xCordEye2Middle_world), int(yCordEye2Middle_world)), 5, (255, 0, 0), 2)

                    eyeDistance_webcam = np.sqrt(np.power(xCordEye1Middle_webcam - xCordEye2Middle_webcam, 2) +
                                                  np.power(yCordEye1Middle_webcam - yCordEye2Middle_webcam, 2))
                    eyeDistance_world = np.sqrt(np.power(xCordEye1Middle_world - xCordEye2Middle_world, 2) +
                                                  np.power(yCordEye1Middle_world - yCordEye2Middle_world, 2))
                    scaleFactor = eyeDistance_world / eyeDistance_webcam
                    if np.isnan(scaleFactor) or np.isinf(scaleFactor):
                        scaleFactor = 1

                    xCordEye1Middle_webcam_transformed = xCordEye1Middle_webcam * scaleFactor
                    yCordEye1Middle_webcam_transformed = yCordEye1Middle_webcam * scaleFactor
                    xCordEye2Middle_webcam_transformed = xCordEye2Middle_webcam * scaleFactor
                    yCordEye2Middle_webcam_transformed = yCordEye2Middle_webcam * scaleFactor

                    screen1x_transformed = 0
                    screen1y_transformed = 0
                    screen2x_transformed = 0
                    screen2y_transformed = 720 * scaleFactor
                    screen3x_transformed = 1280 * scaleFactor
                    screen3y_transformed = 0
                    screen4x_transformed = 1280 * scaleFactor
                    screen4y_transformed = 720 * scaleFactor

                    slope_webcam = ((yCordEye1Middle_webcam_transformed - yCordEye2Middle_webcam_transformed) /
                                    (xCordEye1Middle_webcam_transformed - xCordEye2Middle_webcam_transformed))
                    if np.isnan(slope_webcam) or np.isinf(slope_webcam):
                        slope_webcam = 0
                    angle_webcam = np.arctan(slope_webcam)

                    slope_world = ((yCordEye1Middle_world - yCordEye2Middle_world) /
                                   (xCordEye1Middle_world - xCordEye2Middle_world))
                    if np.isnan(slope_world) or np.isinf(slope_world):
                        slope_world = 0
                    angle_world = np.arctan(slope_world)

                    translationX = xCordEye1Middle_world - xCordEye1Middle_webcam_transformed
                    translationY = yCordEye1Middle_world - yCordEye1Middle_webcam_transformed
                    xCordEye1Middle_webcam_transformed = xCordEye1Middle_webcam_transformed + translationX
                    yCordEye1Middle_webcam_transformed = yCordEye1Middle_webcam_transformed + translationY
                    xCordEye2Middle_webcam_transformed = xCordEye2Middle_webcam_transformed + translationX
                    yCordEye2Middle_webcam_transformed = yCordEye2Middle_webcam_transformed + translationY
                    screen1x_transformed = screen1x_transformed + translationX
                    screen1y_transformed = screen1y_transformed + translationY
                    screen2x_transformed = screen2x_transformed + translationX
                    screen2y_transformed = screen2y_transformed + translationY
                    screen3x_transformed = screen3x_transformed + translationX
                    screen3y_transformed = screen3y_transformed + translationY
                    screen4x_transformed = screen4x_transformed + translationX
                    screen4y_transformed = screen4y_transformed + translationY

                    # prvs_gray = cv2.cvtColor(previousFrame.copy(), cv2.COLOR_BGR2GRAY)
                    # next_gray = cv2.cvtColor(frame.copy(), cv2.COLOR_BGR2GRAY)
                    # flow = cv2.calcOpticalFlowFarneback(prvs_gray, next_gray, None, 0.5, 3, 15, 3, 5, 1.2, 0)
                    # magnitude, angle = cv2.cartToPolar(flow[..., 0], flow[..., 1])
                    # hsv[..., 0] = angle * 180 / np.pi / 2
                    # hsv[..., 2] = cv2.normalize(magnitude, None, 0, 255, cv2.NORM_MINMAX)
                    # bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
                    # cv2.imshow('frame2', hsv[..., 2])
                    # angleDiff = angle - previousAngle
                    # previousAngle = angle
                    # mean_angle1 = np.mean(angleDiff)
                    # mean_angle12 = np.mean(mean_angle1)
                    # angleUnderTest = np.degrees(mean_angle12)
                    # step = 8
                    # anglesList = []
                    # translationList = []
                    # h, w = prvs_gray.shape[:2]
                    # y, x = np.mgrid[step / 2:h:step, step / 2:w:step].reshape(2, -1).astype(int)
                    # fx, fy = flow[y, x].T
                    # lines = np.vstack([x, y, x + fx, y + fy]).T.reshape(-1, 2, 2)
                    # lines = np.int32(lines + 0.5)
                    # for (x1, y1), (x2, y2) in lines:
                    #     angle = math.atan2(- int(y2) + int(y1), int(x2) - int(x1)) * 180.0 / np.pi
                    #     length = math.hypot(int(x2) - int(x1), - int(y2) + int(y1))
                    #     translationList.append(length)
                    #     anglesList.append(angle)
                    # angles = np.array(anglesList)
                    # translation = np.array(translationList)
                    # nonzero = np.where(translation > 0)
                    # angles = angles[nonzero]
                    # ang_mode = mode(angles)[0][0]

                    angleOfRotation = angle_world - angle_webcam
                    xCordEye1Middle_webcam_transformed_r, yCordEye1Middle_webcam_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world,
                                                                                                        xCordEye1Middle_webcam_transformed, yCordEye1Middle_webcam_transformed, angleOfRotation)
                    xCordEye2Middle_webcam_transformed_r, yCordEye2Middle_webcam_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world,
                                                                                                        xCordEye2Middle_webcam_transformed, yCordEye2Middle_webcam_transformed, angleOfRotation)
                    screen1x_transformed_r, screen1y_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world, screen1x_transformed, screen1y_transformed, angleOfRotation)
                    screen2x_transformed_r, screen2y_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world, screen2x_transformed, screen2y_transformed, angleOfRotation)
                    screen3x_transformed_r, screen3y_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world, screen3x_transformed, screen3y_transformed, angleOfRotation)
                    screen4x_transformed_r, screen4y_transformed_r = rotate(xCordEye1Middle_world, yCordEye1Middle_world, screen4x_transformed, screen4y_transformed, angleOfRotation)

                    selectedFrame1 = frame
                    if selectedFrame1.shape[0] > 1 and selectedFrame1.shape[1] > 1:
                        selectedFrame1_gray = cv2.cvtColor(selectedFrame1.copy(), cv2.COLOR_BGR2GRAY)
                        ret, thresh = cv2.threshold(selectedFrame1_gray, 200, 255, cv2.THRESH_BINARY)
                        contours, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
                        # cv2.drawContours(image=selectedFrame1, contours=contours, contourIdx=-1, color=(0, 255, 0), thickness=2, lineType=cv2.LINE_AA)
                        selectedContourPoints = []
                        for i, contour in enumerate(contours):
                            for j, contour_point in enumerate(contour):
                                if contour_point[0][1] > yCordEye1Middle_world:
                                    selectedContourPoints.append(contour_point)

                        if xCordEye1Middle_world > 10 and xCordEye2Middle_world > 10:
                            leftExtremum = 0
                            rightExtremum = 0
                            for ind, point in enumerate(selectedContourPoints):
                                if point[0][0] < xCordEye1Middle_world:
                                    distUnderTest = np.sqrt(np.power(xCordEye1Middle_world - point[0][0], 2) +
                                                      np.power(yCordEye1Middle_world - point[0][1], 2))
                                    if distUnderTest > leftExtremum:
                                        selectedLeftPoint = point
                                        leftExtremum = distUnderTest
                                if point[0][0] > xCordEye2Middle_world:
                                    distUnderTest = np.sqrt(np.power(xCordEye2Middle_world - point[0][0], 2) +
                                                      np.power(yCordEye2Middle_world - point[0][1], 2))
                                    if distUnderTest > rightExtremum:
                                        selectedRightPoint = point
                                        rightExtremum = distUnderTest
                            cv2.circle(frame, ((selectedLeftPoint[0][0], selectedLeftPoint[0][1])), 5, (0, 255, 0), 2, cv2.LINE_AA)
                            cv2.circle(frame, ((selectedRightPoint[0][0], selectedRightPoint[0][1])), 5, (0, 255, 0), 2, cv2.LINE_AA)


                        # edges1 = cv2.Canny(selectedFrame1_gray, 100, 200)
                        # cv2.imshow('GrayImage1', edges1)
                    # selectedFrame2 = frame[int(screen2y_transformed_r-50) : int(screen2y_transformed_r + 50), int(screen2x_transformed_r - 50) : int(screen2x_transformed_r + 50), :]
                    # if selectedFrame2.shape[0] > 1 and selectedFrame2.shape[1] > 1:
                    #     selectedFrame2_gray = cv2.cvtColor(selectedFrame2.copy(), cv2.COLOR_BGR2GRAY)
                    #     edges2 = cv2.Canny(selectedFrame2_gray, 100, 200)
                    #     cv2.imshow('GrayImage2', edges2)
                    # selectedFrame3 = frame[int(screen3y_transformed_r-50) : int(screen3y_transformed_r + 50), int(screen3x_transformed_r - 50) : int(screen3x_transformed_r + 50), :]
                    # if selectedFrame3.shape[0] > 1 and selectedFrame3.shape[1] > 1:
                    #     selectedFrame3_gray = cv2.cvtColor(selectedFrame3.copy(), cv2.COLOR_BGR2GRAY)
                    #     edges3 = cv2.Canny(selectedFrame3_gray, 100, 200)
                    #     cv2.imshow('GrayImage3', edges3)
                    # selectedFrame4 = frame[int(screen4y_transformed_r-50) : int(screen4y_transformed_r + 50), int(screen4x_transformed_r - 50) : int(screen4x_transformed_r + 50), :]
                    # if selectedFrame4.shape[0] > 1 and selectedFrame4.shape[1] > 1:
                    #     selectedFrame4_gray = cv2.cvtColor(selectedFrame4.copy(), cv2.COLOR_BGR2GRAY)
                    #     edges4 = cv2.Canny(selectedFrame4_gray, 100, 200)
                    #     cv2.imshow('GrayImage4', edges4)

                    cv2.circle(frame, (int(xCordEye1Middle_webcam_transformed), int(yCordEye1Middle_webcam_transformed)), 5, (0, 0, 255), 2)
                    cv2.circle(frame, (int(xCordEye2Middle_webcam_transformed), int(yCordEye2Middle_webcam_transformed)), 5, (0, 0, 255), 2)
                    cv2.circle(frame, (int(screen1x_transformed_r), int(screen1y_transformed_r)), 5, (0, 0, 255), 2)
                    cv2.circle(frame, (int(screen2x_transformed_r), int(screen2y_transformed_r)), 5, (0, 0, 255), 2)
                    cv2.circle(frame, (int(screen3x_transformed_r), int(screen3y_transformed_r)), 5, (0, 0, 255), 2)
                    cv2.circle(frame, (int(screen4x_transformed_r), int(screen4y_transformed_r)), 5, (0, 0, 255), 2)
                    cv2.putText(frame, "Rotation Angle : " + str(int(np.degrees(angleOfRotation))), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
                    cv2.putText(frame, "Frame Index : " + str(int(frameIndex)), (100, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)


        cv2.imshow('Image', frame)
        cv2.waitKey(1)
    previousFrame = frame
    frameIndex = frameIndex + 1
#
# print('Number of acceptable frames:', frameIndex)

# acceptableFrameCounter = 0
# for frameIndex in range(5000, 14000):
#     video.set(1, int(frameIndex - 1))
#     ok, frame = video.read()
#
#     if ok == 1:
#         acceptableFrameCounter = acceptableFrameCounter + 1
#         for i in range(68):
#             centrePoint = (int(xCoordinates[frameIndex, i]), int(yCoordinates[frameIndex, i]))
#             cv2.circle(frame, centrePoint, 5, (255, 0, 0), 2)
#
#         cv2.imshow('MultiTracker', frame)
#         cv2.waitKey(10)

print('Number of acceptable frames:', acceptableFrameCounter)