1
# -*- coding: utf-8 -*-
2
"""
3
Created on Fri Jul 31 03:00:36 2020
4

5
@author: hp
6
"""
7

8
import cv2
9
import numpy as np
10
import math
11
from face_detector import get_face_detector, find_faces
12
from face_landmarks import get_landmark_model, detect_marks
13

14
def get_2d_points(img, rotation_vector, translation_vector, camera_matrix, val):
15
    """Return the 3D points present as 2D for making annotation box"""
16
    point_3d = []
17
    dist_coeffs = np.zeros((4,1))
18
    rear_size = val[0]
19
    rear_depth = val[1]
20
    point_3d.append((-rear_size, -rear_size, rear_depth))
21
    point_3d.append((-rear_size, rear_size, rear_depth))
22
    point_3d.append((rear_size, rear_size, rear_depth))
23
    point_3d.append((rear_size, -rear_size, rear_depth))
24
    point_3d.append((-rear_size, -rear_size, rear_depth))
25
    
26
    front_size = val[2]
27
    front_depth = val[3]
28
    point_3d.append((-front_size, -front_size, front_depth))
29
    point_3d.append((-front_size, front_size, front_depth))
30
    point_3d.append((front_size, front_size, front_depth))
31
    point_3d.append((front_size, -front_size, front_depth))
32
    point_3d.append((-front_size, -front_size, front_depth))
33
    point_3d = np.array(point_3d, dtype=np.float64).reshape(-1, 3)
34
    
35
    # Map to 2d img points
36
    (point_2d, _) = cv2.projectPoints(point_3d,
37
                                      rotation_vector,
38
                                      translation_vector,
39
                                      camera_matrix,
40
                                      dist_coeffs)
41
    point_2d = np.int32(point_2d.reshape(-1, 2))
42
    return point_2d
43

44
def draw_annotation_box(img, rotation_vector, translation_vector, camera_matrix,
45
                        rear_size=300, rear_depth=0, front_size=500, front_depth=400,
46
                        color=(255, 255, 0), line_width=2):
47
    """
48
    Draw a 3D anotation box on the face for head pose estimation
49

50
    Parameters
51
    ----------
52
    img : np.unit8
53
        Original Image.
54
    rotation_vector : Array of float64
55
        Rotation Vector obtained from cv2.solvePnP
56
    translation_vector : Array of float64
57
        Translation Vector obtained from cv2.solvePnP
58
    camera_matrix : Array of float64
59
        The camera matrix
60
    rear_size : int, optional
61
        Size of rear box. The default is 300.
62
    rear_depth : int, optional
63
        The default is 0.
64
    front_size : int, optional
65
        Size of front box. The default is 500.
66
    front_depth : int, optional
67
        Front depth. The default is 400.
68
    color : tuple, optional
69
        The color with which to draw annotation box. The default is (255, 255, 0).
70
    line_width : int, optional
71
        line width of lines drawn. The default is 2.
72

73
    Returns
74
    -------
75
    None.
76

77
    """
78
    
79
    rear_size = 1
80
    rear_depth = 0
81
    front_size = img.shape[1]
82
    front_depth = front_size*2
83
    val = [rear_size, rear_depth, front_size, front_depth]
84
    point_2d = get_2d_points(img, rotation_vector, translation_vector, camera_matrix, val)
85
    # # Draw all the lines
86
    cv2.polylines(img, [point_2d], True, color, line_width, cv2.LINE_AA)
87
    cv2.line(img, tuple(point_2d[1]), tuple(
88
        point_2d[6]), color, line_width, cv2.LINE_AA)
89
    cv2.line(img, tuple(point_2d[2]), tuple(
90
        point_2d[7]), color, line_width, cv2.LINE_AA)
91
    cv2.line(img, tuple(point_2d[3]), tuple(
92
        point_2d[8]), color, line_width, cv2.LINE_AA)
93
    
94
    
95
def head_pose_points(img, rotation_vector, translation_vector, camera_matrix):
96
    """
97
    Get the points to estimate head pose sideways    
98

99
    Parameters
100
    ----------
101
    img : np.unit8
102
        Original Image.
103
    rotation_vector : Array of float64
104
        Rotation Vector obtained from cv2.solvePnP
105
    translation_vector : Array of float64
106
        Translation Vector obtained from cv2.solvePnP
107
    camera_matrix : Array of float64
108
        The camera matrix
109

110
    Returns
111
    -------
112
    (x, y) : tuple
113
        Coordinates of line to estimate head pose
114

115
    """
116
    rear_size = 1
117
    rear_depth = 0
118
    front_size = img.shape[1]
119
    front_depth = front_size*2
120
    val = [rear_size, rear_depth, front_size, front_depth]
