<reponame>Ozanaydinn/Ozanaydinn.github.io
import cv2
import numpy as np
import math
from model_init import face_model, landmark_model
import face_detect
import face_landmark_detection


def get_2d_points(rotation_vector, translation_vector, camera_matrix, val):
    """Return the 3D points present as 2D for making annotation box"""
    point_3d = []
    dist_coeffs = np.zeros((4,1))
    rear_size = val[0]
    rear_depth = val[1]
    point_3d.append((-rear_size, -rear_size, rear_depth))
    point_3d.append((-rear_size, rear_size, rear_depth))
    point_3d.append((rear_size, rear_size, rear_depth))
    point_3d.append((rear_size, -rear_size, rear_depth))
    point_3d.append((-rear_size, -rear_size, rear_depth))
    
    front_size = val[2]
    front_depth = val[3]
    point_3d.append((-front_size, -front_size, front_depth))
    point_3d.append((-front_size, front_size, front_depth))
    point_3d.append((front_size, front_size, front_depth))
    point_3d.append((front_size, -front_size, front_depth))
    point_3d.append((-front_size, -front_size, front_depth))
    point_3d = np.array(point_3d, dtype=np.float).reshape(-1, 3)
    
    # Map to 2d img points
    (point_2d, _) = cv2.projectPoints(point_3d,
                                    rotation_vector,
                                    translation_vector,
                                    camera_matrix,
                                    dist_coeffs)
    point_2d = np.int32(point_2d.reshape(-1, 2))
    return point_2d
    
def head_pose_points(image, rotation_vector, translation_vector, camera_matrix):
    """
    Get the points to estimate head pose sideways    
    Parameters
    ----------
    img : np.unit8
        Original Image.
    rotation_vector : Array of float64
        Rotation Vector obtained from cv2.solvePnP
    translation_vector : Array of float64
        Translation Vector obtained from cv2.solvePnP
    camera_matrix : Array of float64
        The camera matrix
    Returns
    -------
    (x, y) : tuple
        Coordinates of line to estimate head pose
    """
    rear_size = 1
    rear_depth = 0
    front_size = image.shape[1]
    front_depth = front_size*2
    val = [rear_size, rear_depth, front_size, front_depth]
    point_2d = get_2d_points(rotation_vector, translation_vector, camera_matrix, val)
    y = (point_2d[5] + point_2d[8])//2
    x = point_2d[2]
    
    return (x, y)

