基于模型的头部姿态估计
Posted mc-curry
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了基于模型的头部姿态估计相关的知识,希望对你有一定的参考价值。
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)
以上是关于基于模型的头部姿态估计的主要内容,如果未能解决你的问题,请参考以下文章