根据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿态角

关键词：旋转矩阵 旋转角 欧拉角 姿态角

用途：相机位姿估计、无人机位姿估计、各种位姿估计

文章类型：概念、公式总结（本文不带推倒过程，若想了解公式是如何推出来的请自习搜索文献），C++函数展示

@Author:VShawn(singlex@foxmail.com)

@Date:2016-11-04

@Lab: CvLab202@CSU

     

前言

相机标定后得出了旋转矩阵，而后应该怎么通过旋转矩阵表示相机姿态这一问题估计困扰过每一位研究这一问题的新同学，那么本文就给出我的一种做法，帮助大家少走歪路。当然有什么好的想法，或者更牛的办法也欢迎留言交流斧正。当然，公式的推倒就不要找我了，参考文献比我厉害多了

公式

有旋转矩阵

\[R\text{=}\left( \begin{matrix} {{r}_{11}} & {{r}_{12}} & {{r}_{13}} \\ {{r}_{21}} & {{r}_{22}} & {{r}_{23}} \\ {{r}_{31}} & {{r}_{32}} & {{r}_{33}} \\\end{matrix} \right)\]

那么可求出各轴旋转角：

Z轴：

    \[{{\theta }_{z}}=a\tan 2({{r}_{21}},{{r}_{11}})\]

Y轴

\[{{\theta }_{y}}=a\tan 2(-{{r}_{31}},\sqrt{{{r}_{31}}^{2}+{{r}_{33}}^{2}})\]

X轴

\[{{\theta }_{x}}=a\tan 2({{r}_{32}},{{r}_{33}})\]

注：atan2为C++中函数，atan2(y,x)的做法：当 x 的绝对值比 y 的绝对值大时使用 atan(y/x)；反之使用 atan(x/y)。这样就保证了数值稳定性。

上面公式的意思是，相机坐标系想要转到与世界坐标系完全平行（即xc平行于x_w，y_c平行于y_w，z_c平行于z_w，且他们的方向都是相同的），需要旋转3次，设原始相机坐标系为C₀。

1、C₀绕其z轴旋转，得到新的坐标系C₁；

2、C₁绕其y轴旋转，得到新的坐标系C₂（注意旋转轴为C₁的y轴，而非C₀的y轴）；

3、C₂绕其x轴旋转，得到新的坐标系C₃。此时C₃与世界坐标系W完全平行。

特别注意：旋转顺序为z y x，切记不能调换。

C++程序

代码就比较简单了

r11-r33从旋转矩阵中提出，为double型变量。

 

    //计算出相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。
    //旋转顺序为z、y、x
    const double PI = 3.141592653;
    double thetaz = atan2(r21, r11) / PI * 180;
    double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / PI * 180;
    double thetax = atan2(r32, r33) / PI * 180;

 

参考文献

Computing Euler angles from a rotation matrix. Gregory G. Slabaugh

http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf