-
Notifications
You must be signed in to change notification settings - Fork 1
/
04_head_pose_estimator.py
123 lines (96 loc) · 4.65 KB
/
04_head_pose_estimator.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#
# Let us say you have 2D image and you would like to find out its 3D model,
# because when you have 3D model you can calculate rotation vector. This example
# is all about transforming 2D image to 3D space. This is achieved using opencv
# function solvePnP().
#
# I am sure you must be wondering how to calculate object_pts?
# Please read following blog: http://www.morethantechnical.com/2010/03/19/quick-and-easy-head-pose-estimation-with-opencv-w-code/
#
# Download Dlib 68 face landmark model: http://dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2
#
# Run : python head_pose_estimator.py
#
import cv2
import dlib
import numpy as np
from imutils import face_utils
face_landmark_path = './shape_predictor_68_face_landmarks.dat'
object_pts = np.float32([[6.825897, 6.760612, 4.402142],
[1.330353, 7.122144, 6.903745],
[-1.330353, 7.122144, 6.903745],
[-6.825897, 6.760612, 4.402142],
[5.311432, 5.485328, 3.987654],
[1.789930, 5.393625, 4.413414],
[-1.789930, 5.393625, 4.413414],
[-5.311432, 5.485328, 3.987654],
[2.005628, 1.409845, 6.165652],
[-2.005628, 1.409845, 6.165652],
[2.774015, -2.080775, 5.048531],
[-2.774015, -2.080775, 5.048531],
[0.000000, -3.116408, 6.097667],
[0.000000, -7.415691, 4.070434]])
reprojectsrc = np.float32([[10.0, 10.0, 10.0],
[10.0, 10.0, -10.0],
[10.0, -10.0, -10.0],
[10.0, -10.0, 10.0],
[-10.0, 10.0, 10.0],
[-10.0, 10.0, -10.0],
[-10.0, -10.0, -10.0],
[-10.0, -10.0, 10.0]])
line_pairs = [[0, 1], [1, 2], [2, 3], [3, 0],
[4, 5], [5, 6], [6, 7], [7, 4],
[0, 4], [1, 5], [2, 6], [3, 7]]
def get_head_pose(shape, frame):
## Cam Settings : starts ##
size = frame.shape
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"
)
dist_coeffs = np.zeros((4,1))
image_pts = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36],
shape[39], shape[42], shape[45], shape[31], shape[35],
shape[48], shape[54], shape[57], shape[8]])
_, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts,
camera_matrix, dist_coeffs)
reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec,
camera_matrix,dist_coeffs)
reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2)))
# calc euler angle
rotation_mat, _ = cv2.Rodrigues(rotation_vec)
pose_mat = cv2.hconcat((rotation_mat, translation_vec))
_, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat)
return reprojectdst, euler_angle
def main():
cap = cv2.VideoCapture(0)
if not cap.isOpened():
print("Unable to connect to camera.")
return
detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor(face_landmark_path)
while cap.isOpened():
ret, frame = cap.read()
if ret:
face_rects = detector(frame, 0)
if len(face_rects) > 0:
shape = predictor(frame, face_rects[0])
shape = face_utils.shape_to_np(shape)
reprojectdst, euler_angle = get_head_pose(shape, frame)
for (x, y) in shape:
cv2.circle(frame, (x, y), 1, (0, 0, 255), -1)
for start, end in line_pairs:
cv2.line(frame, reprojectdst[start], reprojectdst[end], (0, 0, 255))
cv2.putText(frame, "X: " + "{:7.2f}".format(euler_angle[0, 0]), (20, 20), cv2.FONT_HERSHEY_SIMPLEX,
0.75, (0, 0, 0), thickness=2)
cv2.putText(frame, "Y: " + "{:7.2f}".format(euler_angle[1, 0]), (20, 50), cv2.FONT_HERSHEY_SIMPLEX,
0.75, (0, 0, 0), thickness=2)
cv2.putText(frame, "Z: " + "{:7.2f}".format(euler_angle[2, 0]), (20, 80), cv2.FONT_HERSHEY_SIMPLEX,
0.75, (0, 0, 0), thickness=2)
cv2.imshow("demo", frame)
if cv2.waitKey(1) & 0xFF == 27:
break
if __name__ == '__main__':
main()