Eigen/Geometry 模块

最新推荐文章于 2025-06-10 14:45:42 发布

锅碗瓢盆雷阵雨

最新推荐文章于 2025-06-10 14:45:42 发布

阅读量1.9k

点赞数

分类专栏： Eigen

Eigen 专栏收录该内容

2 篇文章

订阅专栏

本文详细介绍了Eigen库中Geometry模块的各种旋转和平移表示方法及其相互转换，包括旋转矩阵、旋转向量、四元数和欧拉角等，并给出了具体的C++实现代码。

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

Eigen/Geometry 模块提供了各种旋转和平移的表示

一、表示：

旋转矩阵直接使用 Matrix3d 或 Matrix3f：

Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();//初始化为一个单位阵。

旋转向量使用 AngleAxis：

Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1)); //角+轴：沿 Z 轴旋转 45 度

欧拉角：

Eigen::Vector3d ea0(yaw,pitching,droll);

二、转化：

旋转向量->旋转矩阵：rotationMatrix=rotation_vector.toRotationMatrix();

旋转向量->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );

旋转矩阵->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_matrix );

四元素->旋转矩阵：Eigen::Matrix3d Rx = q.toRotationMatrix();

旋转向量->欧拉角：Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);

旋转矩阵->欧拉角：Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序

三、C++代码


   
     
      
     
     
      
       #include <iostream> 
      
     

     
      
     
     
      
       #include <Eigen/Eigen> 
      
     

     
      
     
     
      
       #include <stdlib.h> 
      
     

     
      
     
     
      
       #include <Eigen/Geometry> 
      
     

     
      
     
     
      
       #include <Eigen/Core> 
      
     

     
      
     
     
      
       #include <vector> 
      
     

     
      
     
     
      
       #include <math.h> 
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       using 
       namespace 
       std;  
      
     

     
      
     
     
      
       using 
       namespace Eigen;  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
           Eigen::Quaterniond q = rollAngle  yawAngle  pitchAngle;  
      
     

     
      
     
     
          
       cout << “Euler2Quaternion result is:” <<
       endl;  
      
     

     
      
     
     
          
       cout << ”x = ” << q.x() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”y = ” << q.y() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”z = ” << q.z() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”w = ” << q.w() <<
       endl<<
       endl;  
      
     

     
      
     
     
          
       return q;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::Quaterniond q;  
      
     

     
      
     
     
      
           q.x() = x;  
      
     

     
      
     
     
      
           q.y() = y;  
      
     

     
      
     
     
      
           q.z() = z;  
      
     

     
      
     
     
      
           q.w() = w;  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
           Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(
       2, 
       1, 
       0);  
      
     

     
      
     
     
          
       cout << “Quaterniond2Euler result is:” <<
       endl;  
      
     

     
      
     
     
          
       cout << ”x = ”<< euler[
       2] << 
       endl ;  
      
     

     
      
     
     
          
       cout << ”y = ”<< euler[
       1] << 
       endl ;  
      
     

     
      
     
     
          
       cout << ”z = ”<< euler[
       0] << 
       endl << 
       endl;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::Quaterniond q;  
      
     

     
      
     
     
      
           q.x() = x;  
      
     

     
      
     
     
      
           q.y() = y;  
      
     

     
      
     
     
      
           q.z() = z;  
      
     

     
      
     
     
      
           q.w() = w;  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
           Eigen::Matrix3d R = q.normalized().toRotationMatrix();  
      
     

     
      
     
     
          
       cout << “Quaternion2RotationMatrix result is:” <<
       endl;  
      
     

     
      
     
     
          
       cout << ”R = ” << 
       endl << R << 
       endl<< 
       endl;  
      
     

     
      
     
     
          
       return R;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::Quaterniond q = Eigen::Quaterniond(R);  
      
     

     
      
     
     
      
           q.normalize();  
      
     

     
      
     
     
          
       cout << “RotationMatrix2Quaterniond result is:” <<
       endl;  
      
     

     
      
     
     
          
       cout << ”x = ” << q.x() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”y = ” << q.y() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”z = ” << q.z() <<
       endl;  
      
     

     
      
     
     
          
       cout << ”w = ” << q.w() <<
       endl<<
       endl;  
      
     

     
      
     
     
          
       return q;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  
      
     

     
      
     
     
      
           Eigen::
       AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
           Eigen::Quaterniond q = rollAngle  yawAngle  pitchAngle;  
      
     

     
      
     
     
      
           Eigen::Matrix3d R = q.matrix();  
      
     

     
      
     
     
          
       cout << “Euler2RotationMatrix result is:” <<
       endl;  
      
     

     
      
     
     
          
       cout << ”R = ” << 
       endl << R << 
       endl<<
       endl;  
      
     

     
      
     
     
          
       return R;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       Eigen::
       Vector3d RotationMatrix2euler(Eigen::Matrix3d R)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
      
           Eigen::Matrix3d m;  
      
     

     
      
     
     
      
           m = R;  
      
     

     
      
     
     
      
           Eigen::Vector3d euler = m.eulerAngles(
       0, 
       1, 
       2);  
      
     

     
      
     
     
          
       cout << “RotationMatrix2euler result is:” << 
       endl;  
      
     

     
      
     
     
          
       cout << ”x = ”<< euler[
       2] << 
       endl ;  
      
     

     
      
     
     
          
       cout << ”y = ”<< euler[
       1] << 
       endl ;  
      
     

     
      
     
     
          
       cout << ”z = ”<< euler[
       0] << 
       endl << 
       endl;  
      
     

     
      
     
     
          
       return euler;  
      
     

     
      
     
     
      
       }  
      
     

     
      
     
     
        
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       int main(int argc, char **argv)  
      
     

     
      
     
     
      
       {  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is euler2Quaternion transform function,please input your euler angle//  
      
     

     
      
     
     
      
         euler2Quaternion(
       0,
       0,
       0);  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is Quaternion2Euler transform function,please input your euler angle//  
      
     

     
      
     
     
      
         Quaterniond2Euler(
       0,
       0,
       0,
       1);  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is Quaternion2RotationMatrix transform function,please input your Quaternion parameter//  
      
     

     
      
     
     
      
         Quaternion2RotationMatrix(
       0,
       0,
       0,
       1);  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is rotationMatrix2Euler transform function,please input your RotationMatrix parameter like following//  
      
     

     
      
     
     
      
         Eigen::Vector3d x_axiz,y_axiz,z_axiz;  
      
     

     
      
     
     
      
         x_axiz << 
       1,
       0,
       0;  
      
     

     
      
     
     
      
         y_axiz << 
       0,
       1,
       0;  
      
     

     
      
     
     
      
         z_axiz << 
       0,
       0,
       1;  
      
     

     
      
     
     
      
         Eigen::Matrix3d R;  
      
     

     
      
     
     
      
         R << x_axiz,y_axiz,z_axiz;  
      
     

     
      
     
     
      
         rotationMatrix2Quaterniond(R);  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is euler2RotationMatrix transform function,please input your euler angle for the function parameter//  
      
     

     
      
     
     
      
         euler2RotationMatrix(
       0,
       0,
       0);  
      
     

     
      
     
     
        
      
     

     
      
     
     
      
       //this is RotationMatrix2euler transform function,please input your euler angle for the function parameter//  
      
     

     
      
     
     
      
         RotationMatrix2euler(R);  
      
     

     
      
     
     
        
      
     

     
      
     
     
        
       cout << “All transform is done!” << 
       endl;  
      
     

     
      
     
     
      
       }