欧美bbbwbbbw肥妇,免费乱码人妻系列日韩,一级黄片

C++利用eigen庫實(shí)現(xiàn)求歐拉角

 更新時間:2023年11月01日 10:02:13   作者:Cc  
這篇文章主要為大家詳細(xì)介紹了C++如何利用eigen庫自帶的matrix.eulerAngles()函數(shù)實(shí)現(xiàn)求歐拉角,文中的示例代碼講解詳細(xì),有需要的小伙伴可以參考下

不同順序歐拉角轉(zhuǎn)旋轉(zhuǎn)矩陣對照公式

eigen庫求歐拉角公式

分別試驗(yàn)eigen庫自帶的matrix.eulerAngles()函數(shù),與根據(jù)上述公式推導(dǎo)的兩種方法求歐拉角
eigen庫求得歐拉角的范圍一定是x−>roll方向在[0,π]之間,y−>pitch方向在[−π,π]之間,z−>yaw方向在[−π,π]之間,優(yōu)先會保證x−>roll方向處于[0,π]之間,才回去算別的角度。

以下代碼驗(yàn)證:

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
Quaterniond q(0.704,-0.708,0.05,0.002)//定義一個四元數(shù);
Matrix3d m = q.toRotationMatrix();//四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣;
cout << q << endl;
cout << m << endl;
Quaterniond q1(m);//旋轉(zhuǎn)矩陣轉(zhuǎn)四元數(shù),驗(yàn)證下四元數(shù)是否和原來輸入的四元數(shù)一致
cout << q1 << endl;
cout << "XYZ------------" << endl;
Vector3d v=m.eulerAngles(0,1,2);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角,以XYZ為旋轉(zhuǎn)方向
cout << v << endl;
cout << "ZYX------------" << endl;
Vector3d v1=m.eulerAngles(2,1,0);//旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角,以ZYX為旋轉(zhuǎn)方向
cout << v1 << endl;
Vector3d v2;

輸出的結(jié)果為

-0.708i + 0.05j + 0.002k + 0.704//四元數(shù)
 0.994992 -0.073616  0.067568
-0.067984 -0.002536  0.997064
-0.073232 -0.996664 -0.007528//旋轉(zhuǎn)矩陣
-0.70756i + 0.049969j + 0.00199876k + 0.704437//四元數(shù)
XYZ------------//先轉(zhuǎn)x
 1.56325//x roll
 3.07397//y pitch
-3.06774//z yaw
ZYX------------//先轉(zhuǎn)z
3.07337// z yaw
3.06825//y pitch
1.56324//x roll

可以看到以上兩種旋轉(zhuǎn)順序情況下用eigen庫自帶的matrix.eulerAngles()函數(shù)會使得z yaw,y pitch方向上計(jì)算的歐拉角數(shù)值增大,不利于機(jī)器人做角度控制,原本機(jī)器人可能朝反方向轉(zhuǎn)很小的一個角度,可能現(xiàn)在需要轉(zhuǎn)很大一圈。

用公式推導(dǎo)編寫的代碼:

//接著上面代碼段補(bǔ)充運(yùn)行即可
double sy=sqrt(m(0,0) * m(0,0) +  m(0,1) * m(0,1));
        if (sy>0.00001)//判斷萬向鎖奇異
        {
            v2(0) = std::atan2(-m(1, 2), m(2, 2));
            v2(1) = std::atan2(m(0, 2), sy);
            v2(2) = std::atan2(-m(0, 1), m(0, 0));
        }
        else//奇異
        {
            v2(0) = 0;
            v2(1) = std::atan2(m(0, 2), sy);
            v2(2) = std::atan2(-m(1, 0), m(2, 0));
        }
cout << "XYZ------------" << endl;
cout << v2 << endl;
        if (sy>0.00001)//判斷萬向鎖奇異
        {
            v2(0) = std::atan2(m(2, 1), m(2, 2));
            v2(1) = std::atan2(m(2, 0), sy);
            v2(2) = std::atan2(-m(1, 0), m(0, 0));
        }
        else//奇異
        {
            v2(0) = std::atan2(-m(1, 2), m(1, 1));;
            v2(1) = std::atan2(m(2, 0), sy);
            v2(2) = 0;
        }
cout << "ZYX------------" << endl;
cout << v2 << endl;

輸出結(jié)果:

XYZ------------
 -1.57835//x
0.0676197//y
 0.073852//z
ZYX------------
  -1.57835  //注意這里還是x,與`eigen`庫自帶的`matrix.eulerAngles()`出來的順序不一樣
-0.0732686
 0.0682201//注意這里還是z