121
    point_2d = get_2d_points(img, rotation_vector, translation_vector, camera_matrix, val)
122
    y = (point_2d[5] + point_2d[8])//2
123
    x = point_2d[2]
124
    
125
    return (x, y)
126
    
127
face_model = get_face_detector()
128
landmark_model = get_landmark_model()
129

130
font = cv2.FONT_HERSHEY_SIMPLEX 
131
# 3D model points.
132
model_points = np.array([
133
                            (0.0, 0.0, 0.0),             # Nose tip
134
                            (0.0, -330.0, -65.0),        # Chin
135
                            (-225.0, 170.0, -135.0),     # Left eye left corner
136
                            (225.0, 170.0, -135.0),      # Right eye right corne
137
                            (-150.0, -150.0, -125.0),    # Left Mouth corner
138
                            (150.0, -150.0, -125.0)      # Right mouth corner
139
                        ])
140

141
def detect_head_pose(video_path):
142
    cap = cv2.VideoCapture(video_path)
143
    ret, img = cap.read()
144
    size = img.shape
145

146
    # Camera internals
147
    focal_length = size[1]
148
    center = (size[1]/2, size[0]/2)
149
    camera_matrix = np.array(
150
                            [[focal_length, 0, center[0]],
151
                            [0, focal_length, center[1]],
152
                            [0, 0, 1]], dtype = "double"
153
                            )
154
    while True:
155
        ret, img = cap.read()
156
        if ret == True:
157
            faces = find_faces(img, face_model)
158
            for face in faces:
159
                marks = detect_marks(img, landmark_model, face)
160
                # mark_detector.draw_marks(img, marks, color=(0, 255, 0))
161
                image_points = np.array([
162
                                        marks[30],     # Nose tip
163
                                        marks[8],     # Chin
164
                                        marks[36],     # Left eye left corner
165
                                        marks[45],     # Right eye right corne
166
                                        marks[48],     # Left Mouth corner
167
                                        marks[54]      # Right mouth corner
168
                                    ], dtype="double")
169
                dist_coeffs = np.zeros((4,1)) # Assuming no lens distortion
170
                (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_UPNP)
171
                
172
                
173
                # Project a 3D point (0, 0, 1000.0) onto the image plane.
174
                # We use this to draw a line sticking out of the nose
175
                
176
                (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vector, translation_vector, camera_matrix, dist_coeffs)
177
                
178
                for p in image_points:
179
                    cv2.circle(img, (int(p[0]), int(p[1])), 3, (0,0,255), -1)
180
                
181
                
182
                p1 = ( int(image_points[0][0]), int(image_points[0][1]))
183
                p2 = ( int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
184
                x1, x2 = head_pose_points(img, rotation_vector, translation_vector, camera_matrix)
185

186
                cv2.line(img, p1, p2, (0, 255, 255), 2)
187
                cv2.line(img, tuple(x1), tuple(x2), (255, 255, 0), 2)
188
                # for (x, y) in marks:
189
                #     cv2.circle(img, (x, y), 4, (255, 255, 0), -1)
190
                # cv2.putText(img, str(p1), p1, font, 1, (0, 255, 255), 1)
191
                try:
192
                    m = (p2[1] - p1[1])/(p2[0] - p1[0])
193
                    ang1 = int(math.degrees(math.atan(m)))
194
                except:
195
                    ang1 = 90
196
                    
197
                try:
198
                    m = (x2[1] - x1[1])/(x2[0] - x1[0])
199
                    ang2 = int(math.degrees(math.atan(-1/m)))
200
                except:
201
                    ang2 = 90
202
                    
203
                    # print('div by zero error')
204
                if ang1 >= 48:
205
                    print('Head down')
206
                    cv2.putText(img, 'Head down', (30, 30), font, 2, (255, 255, 128), 3)
207
                elif ang1 <= -48:
208
                    print('Head up')
209
                    cv2.putText(img, 'Head up', (30, 30), font, 2, (255, 255, 128), 3)
210
                
211
                if ang2 >= 48:
212
                    print('Head right')
213
                    cv2.putText(img, 'Head right', (90, 30), font, 2, (255, 255, 128), 3)
214
                elif ang2 <= -48:
215
                    print('Head left')
216
                    cv2.putText(img, 'Head left', (90, 30), font, 2, (255, 255, 128), 3)
217
                
218
                cv2.putText(img, str(ang1), tuple(p1), font, 2, (128, 255, 255), 3)
219
                cv2.putText(img, str(ang2), tuple(x1), font, 2, (255, 255, 128), 3)
220
            cv2.imshow('img', img)
221
            if cv2.waitKey(1) & 0xFF == ord('q'):
222
                break
223
        else:
224
            break
225
    cv2.destroyAllWindows()
226
    cap.release()
227