cv2.Rodriguesを用いたpitch・yaw・rollの算出方法

cv2.Rodriguesを用いたpitch・yaw・rollの算出方法

現在、dlibを用いて頭部推定を行っております。顔を検知し、solvePnPで回転ベクトル等は算出することはできました。
また、projectPointsで映像に対して、xyz方向を表示するところも実装はできました。

ネットに落ちているdecomposeProjectionMatrixをしようする方法でpitch・yaw・rollを算出したのですが
値が正確ではありません（正面の正規化ができていない感じがします...）

どのようにプログラムを書けば正確なpitch・yaw・rollが算出できるのでしょうか？
よろしくお願いします

参考にしたサイト
・ここまで参考にしたサイト

該当のソースコード

import cv2
import dlib
import numpy as np
import os

K = [977.93097999805309,0.00000000000000,645.97763974817144,0.00000000000000,979.76590271719897,358.70241515058876,0.00000000000000,0.00000000000000,1.00000000000000]
D = [-0.12590017568789,0.78177975429702,-0.00352001270650,0.00261067326856,-1.50761837519630]

cam_matrix = np.array(K).reshape(3, 3).astype(np.float32)
dist_coeffs = np.array(D).reshape(5, 1).astype(np.float32)

P3D_RIGHT_SIDE = np.float32([-100.0, -77.5, -5.0]) #0
P3D_GONION_RIGHT = np.float32([-110.0, -77.5, -85.0]) #4
P3D_MENTON = np.float32([0.0, 0.0, -122.7]) #8
P3D_GONION_LEFT = np.float32([-110.0, 77.5, -85.0]) #12
P3D_LEFT_SIDE = np.float32([-100.0, 77.5, -5.0]) #16
P3D_FRONTAL_BREADTH_RIGHT = np.float32([-20.0, -56.1, 10.0]) #17
P3D_FRONTAL_BREADTH_LEFT = np.float32([-20.0, 56.1, 10.0]) #26
P3D_SELLION = np.float32([0.0, 0.0, 0.0]) #27
P3D_NOSE = np.float32([21.1, 0.0, -48.0]) #30
P3D_SUB_NOSE = np.float32([5.0, 0.0, -52.0]) #33
P3D_RIGHT_EYE = np.float32([-20.0, -65.5,-5.0]) #36
P3D_RIGHT_TEAR = np.float32([-10.0, -40.5,-5.0]) #39
P3D_LEFT_TEAR = np.float32([-10.0, 40.5,-5.0]) #42
P3D_LEFT_EYE = np.float32([-20.0, 65.5,-5.0]) #45
#P3D_LIP_RIGHT = np.float32([-20.0, 65.5,-5.0]) #48
#P3D_LIP_LEFT = np.float32([-20.0, 65.5,-5.0]) #54
P3D_STOMION = np.float32([10.0, 0.0, -75.0]) #62
landmarks_3D = np.float32([P3D_RIGHT_SIDE,
                                  P3D_GONION_RIGHT,
                                  P3D_MENTON,
                                  P3D_GONION_LEFT,
                                  P3D_LEFT_SIDE,
                                  P3D_FRONTAL_BREADTH_RIGHT,
                                  P3D_FRONTAL_BREADTH_LEFT,
                                  P3D_SELLION,
                                  P3D_NOSE,
                                  P3D_SUB_NOSE,
                                  P3D_RIGHT_EYE,
                                  P3D_RIGHT_TEAR,
                                  P3D_LEFT_TEAR,
                                  P3D_LEFT_EYE,
                                  P3D_STOMION])
TRACKED_POINTS = (0, 4, 8, 12, 16, 17, 26, 27, 30, 33, 36, 39, 42, 45, 62)

def main():
    cap = cv2.VideoCapture(0)
    if not cap.isOpened():
        print("Unable to connect to camera.")
        return
    #Declaring the two classifiers
    dlib_landmarks_file = "./shape_predictor_68_face_landmarks.dat"
    if(os.path.isfile(dlib_landmarks_file)==False):
        print("The dlib landmarks file is missing! Use the following commands to download and unzip: ")
        print(">> wget dlib.net/files/shape_predictor_68_face_landmarks.dat.bz2")
        print(">> bzip2 -d shape_predictor_68_face_landmarks.dat.bz2")
        return

    my_detector = face_landmark_detection.faceLandmarkDetection(dlib_landmarks_file)
    my_face_detector = dlib.get_frontal_face_detector()

    while cap.isOpened():
        ret, frame = cap.read()
        if ret:

            faces_array = my_face_detector(frame, 1)
            for i, pos in enumerate(faces_array):


                face_x1 = pos.left()
                face_y1 = pos.top()
                face_x2 = pos.right()
                face_y2 = pos.bottom()
                landmarks_2D = my_detector.returnLandmarks(frame, face_x1, face_y1, face_x2, face_y2,points_to_return=TRACKED_POINTS)
                retval, rvec, tvec = cv2.solvePnP(landmarks_3D,
                                                  landmarks_2D,
                                                  cam_matrix, dist_coeffs, )


                axis = np.float32([[150, 0, 0],
                                      [0, 150, 0],
                                      [0, 0, 150]])
                imgpts, jac = cv2.projectPoints(axis, rvec, tvec, cam_matrix, dist_coeffs)


                sellion_xy = (landmarks_2D[7][0], landmarks_2D[7][1])
                cv2.line(frame, sellion_xy, tuple(imgpts[1].ravel()), (0, 255, 0), 3)  # GREEN
                cv2.line(frame, sellion_xy, tuple(imgpts[2].ravel()), (255, 0, 0), 3)  # BLUE
                cv2.line(frame, sellion_xy, tuple(imgpts[0].ravel()), (0, 0, 255), 3)  # RED

#下記の角度が値が正確ではない。
                rMatrix, jacobian = cv2.Rodrigues(rvec)
                pose_mat = cv2.hconcat((rMatrix, tvec))
                euler_angle = cv2.decomposeProjectionMatrix(pose_mat)[6]