单应矩阵分解

import cv2
import numpy as np
import math

def f(rmat):
    rmat = rmat.T
    thetaz = - math.atan2(rmat[1,0], rmat[0,0])
    thetay = math.atan2(-1 * rmat[2,0], math.sqrt(rmat[2,1]*rmat[2,1] + rmat[2,2]*rmat[2,2]))
    thetax = math.atan2(rmat[2,1], rmat[2,2]) + math.pi/2.0
    print("thetax::", thetax, "thetay::", thetay, "thetaz::", thetaz)

def f2(rmat):
    rmat = rmat.T
    thetaz = - math.atan2(rmat[1,0], rmat[0,0])
    thetay = -math.atan2(-1 * rmat[2,0], math.sqrt(rmat[2,1]*rmat[2,1] + rmat[2,2]*rmat[2,2]))
    thetax = math.atan2(rmat[2,1], rmat[2,2]) - math.pi/2.0
    print("thetax::", thetax, "thetay::", thetay, "thetaz::", thetaz)

p3d = np.array(
[[   0.,            0.,            0.        ],
 [   7.7055335,    50.15200424,    0.        ],
 [  74.38413239,   40.25504684,    0.        ],
 [  67.29525757,  -10.45296764,    0.        ],
 [  16.02745819,  100.30400848,    0.        ],
 [  83.21954346,   90.96305847,    0.        ],
 [  41.30167007,   45.81513596,    0.        ]]
)
K = np.array(
[[  3.74630677e+03,   0.00000000e+00,   2.73550000e+03],
 [  0.00000000e+00,   3.74630677e+03,   1.53850000e+03],
 [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00]]
)
Q, _ = cv2.projectPoints(p3d, np.array([0, 0, 0], np.float) * math.pi / 180, np.array([0, 0, 0], np.float), K, None)
print("Q::", Q)
P, _ = cv2.projectPoints(p3d, np.array([[ 1.92324683],[-1.95083484],[ 0.51931905]], np.float), np.array([[  69.76540137],[  39.36670081],[ 112.09407784]], np.float), K, None)
print("P::", P)
H, _ = cv2.findHomography(Q, P)
print("H::", H)
_, Rs, Ts, Ns  = cv2.decomposeHomographyMat(H, K)
print("Rs::", Rs)

print ("origin::")
f(cv2.Rodrigues(np.array([[ 1.92324683],[-1.95083484],[ 0.51931905]], np.float))[0])

for idx in range(len(Rs)):
    R_ = Rs[idx]
    T_ = Ts[idx]
    rvec = cv2.Rodrigues(R_)[0] #* 180 / pi
    print(rvec)
    f2(R_)
    tmp, _ = cv2.projectPoints(p3d, R_, T_, K, None)
    print(tmp)