def estimate_head_pose(image):
    
    image = np.array(image)
    image = image[:,:,:3]
    size = image.shape
    font = cv2.FONT_HERSHEY_SIMPLEX 
    # 3D model points.
    model_points = np.array([
                                (0.0, 0.0, 0.0),             # Nose tip
                                (0.0, -330.0, -65.0),        # Chin
                                (-225.0, 170.0, -135.0),     # Left eye left corner
                                (225.0, 170.0, -135.0),      # Right eye right corne
                                (-150.0, -150.0, -125.0),    # Left Mouth corner
                                (150.0, -150.0, -125.0)      # Right mouth corner
                            ])

    # Camera internals
    focal_length = size[1]
    center = (size[1]/2, size[0]/2)
    camera_matrix = np.array(
                            [[focal_length, 0, center[0]],
                            [0, focal_length, center[1]],
                            [0, 0, 1]], dtype = "double"
                            )

    result_dict = {
        "vertical": "none",
        "horizontal": "none"
    }

    faces = face_detect.find_faces(image, face_model)
    for face in faces:
        marks = face_landmark_detection.detect_landmarks(image, landmark_model, face) 
        # mark_detector.draw_marks(img, marks, color=(0, 255, 0))
        image_points = np.array([
                                marks[30],     # Nose tip
                                marks[8],     # Chin
                                marks[36],     # Left eye left corner
                                marks[45],     # Right eye right corne
                                marks[48],     # Left Mouth corner
                                marks[54]      # Right mouth corner
                            ], dtype="double")
        dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
        (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_UPNP)
        
        
        # Project a 3D point (0, 0, 1000.0) onto the image plane.
        # We use this to draw a line sticking out of the nose
        
        (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
        
        
        p1 = ( int(image_points[0][0]), int(image_points[0][1]))
        p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
        x1, x2 = head_pose_points(image, rotation_vector, translation_vector, camera_matrix)


        try:
            m = (p2[1] - p1[1])/(p2[0] - p1[0])
            ang1 = int(math.degrees(math.atan(m)))
        except:
            ang1 = 90
        try:
            m = (x2[1] - x1[1])/(x2[0] - x1[0])
            ang2 = int(math.degrees(math.atan(-1/m)))
        except:
            ang2 = 90
            
            # print('div by zero error')
        if ang1 >= 30: #Can be the other way around, need to test more
            result_dict["vertical"] = "down"
        elif ang1 <= -30:
            result_dict["vertical"] = "up"
        else:
            result_dict["vertical"] = "straight"
            
        if ang2 >= 30:
            result_dict["horizontal"] = "right"
        elif ang2 <= -35:
            result_dict["horizontal"] = "left"
        else:
            result_dict["horizontal"] = "straight"

    return result_dict

import cv2
import numpy as np
import math
from model_init import face_model, landmark_model
import face_detect
import face_landmark_detection


def get_2d_points(rotation_vector, translation_vector, camera_matrix, val):
    """Return the 3D points present as 2D for making annotation box"""
    point_3d = []
    dist_coeffs = np.zeros((4,1))
    rear_size = val[0]
    rear_depth = val[1]
    point_3d.append((-rear_size, -rear_size, rear_depth))
    point_3d.append((-rear_size, rear_size, rear_depth))
    point_3d.append((rear_size, rear_size, rear_depth))
    point_3d.append((rear_size, -rear_size, rear_depth))
    point_3d.append((-rear_size, -rear_size, rear_depth))
    
    front_size = val[2]
    front_depth = val[3]
    point_3d.append((-front_size, -front_size, front_depth))
    point_3d.append((-front_size, front_size, front_depth))
    point_3d.append((front_size, front_size, front_depth))
    point_3d.append((front_size, -front_size, front_depth))
    point_3d.append((-front_size, -front_size, front_depth))
    point_3d = np.array(point_3d, dtype=np.float).reshape(-1, 3)
    
    # Map to 2d img points
    (point_2d, _) = cv2.projectPoints(point_3d,
                                    rotation_vector,
                                    translation_vector,
                                    camera_matrix,
                                    dist_coeffs)
    point_2d = np.int32(point_2d.reshape(-1, 2))
    return point_2d
    
def head_pose_points(image, rotation_vector, translation_vector, camera_matrix):
    """
    Get the points to estimate head pose sideways    
    Parameters
    ----------
    img : np.unit8
        Original Image.
    rotation_vector : Array of float64
        Rotation Vector obtained from cv2.solvePnP
    translation_vector : Array of float64
        Translation Vector obtained from cv2.solvePnP
    camera_matrix : Array of float64
        The camera matrix
    Returns
    -------
    (x, y) : tuple
        Coordinates of line to estimate head pose
    """
    rear_size = 1
    rear_depth = 0
    front_size = image.shape[1]
    front_depth = front_size*2
    val = [rear_size, rear_depth, front_size, front_depth]
    point_2d = get_2d_points(rotation_vector, translation_vector, camera_matrix, val)
    y = (point_2d[5] + point_2d[8])//2
    x = point_2d[2]
    
    return (x, y)

def estimate_head_pose(image):
    
    image = np.array(image)
    image = image[:,:,:3]
    size = image.shape
    font = cv2.FONT_HERSHEY_SIMPLEX 
    # 3D model points.
    model_points = np.array([
                                (0.0, 0.0, 0.0),             # Nose tip
                                (0.0, -330.0, -65.0),        # Chin
                                (-225.0, 170.0, -135.0),     # Left eye left corner
                                (225.0, 170.0, -135.0),      # Right eye right corne
                                (-150.0, -150.0, -125.0),    # Left Mouth corner
                                (150.0, -150.0, -125.0)      # Right mouth corner
                            ])

    # Camera internals
    focal_length = size[1]
    center = (size[1]/2, size[0]/2)
    camera_matrix = np.array(
                            [[focal_length, 0, center[0]],
                            [0, focal_length, center[1]],
                            [0, 0, 1]], dtype = "double"
                            )

    result_dict = {
        "vertical": "none",
        "horizontal": "none"
    }

    faces = face_detect.find_faces(image, face_model)
    for face in faces:
        marks = face_landmark_detection.detect_landmarks(image, landmark_model, face) 
        # mark_detector.draw_marks(img, marks, color=(0, 255, 0))
        image_points = np.array([
                                marks[30],     # Nose tip
                                marks[8],     # Chin
                                marks[36],     # Left eye left corner
                                marks[45],     # Right eye right corne
                                marks[48],     # Left Mouth corner
                                marks[54]      # Right mouth corner
                            ], dtype="double")
        dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
        (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_UPNP)
        
        
        # Project a 3D point (0, 0, 1000.0) onto the image plane.
        # We use this to draw a line sticking out of the nose
        
        (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
        
        
        p1 = ( int(image_points[0][0]), int(image_points[0][1]))
        p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
        x1, x2 = head_pose_points(image, rotation_vector, translation_vector, camera_matrix)


        try:
            m = (p2[1] - p1[1])/(p2[0] - p1[0])
            ang1 = int(math.degrees(math.atan(m)))
        except:
            ang1 = 90
        try:
            m = (x2[1] - x1[1])/(x2[0] - x1[0])
            ang2 = int(math.degrees(math.atan(-1/m)))
        except:
            ang2 = 90
            
            # print('div by zero error')
        if ang1 >= 30: #Can be the other way around, need to test more
            result_dict["vertical"] = "down"
        elif ang1 <= -30:
            result_dict["vertical"] = "up"
        else:
            result_dict["vertical"] = "straight"
            
        if ang2 >= 30:
            result_dict["horizontal"] = "right"
        elif ang2 <= -35:
            result_dict["horizontal"] = "left"
        else:
            result_dict["horizontal"] = "straight"

    return result_dict
Return the 3D points present as 2D for making annotation box # Map to 2d img points Get the points to estimate head pose sideways         Parameters     ----------     img : np.unit8         Original Image.     rotation_vector : Array of float64         Rotation Vector obtained from cv2.solvePnP     translation_vector : Array of float64         Translation Vector obtained from cv2.solvePnP     camera_matrix : Array of float64         The camera matrix     Returns     -------     (x, y) : tuple         Coordinates of line to estimate head pose # 3D model points. # Nose tip # Chin # Left eye left corner # Right eye right corne # Left Mouth corner # Right mouth corner # Camera internals # mark_detector.draw_marks(img, marks, color=(0, 255, 0)) # Nose tip # Chin # Left eye left corner # Right eye right corne # Left Mouth corner # Right mouth corner # Assuming no lens distortion # Project a 3D point (0, 0, 1000.0) onto the image plane. # We use this to draw a line sticking out of the nose # print('div by zero error') #Can be the other way around, need to test more