从solvePnP中获得相机姿态

Question

目标

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

定义

姿态角由以下定义：

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

俯仰角是相机的“鼻子”方向：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，它们是： ; 输出旋转向量（参见Rodrigues()），与 tvec 一起将模型坐标系中的点带到相机坐标系中。
; 输出平移向量。

这些向量是提供给相机参考框架的。
我需要执行确切的反向操作，从而检索相机位置和姿态相对于世界坐标。

相机位置：

因此，我已经使用 Rodrigues() 从 rvec 计算出旋转矩阵：

rmat = cv2.Rodrigues(rvec)[0]

如果我没错的话，相机在世界坐标系中的位置可以表示为：

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

(来源: 从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°。无论如何。

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

问题

我在滚转角方面漏掉了什么吗？

- swiss_knight

---

我遇到了完全相同的问题：我使用与您相同的过程获取相机位置，看起来是正确的。但是偏航、俯仰和翻滚角似乎毫无意义...您是否成功解决了这个问题？我非常感兴趣。 - user2756724

根据x约定，可以通过以下方式获得3-1-3外在欧拉角φ、θ和ψ：alpha = math.atan2(R[2,0], R[2,1])，beta = math.acos(R[2,2])和gamma= -math.atan2(R[0,2], R[1,2])。 - cyborg

Answer 1

欧拉旋转的顺序（俯仰、偏航、翻滚）很重要。 根据x约定，可以通过以下方式获得3-1-3外部欧拉角φ、θ和ψ（首先绕z轴，然后绕x轴，最后再次绕z轴）：

sx = math.sqrt((R[2,0])**2 +  (R[2,1])**2)
tolerance = 1e-6;

if (sx > tolerance): # no singularity
    alpha = math.atan2(R[2,0], R[2,1])
    beta = math.atan2(sx,  R[2,2])  
    gamma= -math.atan2(R[0,2], R[1,2])
else:
    alpha = 0
    beta = math.atan2(sx,  R[2,2])  
    gamma= 0

但这并不是唯一的解决方案。例如ZYX，

sy = math.sqrt((R[0,0])**2 +  (R[1,0])**2)
tolerance = 1e-6;

if (sy > tolerance): # no singularity
    alpha = math.atan2(R[1,0], R[0,0])
    beta = math.atan2(-R[2,0], sy)  
    gamma= math.atan2(R[2,1] , R[2,2])
else:
    alpha = 0
    beta = math.atan2(-R[2,0], sy) 
    gamma= math.atan2(-R[1,2], R[1,1])

- cyborg

Answer 2

在这个链接中：http://planning.cs.uiuc.edu/node102.html#eqn:yprmat，他们假设对象的坐标系与您的相机不同。
他们定义：
Roll - 绕 x 轴旋转（在您的情况下是绕 z 轴）
Pitch - 绕 y 轴旋转（在您的情况下是绕 x 轴）
Yaw - 绕 z 轴旋转（在您的情况下是绕 y 轴）

为了得到正确的转换，您需要根据这三个角度重新计算完整的旋转矩阵：

因此，对于逆转换，您将得到：

 cos_beta = math.sqrt(R[0,2] * R[0,2] + R[2,2] * R[2,2])
 alpha = math.atan2(R[0,2], R[2,2])    # yaw   [z]
 beta  = math.atan2(-R[1, 2], cos_beta) # pitch [y]
 gamma = math.atan2(R[1, 0], R[1,1])

- Daniel Tomer

Answer 3

我想你的变换缺少了旋转。如果我正确理解你的问题，你正在问的是(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}$

非latex模式下的(R^-1*r-R^-1*T)是(R.r+T)的逆。

- user3282375