基于模型的头部姿态估计

  1 import cv2 as cv
  2 import numpy as np
  3 import dlib
  4 import math
  5 
  6 detector = dlib.get_frontal_face_detector()
  7 predictor = dlib.shape_predictor('../dlib/shape_predictor_68_face_landmarks.dat')
  8 
  9 
 10 def open_camera(camera_id):
 11     cap = cv.VideoCapture(camera_id)
 12     while cap.isOpened():
 13         ok, frame = cap.read()
 14         if not ok:
 15             break
 16         if camera_id == 0 or camera_id == 1:
 17             frame = cv.flip(frame, 1, dst=None)
 18         # 获取面部2d关键点
 19         size = frame.shape
 20         image_points = get_2d_points(frame)
 21         try:
 22             camera_matrix, dist_coeffs, rotation_vec, translation_vec = get_vector(size, image_points)
 23             # print('rotation:', rotation_vec)
 24             # 通过给定的内参和外参计算三维点投影到二维图像平面上的坐标
 25             (nose_end, jacobian) = cv.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
 26                                                   rotation_vec, translation_vec,
 27                                                   camera_matrix, dist_coeffs)
 28             p1 = (int(image_points[0][0]), int(image_points[0][1]))
 29             p2 = (int(nose_end[0][0][0]), int(nose_end[0][0][1]))
 30             cv.line(frame, p1, p2, (255, 0, 0), 2)
 31         except:
 32             print('not detect face points')
 33         cv.imshow('frame', frame)
 34         c = cv.waitKey(40)
 35         if c & 0xFF == ord('q'):
 36             break
 37     cv.destroyAllWindows()
 38 
 39 
 40 def get_image_pose(frame):
 41     frame = cv.imread(frame)
 42     size = frame.shape
 43     image_points = get_2d_points(frame)
 44     try:
 45         camera_matrix, dist_coeffs, rotation_vec, translation_vec = get_vector(size, image_points)
 46         # print('rotation:', rotation_vec)
 47         # 通过给定的内参和外参计算三维点投影到二维图像平面上的坐标
 48         (nose_end, jacobian) = cv.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
 49                                                 rotation_vec, translation_vec,
 50                                                 camera_matrix, dist_coeffs)
 51         p1 = (int(image_points[0][0]), int(image_points[0][1]))
 52         p2 = (int(nose_end[0][0][0]), int(nose_end[0][0][1]))
 53         cv.line(frame, p1, p2, (255, 0, 0), 2)
 54     except:
 55         print('no')
 56     cv.imshow('frame', frame)
 57     cv.waitKey(0)
 58 
 59 def get_2d_points(frame):
 60     # 调用dlib库得到人脸关键点
 61     gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
 62     rects = detector(gray, 1)
 63     # 人脸数len(rects)
 64     # print(len(rects))
 65     for i in range(len(rects)):
 66         landmarks = np.matrix([[p.x, p.y] for p in predictor(frame, rects[i]).parts()])
 67         image_points = np.array([
 68             (landmarks[30][0, 0], landmarks[30][0, 1]),  # nose tip
 69             (landmarks[8][0, 0], landmarks[8][0, 1]),  # chin
 70             (landmarks[36][0, 0], landmarks[36][0, 1]),  # left eye left corner
 71             (landmarks[45][0, 0], landmarks[45][0, 1]),  # right eye right corner
 72             (landmarks[48][0, 0], landmarks[48][0, 1]),  # left mouth corner
 73             (landmarks[54][0, 0], landmarks[54][0, 1]),  # right mouth corner
 74         ], dtype='double')
 75         return image_points
 76 
 77 
 78 # 根据2d点，3d点，相机内参，畸变参数 获取旋转矩阵和平移矩阵
 79 def get_vector(img_size, image_points):
 80     # 3d模型坐标
 81     object_model = np.array([
 82         (0.0, 0.0, 0.0),  # Nose tip
 83         (0.0, -330.0, -65.0),  # Chin
 84         (-225.0, 170.0, -135.0),  # Left eye left corner
 85         (225.0, 170.0, -135.0),  # Right eye right corner
 86         (-150.0, -150.0, -125.0),  # Left Mouth corner
 87         (150.0, -150.0, -125.0)  # Right mouth corner
 88     ])
 89     # 焦距focal_length(相机坐标系与图像坐标系之间的距离为焦距f，也即图像坐标系原点与焦点重合)
 90     focal_length = img_size[1]
 91     center = (img_size[1] / 2, img_size[0] / 2)
 92     camera_matrix = np.array(
 93         [[focal_length, 0, center[0]],
 94          [0, focal_length, center[1]],
 95          [0, 0, 1]], dtype="double"
 96     )
 97     # camera_matrix = np.load('../camera_parameter/mtx.npy')
 98 
 99     # 相机外参假设为0
100     dist_coeffs = np.zeros((4, 1))  # Assuming no lens distortion
101     # dist_coeffs = np.load('../camera_parameter/dist.npy')
102     _, rotation_vector, translation_vector = cv.solvePnP(objectPoints=object_model, imagePoints=image_points,
103                                                          cameraMatrix=camera_matrix, distCoeffs=dist_coeffs,
104                                                          flags=cv.SOLVEPNP_ITERATIVE)
105     return camera_matrix, dist_coeffs, rotation_vector, translation_vector
106 
107 
108 # 将旋转矩阵转为欧拉角
109 # def trans_euler_angle(rotation_vector):
110 #     # calculate rotation angles
111 #     theta = cv.norm(rotation_vector, cv.NORM_L2)
112 #
113 #     # transformed to quaterniond
114 #     w = math.cos(theta / 2)
115 #     x = math.sin(theta / 2) * rotation_vector[0][0] / theta
116 #     y = math.sin(theta / 2) * rotation_vector[1][0] / theta
117 #     z = math.sin(theta / 2) * rotation_vector[2][0] / theta
118 #
119 #     ysqr = y * y
120 #     # pitch (x-axis rotation)
121 #     t0 = 2.0 * (w * x + y * z)
122 #     t1 = 1.0 - 2.0 * (x * x + ysqr)
123 #     print('t0:{}, t1:{}'.format(t0, t1))
124 #     pitch = math.atan2(t0, t1)
125 #
126 #     # yaw (y-axis rotation)
127 #     t2 = 2.0 * (w * y - z * x)
128 #     if t2 > 1.0:
129 #         t2 = 1.0
130 #     if t2 < -1.0:
131 #         t2 = -1.0
132 #     yaw = math.asin(t2)
133 #
134 #     # roll (z-axis rotation)
135 #     t3 = 2.0 * (w * z + x * y)
136 #     t4 = 1.0 - 2.0 * (ysqr + z * z)
137 #     roll = math.atan2(t3, t4)
138 #
139 #     print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll))
140 #
141 #     # 单位转换：将弧度转换为度
142 #     Y = int((pitch / math.pi) * 180)
143 #     X = int((yaw / math.pi) * 180)
144 #     Z = int((roll / math.pi) * 180)
145 #
146 #     return 0, Y, X, Z
147 
148 
149 if __name__ == '__main__':
150     get_image_pose(1)