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件

numpyで適当な回転行列を作成し、cv2.decomposeProjectionMatrix()を使ってRoll, Pitch, Yawを求めて見ました。いろいろな回転角を試すべきですが、少し試したところだと矛盾は無いように見えます。

"""
cf. https://en.wikipedia.org/wiki/Rotation_matrix
"""
import numpy as np
import cv2


def rot_x(theta_in_deg):
    t = np.radians(theta_in_deg)
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])


def rot_y(theta_in_deg):
    t = np.radians(theta_in_deg)
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])


def rot_z(theta_in_deg):
    t = np.radians(theta_in_deg)
    c = np.cos(t)
    s = np.sin(t)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])


def make_proj(roll, pitch, yaw):
    rot_matrix = rot_z(yaw) @ rot_y(pitch) @ rot_x(roll)
    trans_vector = np.array([0, 0, 0])
    return np.hstack((rot_matrix, trans_vector.reshape(3, 1)))


def get_euler(proj):
    (
        camera,
        rot,
        trans,
        rotX,
        rotY,
        rotZ,
        euler,
    ) = cv2.decomposeProjectionMatrix(proj)
    return euler.T[0]


def main():
    proj_matrix = make_proj(roll=0.0, pitch=0.0, yaw=90.0)
    print(get_euler(proj_matrix))

    proj_matrix = make_proj(roll=10.0, pitch=20.0, yaw=30.0)
    print(get_euler(proj_matrix))

    proj_matrix = make_proj(roll=100.0, pitch=80.0, yaw=30.0)
    print(get_euler(proj_matrix))


if __name__ == "__main__":
    main()

以下、結果です。

terminal
1$ python euler_test.py
2[ 0.  0. 90.]
3[10. 20. 30.]
4[100.  80.  30.]

投稿2020/06/16 23:11

yymmt

総合スコア1615

ベストアンサー

現在私も同じ問題にぶち当たっています。
行列の要素を用いて自前で逆算するのはかなり骨です。私はこの方法は諦めました。

ここで、solvePNPなどから出力される回転ベクトルから導かれる回転行列ですが、これは任意の点を回転させて移動させるものですよね？たしか。
従って(x:右, y:上, z:正面) = (0,0,1)の点を移動させて(a,b,c)になったとすると、この座標から原始的な幾何学によってYawとPitchを求めることができるはずです。
また、(0,0,1)に加えて(1,0,1)の点をBとし、これを同様に回転で移動させた点をB'とします。またBを、先に求めたPitchとYawの回転だけさせた点をB''とすると、AB'ベクトルとAB''ベクトルのなす角がRollになることが分かります。従ってここでRollも求めることができるはずです。

ここまでで問題なくRoll、Pitch、Yawを求めることができるはずなのですが、自分で実装してみたところどうも変な値になるので、うーんという感じです。

あくまで参考までに。
もしこの回答がヒントになって解決策が分かったら返信で教えてほしいところです。

投稿2020/06/16 20:29

CelticJapPiper

総合スコア7

fana

2020/06/17 01:03

> 行列の要素を用いて自前で逆算するのはかなり骨です。そうですか？ Roll,Pitch,Yawを用いた行列の要素を眺めれば，高校レベルの計算で解けるような話なハズですが．（ただし，特殊ケースをifで分岐させる必要はあるかな）＃コメント入れようとした際に誤って低評価が押ささってしまったのですぐ解除しました．変な通知が行っていたらすみません．

行動規範の内容に同意します

rMatrixが3x3の回転行列なのであれば，
この行列の要素を用いてroll,pitch,yawを自前で逆算すれば良いのではないでしょうか．

投稿2019/07/04 04:30

fana

総合スコア12175