来自solvePnP的相机姿势

Question

目标

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

定义

姿态角定义为：

Yaw是相机放置在水平面上时的大致方向：朝向北=0，朝向东=90°，南=180°，西=270°，等等。

俯仰是相机的“鼻子”方向：0° = 水平看地平线上的某个点，-90° = 垂直向下看，+90° = 垂直向上看，45 ° = 从地平线向上看 45° 等。

滚动表示相机在您手中时向左或向右倾斜（因此当该角度变化时，它始终注视着地平线上的一个点）：+45° = 倾斜 45°当您抓住相机时顺时针旋转，因此 +90°（和 -90°）将是例如肖像照片所需的角度，等等。

世界参考系：

我的世界参考系是这样定位的：

Toward east = +X
Toward north = +Y
Up toward the sky = +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)

（来源：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) 的倒数