来自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,它们是:

rvec ; 输出旋转矢量(请参阅Rodrigues())tvec,它将点从模型坐标系带到摄像机坐标系.
tvec ; 输出翻译矢量.

这些载体被给予去到相机参考帧.
我需要进行精确的逆操作,从而检索相对于世界坐标的摄像机位置和姿态.

相机位置:

所以,我已经计算从旋转矩阵rvec有Rodrigues():

rmat = cv2.Rodrigues(rvec)[0]

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

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

(src:来自cv :: solvePnP的世界坐标中的相机位置)
这看起来相当不错.

相机姿态(偏航,俯仰和滚动):

但是如何从相机的角度检索相应的姿态角度(如上所述的偏航,俯仰和滚动)(好像它基本上在你的手中)？

我试过在函数中实现这个:http://planning.cs.uiuc.edu/node102.html#eqn:patchrmat:

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)