python相机拍照显示_从solvePnP获取摄影机姿势

目标：

我需要检索相机的位置和姿态角（使用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，摄像机参考坐标系的方向如下所示：

实现目标：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}$

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

但是如何从相机的角度（就好像它基本上就在你手里一样）检索出相应的姿态角（如上所述的偏航、俯仰和滚转）？在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°。不管怎样。在

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

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