zoukankan      html  css  js  c++  java
  • 旋转矩阵、旋转向量(轴角)、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)

     1 #include <iostream>
     2 #include <cmath>
     3 using namespace std;
     4 
     5 #include <Eigen/Core>
     6 // Eigen 几何模块
     7 #include <Eigen/Geometry>
     8 
     9 
    10 int main ( int argc, char** argv )
    11 {
    12 
    13     //旋转矩阵R
    14     //3X3的旋转矩阵可以用Matrix3d,也Matrix3f
    15     Eigen::Matrix3d R=Eigen::Matrix3d::Identity();//将其赋值为单位矩阵
    16 //    Eigen::Matrix3d R;
    17 //    R.setIdentity();  //也是将旋转矩阵赋值为单位矩阵,与上面效果一样
    18     cout<<"R= 
    "<<R<<endl;
    19 
    20     //旋转向量v(轴角)
    21     //旋转向量使用AngleAxisd(f),底层不直接是Matrix,但因为重载了运算符,运算可以当作矩阵
    22     Eigen::AngleAxisd v(M_PI/4,Eigen::Vector3d(0,0,1));//沿z轴旋转了45度
    23     cout << "rotation vector: Angle is: " << v.angle() * (180 / M_PI)<<endl//旋转角
    24          << "  Axis is: " << v.axis().transpose() << endl<<endl;//旋转轴
    25     //将旋转向量转换为旋转矩阵
    26     //方式一:用matrix()
    27 //    R=v.matrix();
    28     //方式二:用toRotationMatrix()
    29     R=v.toRotationMatrix();
    30     cout<<"R=
    "<<R<<endl<<endl;
    31     //将旋转向量转化为四元数q
    32     Eigen::Quaterniond q = Eigen::Quaterniond(v);
    33     cout<<"q=
    "<<q.coeffs()<<endl<<endl;//coeffs的顺序:(x,y,z,w)
    34     cout<<"q=
    "<<q.x()<<endl<<q.y()<<endl<<q.z()<<endl<<q.w()<<endl<<endl;//四元数的另一种输出方式
    35 
    36 
    37     //将旋转矩阵转化为四元数
    38     q = Eigen::Quaterniond(R);
    39      cout<<"q=
    "<<q.coeffs()<<endl<<endl;
    40     //将旋转矩阵转化为欧拉角
    41      Eigen::Vector3d euler_angles=R.eulerAngles(2,1,0);
    42      cout<<"yaw(Z) pitch(Y) roll(X)=
    "<<euler_angles.transpose()<<endl<<endl;
    43 
    44 
    45      //将四元数转化为旋转矩阵
    46      R=Eigen::Matrix3d(q);
    47      cout<<"R=
    "<<R<<endl<<endl;
    48 
    49      //将四元数转化为旋转向量
    50      v=Eigen::AngleAxisd(q);
    51      cout<<v.matrix()<<endl<<endl;
    52         return 0;
    53 }

    运行结果:R=
    1 0 0
    0 1 0
    0 0 1
    rotation vector: Angle is: 45
      Axis is: 0 0 1

    R=
     0.707107 -0.707107         0
     0.707107  0.707107         0
            0         0         1

    q=
           0
           0
    0.382683
     0.92388

    q=
    0
    0
    0.382683
    0.92388

    q=
           0
           0
    0.382683
     0.92388

    yaw(Z) pitch(Y) roll(X)=
    0.785398       -0        0

    R=
     0.707107 -0.707107         0
     0.707107  0.707107         0
            0         0         1

     0.707107 -0.707107         0
     0.707107  0.707107         0
            0         0         1

  • 相关阅读:
    485通讯和电力线载波通讯[收集转载]
    鹿城通综合业务系统常见问题
    485总表专题
    试试Live Writer的代码插件
    WPF路径动画——XAML篇
    无功电量的抄收
    一个JQuery操作Table的好方法
    季节计算脚本
    【学艺不精系列】关于Json.NET的反序列化
    NT6.x以上系统,多版本SQL Server局域网配置
  • 原文地址:https://www.cnblogs.com/cc111/p/9354924.html
Copyright © 2011-2022 走看看