根據相機旋轉矩陣求解三個軸的旋轉角/歐拉角/姿態角

關鍵詞：旋轉矩陣 旋轉角 歐拉角 姿態角

用途：相機位姿估計、無人機位姿估計、各種位姿估計

文章類型：概念、公式總結（本文不帶推倒過程，若想了解公式是如何推出來的請自習搜索文獻），C++函數展示

@Author:VShawn([e-mail])

@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