這里角度明顯小很多,兩種不同的方法的結(jié)果可以通過加減π得到,但是XYZ、ZYX兩種不同順序下的結(jié)果還不一樣

因此建議直接用公式推導(dǎo)出來的方法。

到此這篇關(guān)于C++利用eigen庫實(shí)現(xiàn)求歐拉角的文章就介紹到這了,更多相關(guān)C++求歐拉角內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家!

相關(guān)文章

  • C++?繼承的范例講解

    C++?繼承的范例講解

    繼承是C++面向?qū)ο缶幊讨械囊婚T。繼承是子類繼承父類的特征和行為,或者是繼承父類得方法,使的子類具有父類得的特性和行為。重寫是子類對父類的允許訪問的方法實(shí)行的過程進(jìn)行重新編寫,返回值和形參都不能改變。就是對原本的父類進(jìn)行重新編寫,但是外部接口不能被重寫
    2022-06-06
  • C語言之浮點(diǎn)數(shù)的表示與儲存方式

    C語言之浮點(diǎn)數(shù)的表示與儲存方式

    這篇文章主要介紹了C語言之浮點(diǎn)數(shù)的表示與儲存方式,具有很好的參考價值,希望對大家有所幫助,如有錯誤或未考慮完全的地方,望不吝賜教
    2025-03-03
  • 一文帶你深入了解C++中的類型轉(zhuǎn)換

    一文帶你深入了解C++中的類型轉(zhuǎn)換

    在C語言中,如果賦值運(yùn)算符左右兩側(cè)類型不同,或者形參與實(shí)參類型不匹配,或者返回值類型與接收返回值類型不一致時,就需要發(fā)生類型轉(zhuǎn)化。本文主要介紹了C++中常見的四個類型轉(zhuǎn)換,需要的可以參考一下
    2022-12-12
  • C++實(shí)現(xiàn)LeetCode(76.最小窗口子串)

    C++實(shí)現(xiàn)LeetCode(76.最小窗口子串)

    這篇文章主要介紹了C++實(shí)現(xiàn)LeetCode(76.最小窗口子串),本篇文章通過簡要的案例,講解了該項(xiàng)技術(shù)的了解與使用,以下就是詳細(xì)內(nèi)容,需要的朋友可以參考下
    2021-07-07
  • C語言超詳細(xì)講解線性表

    C語言超詳細(xì)講解線性表

    線性表,數(shù)據(jù)結(jié)構(gòu)中最簡單的一種存儲結(jié)構(gòu),專門用于存儲邏輯關(guān)系為"一對一"的數(shù)據(jù)。線性表是基于數(shù)據(jù)在實(shí)際物理空間中的存儲狀態(tài),又可細(xì)分為順序表(順序存儲結(jié)構(gòu))和鏈表
    2022-07-07
  • C++中單鏈表的建立與基本操作

    C++中單鏈表的建立與基本操作

    以下是對C++中單鏈表的建立與基本操作進(jìn)行了詳細(xì)的介紹,需要的朋友可以過來參考下,希望對大家有所幫助
    2013-10-10
  • C++中的各種容器的使用方法匯總

    C++中的各種容器的使用方法匯總

    這篇文章主要介紹了C++中的各種容器的使用方法,本文結(jié)合示例代碼給大家介紹的非常詳細(xì),對大家的學(xué)習(xí)或工作具有一定的參考借鑒價值,需要的朋友可以參考下
    2023-01-01
  • C++ const引用、臨時變量 引用參數(shù)詳解

    C++ const引用、臨時變量 引用參數(shù)詳解

    下面小編就為大家?guī)硪黄狢++ const引用、臨時變量 引用參數(shù)詳解。小編覺得挺不錯的現(xiàn)在就分享給大家,也給大家做個參考。一起跟隨小編過來看看吧
    2017-01-01
  • OpenCV實(shí)現(xiàn)幀間差分法詳解

    OpenCV實(shí)現(xiàn)幀間差分法詳解

    這篇文章主要為大家詳細(xì)介紹了OpenCV實(shí)現(xiàn)幀間差分法,文中示例代碼介紹的非常詳細(xì),具有一定的參考價值,感興趣的小伙伴們可以參考一下
    2020-03-03
  • C/C++中CONST用法總結(jié)(推薦)

    C/C++中CONST用法總結(jié)(推薦)

    這篇文章主要介紹了C/C++中CONST用法總結(jié)(推薦),包括const常量與define宏定義的區(qū)別介紹,非常不錯,具有參考借鑒價值,需要的朋友參考下吧
    2017-07-07

最新評論