Question

目标：

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

说明：

姿态角度定义为：

偏航是相机在水平面上的一般方向：朝北= 0，朝东= 90°，南= 180°，西= 270°等/ p>

俯仰是摄影机的“鼻子”方向：0°=水平看水平点，-90°=垂直向下看，+ 90°=垂直向上看，45 °=仰视与地平线成45°的角度等。

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

世界参考框架：

我的世界参考框架是这样的：

向东= + X
  向北= + Y
  向天空= + Z

我的世界物体点在该参考框架中给出。

相机参考框架：

根据文档，相机参考框架的方向如下：

实现目标：

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

rvec ;输出旋转矢量（参见Rodrigues()），它与tvec一起将模型坐标系中的点带到摄像机坐标系。
tvec ;输出翻译矢量。

这些矢量用于到相机参考框架我需要进行精确的逆操作，从而检索相对于世界坐标的摄像机位置和姿态。

摄像头位置：

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

rmat = cv2.Rodrigues(rvec)[0]

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

camera_position = -np.matrix(rmat).T * np.matrix(tvec)

（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°。无论如何。

偏航似乎很好。为主。

我是否错过了滚动角度的东西？

Answer 1

我认为您的转换缺少轮换。如果我正确解释了您的问题，那么您是在问（旋转 R 后跟平移 T）的倒数是什么

${\hat{R}|\vec{T}}.\vec{r}=\hat{R}.\vec{r}+\vec{T}$

逆应该返回身份

${\hat{R}|\vec{T}}^{-1}.{\hat{R}|\vec{T}}={\hat{1}|0}$

通过收益来解决这个问题

${\hat{R}|\vec{T}}^{-1}={\hat{R}^-1|-\hat{R}^-1\cdot \vec{T}}$

据我所知，您正在使用答案的 $-\hat{R}^-1\cdot \vec{T}$ （撤消翻译）部分，但省略了反向旋转 $\hat{R}^-1$

旋转+平移：

${\hat{R}|\vec{T}}\vec{r}=\hat{R}\cdot\vec{r}+\vec{T}$

逆（旋转+平移）：

${\hat{R}|\vec{T}}^{-1}\vec{r}=\hat{R}^{-1}\cdot\vec{r}-\hat{R}^{-1}\cdot \vec{T}$

非乳胶模式 (R^-1*r-R^-1*T) 是 (R.r+T) 的倒数

来自solvePnP

目标：

说明：