Eigen(2):使用eigen进行四元数与旋转矩阵转换(c++实现)

1 四元数转旋转矩阵

        使用了normalized()函数和toRotationMatrix()函数

//下面的变量名称自拟      
    Eigen::Quaterniond q_odom_curr_tmp;//声明一个Eigen类的四元数
    //此处进行赋值，使用其他语句以及合理的常数也可
    q_odom_curr_tmp.x() = imuVec[imuVec.size()-1].orientation.x;
    q_odom_curr_tmp.y() = imuVec[imuVec.size()-1].orientation.y;
    q_odom_curr_tmp.z() = imuVec[imuVec.size()-1].orientation.z;
    q_odom_curr_tmp.w() =imuVec[imuVec.size()-1].orientation.w;
    Eigen::Matrix3d R_odom_curr_tmp;//声明一个Eigen类的3*3的旋转矩阵
    //四元数转为旋转矩阵--先归一化再转为旋转矩阵
    R_odom_curr_tmp= q_odom_curr_tmp.normalized().toRotationMatrix();  

        打印输出四元数：

    std::cout<<"q_odom_curr_now:"<< q_odom_curr_now.x() <<q_odom_curr_now.y()<<q_odom_curr_now.z()<<q_odom_curr_now.w()<<std::endl;

2 旋转矩阵转四元数

        下面使用了inverse()--求逆函数，Eigen::Quaterniond()--求四元数，normalize()--归一化

//注：下面的赋值方法及变量名称自己按情况定
Eigen::Matrix3d R_odom_curr_now;//3*3旋转矩阵
R_odom_curr_now=R_odom_curr0.inverse()*R_odom_curr_tmp;//对旋转矩阵进行赋值
q_odom_curr_now=Eigen::Quaterniond(R_odom_curr_now);//旋转矩阵转为四元数
q_odom_curr_now.normalize();//转为四元数之后，需要进行归一化

3 四元数转欧拉角

        下面是函数的主体代码，用到了ros的tf::Quaternion和tf::Matrix3x3，Eigen可用可不用，主要传递给函数的参数能够代替四元数即可，返回值为角度值

#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#define M_PI 3.1415926;

double getyaw(Eigen::Quaterniond qua){
  tf::Quaternion q(qua.x(),
    qua.y(),
    qua.z(),
    qua.w());
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  return yaw*180.0/M_PI;
}

4 变换矩阵、旋转矩阵/四元数、平移向量

四元数+平移向量构造变换矩阵

      Eigen::Quaterniond q_odom_curr0;//四元数
      Eigen::Isometry3d T_1=Eigen::Isometry3d::Identity(); //变换矩阵
      T_1.rotate ( q_odom_curr0.toRotationMatrix() );  
      T_1.pretranslate ( Eigen::Vector3d ( 0,0,0 ) ); 

 变换矩阵转旋转向量/四元数+平移向量

      Eigen::Matrix3d rotation_matrix;//旋转矩阵
      rotation_matrix = T_1.rotation();
      t_odom_curr_now = T_1.translation();//t_odom_curr_now 平移向量
      q_odom_curr_now = Eigen::Quaterniond ( rotation_matrix );//转为四元数

5 四元数转欧拉角度--欧拉角转四元数

见：无人驾驶学习（二）：定位与导航_biter0088的博客-CSDN博客_无人驾驶定位

 有c++代码

 

参考链接：

讲解四元数、旋转矩阵、欧拉角的转换，很不错：

https://zhuanlan.zhihu.com/p/259999988

讲解了Eigen与矩阵相关的函数和类，也很不错

https://blog.csdn.net/weixin_31707171/article/details/112363668

eigen矩阵索引：

https://blog.csdn.net/xiaoyucyt/article/details/87866445

四元数、旋转矩阵、平移向量、变换矩阵等：

https://blog.csdn.net/HERO_CJN/article/details/82426372

https://blog.csdn.net/qq_40081208/article/details/102937581

https://www.cnblogs.com/jerry323/p/9097264.html

@meng