Eigen：旋转向量（Angle-Axis）转换为四元素和旋转矩阵（二）

#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;




#define PI 3.1415926535897932346f

int main(int argc, char** argv) 
{


    //3D旋转矩阵 定义为单位阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    //定义旋转向量  沿着z轴旋转45度
    Eigen::AngleAxisd rotation_vector(PI / 4, Eigen::Vector3d(0, 0, 1));
    //  隐含了 一个变换  angleaxisd

    /*
     * 旋转向量  -->  旋转矩阵
     * 旋转向量  -->  旋转矩阵  --->   欧拉角
     * 旋转向量  -->  四元数
     */
    cout << "旋转矩阵：" << endl;
    cout << rotation_vector.matrix() << endl;   //..............................rotation_vector   to   rotation_matrix(1)
    //旋转向量--->旋转矩阵

    rotation_matrix = rotation_vector.toRotationMatrix();
    cout << "旋转矩阵：" << endl;
    cout << rotation_matrix << endl;            //...............................rotation_vector to rotation_matrix(2)
    //旋转向量--->旋转矩阵




 







    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);  //....................rotation_vector  to Quaterniond
    cout << "四元数：" << endl;
    cout << q.coeffs() << endl;  // (x,y,z,w)
    //旋转向量--->四元数

/*
 * 旋转矩阵  -->  旋转向量
 * 旋转矩阵  --->   欧拉角
 * 旋转矩阵   --->   四元数
 */
    rotation_vector = rotation_matrix;   // ..................................rotation_matrix  to rotation_vector
    //旋转矩阵--->旋转向量  

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);  //.......rotation_matrix  to euler_angles .........attention:  ypr
    cout << "欧拉角：" << endl;
    cout << euler_angles.transpose() << endl;                                   //  ypr
    //旋转矩阵--->欧拉角



   
        
        
     

  
 

      // Eigen::Matrix3d rotation_matrix123 = Eigen::Matrix3d::Identity();

  /*  rotation_matrix << 0.22544, -0.929073, -0.2932,
        0.969474, 0.243721, -0.02682,
        0.096397, -0.27824, 0.95565;*/



    rotation_matrix << 0.194132, 0.97217, -0.131,
        -0.889, 0.1179, -0.442,
        0.414, 0.2024, 0.8873;







    q = Eigen::Quaterniond(rotation_matrix);  // .......................................rotation_matrix to Quaterniond
    cout << "四元数：" << endl;
    cout << q.coeffs() << endl;  // (x,y,z,w)
    //旋转矩阵--->四元数  

/*
 * 四元数--->旋转矩阵
 * 四元数--->旋转矩阵 --->欧拉角
 * 四元数--->旋转向量
 */
    cout << "旋转矩阵：" << endl;
    cout << q.toRotationMatrix() << endl;  // .............................................Quaterniond  to rotatin_matrix
    //四元数--->旋转矩阵   

    cout << "欧拉角：" << endl;
    cout << q.toRotationMatrix().eulerAngles(2, 1, 0); //......................................Quaterniond  to rotatin_matrix  to euler_angles  ....attention:  ypr
    //四元数--->旋转矩阵--->欧拉角

    cout << "旋转向量：" << endl;
    rotation_vector = q;             // ..................................Quaterniond  to rotation_vector
    //四元数--->旋转向量  

/*
 * 都是因为定义的不同，存在隐式转换，
 * Eigen::Matrix3d rotation_matrix, Eigen::AngleAxisd rotation_vector,Eigen::Quaterniond q
 * 欧拉角 --->  旋转向量
 * 欧拉角 --->  旋转矩阵
 * 欧拉角 --->  四元数
 *
 * 按照rpy旋转
 * tran.rotate(Eigen::AngleAxisf(FLAGS_roll / 180.0 * M_PI, Eigen::Vector3f::UnitX()) *
             Eigen::AngleAxisf(FLAGS_pitch / 180.0 * M_PI, Eigen::Vector3f::UnitY()) *
             Eigen::AngleAxisf(FLAGS_yaw / 180.0 * M_PI, Eigen::Vector3f::UnitZ()));
 */
 //  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX()));
 //  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY()));
 //  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ()));
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(0.362569 / 180.0 * PI, Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(-2.22152 / 180.0 * PI, Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(-88.5397 / 180.0 * PI, Eigen::Vector3d::UnitZ()));
    rotation_matrix = rollAngle * pitchAngle * yawAngle;//................euler_angles to rotation_matrix
    cout << "旋转矩阵：" << endl;
    cout << rotation_matrix << endl;
    rotation_vector = rollAngle * pitchAngle * yawAngle;//................euler_angles to rotation_vector
    cout << "旋转矩阵：" << endl;
    cout << rotation_vector.matrix() << endl;
    q = rollAngle * pitchAngle * yawAngle;//................euler_angles to Quaterniond
    cout << "四元数：" << endl;
    cout << q.coeffs() << endl;

}