从solvePnP获取摄影机姿势

Question

目标：

我需要检索相机的位置和姿态角（使用OpenCV/Python）。在

定义：

姿态角定义为：

偏航是相机放置在水平面上时的总体方向：朝北=0，朝东=90°，南=180°，西=270°，等等

俯仰是相机的“机头”方向：0°=水平注视地平线上的一个点，-90°=垂直向下看，+90°=垂直向上看，45°=从地平线以45°的角度向上看，等等

滚动如果相机在你手中向左或向右倾斜（因此当这个角度变化时，它总是注视地平线上的一个点）：+45°=当你抓住相机时，顺时针旋转45°，因此+90°（和-90°）将是肖像画所需的角度，等等

世界参考系：

我的世界参照系的方向是：

Toward east = +X
Toward north = +Y
Up toward the sky = +Z

我的世界物体点是在那个参照系中给出的。在

摄像机参考帧：

根据doc，摄像机参考坐标系的方向如下所示：

实现目标：

现在，从cv2.solvepnp()到一组图像点及其对应的世界坐标，我计算了rvec和{}。 但是，根据文件：http://docs.opencv.org/trunk/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d，它们是：

rvec ; Output rotation vector (see Rodrigues()) that, together with tvec, brings points from the model coordinate system to the camera coordinate system.
tvec ; Output translation vector.

这些向量被赋予到相机参考帧。
我需要做精确的逆操作，从而检索相机相对于世界坐标的位置和姿态。在

摄像机位置：

所以我用rvec计算了rvec的旋转矩阵：

rmat = cv2.Rodrigues(rvec)[0]

如果我就在这里，摄像机在世界坐标系中的位置由下式给出：

^{pr2}$

（src:Camera position in world coordinate from cv::solvePnP）
这看起来相当不错。在

摄像机姿态（偏航、俯仰和横摇）：

但是如何从相机的角度（就好像它基本上就在你手里一样）检索出相应的姿态角（如上所述的偏航、俯仰和滚转）？在

我尝试在函数中实现：http://planning.cs.uiuc.edu/node102.html#eqn:yprmat

def rotation_matrix_to_attitude_angles(R):
    import math
    import numpy as np 
    cos_beta = math.sqrt(R[2,1] * R[2,1] + R[2,2] * R[2,2])
    validity = cos_beta < 1e-6
    if not validity:
        alpha = math.atan2(R[1,0], R[0,0])    # yaw   [z]
        beta  = math.atan2(-R[2,0], cos_beta) # pitch [y]
        gamma = math.atan2(R[2,1], R[2,2])    # roll  [x]
    else:
        alpha = math.atan2(R[1,0], R[0,0])    # yaw   [z]
        beta  = math.atan2(-R[2,0], cos_beta) # pitch [y]
        gamma = 0                             # roll  [x]  
    return np.array([alpha, beta, gamma])    

但结果和我想要的不一致。例如，我有一个~-90°的滚动角，但相机是水平的，所以应该是0左右。在

俯仰角大约是0，所以它看起来是正确的，但是我真的不明白为什么它在0左右，因为相机参考系的Z轴是水平的，所以它已经从世界参考系的垂直轴倾斜了90°。我本以为这里的值是-90°或+270°。不管怎样。在

偏航看起来不错。主要是。在

我是不是漏掉了什么东西？在