Eigen：：关于旋转(旋转向量\旋转矩阵\四元数\欧拉角)的初始化和转换以及应用

Eigen：：关于旋转(旋转向量\旋转矩阵\四元数\欧拉角)的初始化和转换以及应用 

---

code:

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>


using namespace std;
using namespace Eigen;

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

    //下面三个变量作为下面演示的中间变量

    AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));    //旋转向量:绕z轴旋转45度
    Matrix3d t_R = t_V.matrix();                    //旋转向量--->旋转矩阵
    Quaterniond t_Q(t_V);                           //旋转向量--->四元数


    /***************************旋转向量的初始化***************************/

    /*******************************
     * 对旋转向量（轴角）赋值的三大种方法
     * 1.使用角度和矢量
     * 2.使用旋转矩阵
     * 3.使用四元数
     *******************************/

    ///1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
    AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度
    cout << "Rotation_vector1" << endl << V1.matrix() << endl;

    ///2.使用旋转矩阵转旋转向量的方式

    //2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
    AngleAxisd V2;
    V2.fromRotationMatrix(t_R);
    cout << "Rotation_vector2" << endl << V2.matrix() << endl;

    //2.2 直接使用旋转矩阵来对旋转向量赋值
    AngleAxisd V3;
    V3 = t_R;
    cout << "Rotation_vector3" << endl << V3.matrix() << endl;

    //2.3 使用旋转矩阵来对旋转向量进行初始化
    AngleAxisd V4(t_R);
    cout << "Rotation_vector4" << endl << V4.matrix() << endl;

    ///3. 使用四元数来对旋转向量进行赋值

    //3.1 直接使用四元数来对旋转向量赋值
    AngleAxisd V5;
    V5 = t_Q;
    cout << "Rotation_vector5" << endl << V5.matrix() << endl;

    //3.2 使用四元数来对旋转向量进行初始化
    AngleAxisd V6(t_Q);
    cout << "Rotation_vector6" << endl << V6.matrix() << endl;


    /***************************四元数的初始化***************************/

    /*******************************
     * 对四元数初始化的三大种方法
     * 1.使用四元数的值Quaterniond(Vector4d(x,y,z,w)) 或者 Quaterniond(w,x,y,z,w)
     * 2.使用旋转矩阵
     * 3.使用旋转向量
     *******************************/

    ///1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
    Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));
    //以（0,0,1）为旋转轴，旋转45度
    //第一种输出四元数的方式
    cout << "Quaternion1" << endl << Q1.coeffs() << endl;

    //第二种输出四元数的方式
    cout << Q1.x() << endl << endl;
    cout << Q1.y() << endl << endl;
    cout << Q1.z() << endl << endl;
    cout << Q1.w() << endl << endl;


    ///2. 使用旋转矩阵初始化四元數的方式

    //2.1 直接使用旋转矩阵来对旋转向量赋值
    Quaterniond Q2;
    Q2 = t_R;
    cout << "Quaternion2" << endl << Q2.coeffs() << endl;

    //2.2 使用旋转矩阵来对四元數进行初始化
    Quaterniond Q3(t_R);
    cout << "Quaternion3" << endl << Q3.coeffs() << endl;



    ///3. 使用旋转向量对四元数来进行赋值

    //3.1 直接使用旋转向量对四元数来赋值
    Quaterniond Q4;
    Q4 = t_V;
    cout << "Quaternion4" << endl << Q4.coeffs() << endl;

    //3.2 使用旋转向量来对四元数进行初始化
    Quaterniond Q5(t_V);
    cout << "Quaternion5" << endl << Q5.coeffs() << endl;



    /***************************旋转矩阵的初始化***************************/
    /*******************************
     * 对旋转矩阵初始化的三大种方法
     * 1.直接矩阵赋值
     * 2.使用旋转向量
     * 3.使用四元数
     *******************************/

    ///1.使用旋转矩阵的函数来初始化旋转矩阵
    Matrix3d R1=Matrix3d::Identity();
    cout << "Rotation_matrix1" << endl << R1 << endl;

    ///2. 使用旋转向量转旋转矩阵来对旋转矩阵赋值

    //2.1 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R2;
    R2 = t_V.matrix();
    cout << "Rotation_matrix2" << endl << R2 << endl;

    //2.2 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R3;
    R3 = t_V.toRotationMatrix();
    cout << "Rotation_matrix3" << endl << R3 << endl;

    //3. 使用四元数转旋转矩阵来对旋转矩阵赋值

    //3.1 使用四元数的成员函数matrix()来对旋转矩阵赋值
    Matrix3d R4;
    R4 = t_Q.matrix();
    cout << "Rotation_matrix4" << endl << R4 << endl;

    //3.2 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
    Matrix3d R5;
    R5 = t_Q.toRotationMatrix();
    cout << "Rotation_matrix5" << endl << R5 << endl;


    /***************************旋转向量\旋转矩阵\四元数====>欧拉角***************************/
    ///1.旋转向量===>欧拉角
    Vector3d eulerFromAngleAxisd=t_V.matrix().eulerAngles(2,1,0);   //(z,y,x)--(2,1,0)   (z,x,y)--(2,0,1)
    cout << "euler from 旋转向量:\n"<<(180)/(M_PI)*eulerFromAngleAxisd.transpose()<<endl;

    ///2.旋转矩阵===>欧拉角
    Vector3d eulerFromRotationMatrix=t_R.eulerAngles(2,1,0);
    cout << "euler from 旋转矩阵:\n"<<(180)/(M_PI)*eulerFromRotationMatrix.transpose()<<endl;

    ///3.四元数===>欧拉角
    Vector3d eulerFromQuaternion = t_Q.matrix().eulerAngles(2,1,0);
    cout << "euler from 四元数:\n"<<(180)/(M_PI)*eulerFromQuaternion.transpose()<<endl;

    /***************************应用举例***************************/
    /*******************************
     * 利用3次旋转构造一个姿态R
     * 1.绕z轴旋转45度
     * 2.绕y轴旋转70度
     * 3.绕x轴旋转30度
     *******************************/
    Vector3d rollpitchyaw((30.0/180.0)*M_PI,(70.0/180.0)*M_PI,(45.0/180.0)*M_PI);
    AngleAxisd v_z((45.0/180.0)*M_PI , Vector3d(0, 0, 1));
    AngleAxisd v_y((70.0/180.0)*M_PI , Vector3d(0, 1, 0));
    AngleAxisd v_x((30.0/180.0)*M_PI , Vector3d(1, 0, 0));

    //欧拉角为ZYX-321顺序的调用框架---------------------------------------
    /*******************************
     * 利用3次旋转构造一个姿态R
     * 1.绕z轴旋转45度
     * 2.绕y轴旋转70度
     * 3.绕x轴旋转30度
     *******************************/
    Matrix3d R_n2b_zyx=v_x.matrix()*v_y.matrix()*v_z.matrix();
    cout << "R_ZYX :\n"<<R_n2b_zyx<<endl;

    AngleAxisd total;
    total=v_x*v_y*v_z;

    Vector3d zyx_Euler_fromR=R_n2b_zyx.eulerAngles(0,1,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012,实际上这个编号跟乘法的顺序一致就可以了(从左向又右看的顺序)
    cout<< "zyx euler from Rotation [输出顺序为:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
    //------------------------------------------------------------

    //欧拉角为ZXY-312顺序的调用框架---------------------------------------
    /*******************************
     * 利用3次旋转构造一个姿态R
     * 1.绕z轴旋转45度
     * 2.绕x轴旋转30度
     * 3.绕y轴旋转70度
     *******************************/
    Matrix3d R_n2b_zxy=v_y.matrix()*v_x.matrix()*v_z.matrix();
    cout << "R_ZXY :\n"<<R_n2b_zxy<<endl;

    Vector3d zxy_Euler_fromR=R_n2b_zxy.eulerAngles(1,0,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012
    cout<< "zxy euler from Rotation [输出顺序为:y,x,z]:\n"<<(180)/(M_PI)*zxy_Euler_fromR.transpose()<<endl;
    //------------------------------------------------------------


    ///关于输出顺序: 就是做旋转矩阵的乘法的顺序
    //(ZYX-321):v_x.matrix()*v_y.matrix()*v_z.matrix() 对应 pitch,roll,yaw的输出顺序
    //(ZXY-312):v_y.matrix()*v_x.matrix()*v_z.matrix() 对应 roll,pitch,yaw的输出顺序

    return 0;

}

---

参考内容:

https://blog.csdn.net/weixin_38213410/article/details/89892336

https://blog.csdn.net/yang__jing/article/details/82316093

Eigen:关于转换成欧拉角的函数的说明:

https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#ga17994d2e81b723295f5bc3b1f862ed3b

#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/TransformStamped.h"
#include "eigen_conversions/eigen_msg.h"

int main(int argc, char **argv)
{
    ros::init(argc,argv,"test");

    tf2_ros::TransformBroadcaster tfb;

    geometry_msgs::TransformStamped tfs;

    ros::Rate r(100);

    while(ros::ok()){
        tfs.header.stamp = ros::Time::now();

        tfs.transform.translation.x=1;
        tfs.transform.translation.y=0;
        tfs.transform.translation.z=0;

        tfs.header.frame_id="map";
        tfs.child_frame_id="camera";

        // 1. map 坐标系沿 原map坐标系x轴顺时针旋转90度 (roll)
        // 2. map 坐标系沿 原map坐标系y轴顺时针旋转90度 (roll)
        // 3. map 坐标系沿 原map坐标系z轴顺时针旋转90度 (yaw)
        //=================================================
        tfs.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(-M_PI/2,M_PI/2,-M_PI/2);

        // 另外一种思路是： 以map坐标系为参考，camera姿态为 -M_PI/2 , 即 yaw角为 -90度
        //tfs.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-M_PI/2);

        std::cout<<tfs.transform.rotation.w<<" "<<tfs.transform.rotation.x<<" "<< tfs.transform.rotation.y<<" "
                <<tfs.transform.rotation.z<<std::endl;

        Eigen::Isometry3d transform;
        tf::transformMsgToEigen(tfs.transform,transform);

        ///
        /// tf::createQuaternionMsgFromRollPitchYaw(-M_PI/2,M_PI/2,-M_PI/2);
        /// 上面的操作，跟下面的等价
        ///     R_C_W = roll * yaw
        /// 即相当于将世界坐标系：
        /// 1. 沿 原map坐标系绕Z轴顺时针旋转90度 (yaw)
        /// 2. 得到的坐标系 沿 新的X轴顺时针旋转90度 (roll)
        /// 3. 得到camera坐标系
        /// 4. 将camera坐标系的点转换到世界坐标系：
        ///    p_w = R_C_W.inv()*p_c + t_W_C
        ///
        /// R_C_W.inv() 等价于 tfs.transform.rotation 表示从相机坐标系到map坐标系的旋转变换
        /// 


        Eigen::Matrix3d R_C_W=Eigen::Matrix3d::Identity();

        // 顺时针90度
        Eigen::Matrix3d yaw=Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).matrix();
        // 逆时针90度
        Eigen::Matrix3d pitch=Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d(0, 1, 0)).matrix();
        // 顺时针90度
        Eigen::Matrix3d roll=Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(1, 0, 0)).matrix();


        R_C_W = (roll) * (pitch) * (yaw);

        Eigen::Quaterniond q(R_C_W.transpose());
        std::cout<<"q: "<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<std::endl;


        Eigen::Vector3d point_in_camera(1,0,0);

//        Eigen::Vector3d point_in_map= transform* point_in_camera;

        Eigen::Vector3d point_in_map= R_C_W.transpose()* point_in_camera + Eigen::Vector3d(1,0,0);

//        std::cout<<point_in_map.transpose()<<std::endl;

        tfb.sendTransform(tfs);


        r.sleep();
    }

//    google::ShutdownGoogleLogging();
}