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

biter0088

已于 2022-09-11 12:06:22 修改

阅读量1.6w

点赞数 9

分类专栏： c++学习文章标签：矩阵线性代数 eigen c++

于 2022-05-11 21:02:29 首次发布

本文链接：https://blog.youkuaiyun.com/BIT_HXZ/article/details/124718532

版权

c++学习专栏收录该内容

13 篇文章

订阅专栏

本文介绍了如何在C++中使用Eigen库进行四元数到旋转矩阵的转换，以及旋转矩阵转回四元数，并探讨了四元数转欧拉角的方法。此外，还涉及到了变换矩阵、平移向量和相关操作，以及关键函数如normalize()和inverse()的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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 );//转为四元数