cv2.Rodriguesを用いたpitch・yaw・rollの算出方法
現在、dlibを用いて頭部推定を行っております。顔を検知し、solvePnPで回転ベクトル等は算出することはできました。
また、projectPointsで映像に対して、xyz方向を表示するところも実装はできました。
ネットに落ちているdecomposeProjectionMatrixをしようする方法でpitch・yaw・rollを算出したのですが
値が正確ではありません(正面の正規化ができていない感じがします...)
どのようにプログラムを書けば正確なpitch・yaw・rollが算出できるのでしょうか?
よろしくお願いします
参考にしたサイト
・ここまで参考にしたサイト
該当のソースコード
python
1import cv2 2import dlib 3import numpy as np 4import os 5 6K = [977.93097999805309,0.00000000000000,645.97763974817144,0.00000000000000,979.76590271719897,358.70241515058876,0.00000000000000,0.00000000000000,1.00000000000000] 7D = [-0.12590017568789,0.78177975429702,-0.00352001270650,0.00261067326856,-1.50761837519630] 8 9cam_matrix = np.array(K).reshape(3, 3).astype(np.float32) 10dist_coeffs = np.array(D).reshape(5, 1).astype(np.float32) 11 12P3D_RIGHT_SIDE = np.float32([-100.0, -77.5, -5.0]) #0 13P3D_GONION_RIGHT = np.float32([-110.0, -77.5, -85.0]) #4 14P3D_MENTON = np.float32([0.0, 0.0, -122.7]) #8 15P3D_GONION_LEFT = np.float32([-110.0, 77.5, -85.0]) #12 16P3D_LEFT_SIDE = np.float32([-100.0, 77.5, -5.0]) #16 17P3D_FRONTAL_BREADTH_RIGHT = np.float32([-20.0, -56.1, 10.0]) #17 18P3D_FRONTAL_BREADTH_LEFT = np.float32([-20.0, 56.1, 10.0]) #26 19P3D_SELLION = np.float32([0.0, 0.0, 0.0]) #27 20P3D_NOSE = np.float32([21.1, 0.0, -48.0]) #30 21P3D_SUB_NOSE = np.float32([5.0, 0.0, -52.0]) #33 22P3D_RIGHT_EYE = np.float32([-20.0, -65.5,-5.0]) #36 23P3D_RIGHT_TEAR = np.float32([-10.0, -40.5,-5.0]) #39 24P3D_LEFT_TEAR = np.float32([-10.0, 40.5,-5.0]) #42 25P3D_LEFT_EYE = np.float32([-20.0, 65.5,-5.0]) #45 26#P3D_LIP_RIGHT = np.float32([-20.0, 65.5,-5.0]) #48 27#P3D_LIP_LEFT = np.float32([-20.0, 65.5,-5.0]) #54 28P3D_STOMION = np.float32([10.0, 0.0, -75.0]) #62 29landmarks_3D = np.float32([P3D_RIGHT_SIDE, 30 P3D_GONION_RIGHT, 31 P3D_MENTON, 32 P3D_GONION_LEFT, 33 P3D_LEFT_SIDE, 34 P3D_FRONTAL_BREADTH_RIGHT, 35 P3D_FRONTAL_BREADTH_LEFT, 36 P3D_SELLION, 37 P3D_NOSE, 38 P3D_SUB_NOSE, 39 P3D_RIGHT_EYE, 40 P3D_RIGHT_TEAR, 41 P3D_LEFT_TEAR, 42 P3D_LEFT_EYE, 43 P3D_STOMION]) 44TRACKED_POINTS = (0, 4, 8, 12, 16, 17, 26, 27, 30, 33, 36, 39, 42, 45, 62) 45 46def main(): 47 cap = cv2.VideoCapture(0) 48 if not cap.isOpened(): 49 print("Unable to connect to camera.") 50 return 51 #Declaring the two classifiers 52 dlib_landmarks_file = "./shape_predictor_68_face_landmarks.dat" 53 if(os.path.isfile(dlib_landmarks_file)==False): 54 print("The dlib landmarks file is missing! Use the following commands to download and unzip: ") 55 print(">> wget dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2") 56 print(">> bzip2 -d shape_predictor_68_face_landmarks.dat.bz2") 57 return 58 59 my_detector = face_landmark_detection.faceLandmarkDetection(dlib_landmarks_file) 60 my_face_detector = dlib.get_frontal_face_detector() 61 62 while cap.isOpened(): 63 ret, frame = cap.read() 64 if ret: 65 66 faces_array = my_face_detector(frame, 1) 67 for i, pos in enumerate(faces_array): 68 69 70 face_x1 = pos.left() 71 face_y1 = pos.top() 72 face_x2 = pos.right() 73 face_y2 = pos.bottom() 74 landmarks_2D = my_detector.returnLandmarks(frame, face_x1, face_y1, face_x2, face_y2,points_to_return=TRACKED_POINTS) 75 retval, rvec, tvec = cv2.solvePnP(landmarks_3D, 76 landmarks_2D, 77 cam_matrix, dist_coeffs, ) 78 79 80 axis = np.float32([[150, 0, 0], 81 [0, 150, 0], 82 [0, 0, 150]]) 83 imgpts, jac = cv2.projectPoints(axis, rvec, tvec, cam_matrix, dist_coeffs) 84 85 86 sellion_xy = (landmarks_2D[7][0], landmarks_2D[7][1]) 87 cv2.line(frame, sellion_xy, tuple(imgpts[1].ravel()), (0, 255, 0), 3) # GREEN 88 cv2.line(frame, sellion_xy, tuple(imgpts[2].ravel()), (255, 0, 0), 3) # BLUE 89 cv2.line(frame, sellion_xy, tuple(imgpts[0].ravel()), (0, 0, 255), 3) # RED 90 91#下記の角度が値が正確ではない。 92 rMatrix, jacobian = cv2.Rodrigues(rvec) 93 pose_mat = cv2.hconcat((rMatrix, tvec)) 94 euler_angle = cv2.decomposeProjectionMatrix(pose_mat)[6] 95

回答3件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。