C++ Eigen 库的使用

2022/2/3 14:12:54

本文主要是介绍C++ Eigen 库的使用,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!

#include "ros/ros.h"
#include<Eigen/Core>
#include<Eigen/Geometry>
// using namespace std; https://www.cnblogs.com/lovebay/p/11215028.html  https://blog.csdn.net/u011092188/article/details/77430988/
// using namespace Eigen;
double m_PI = 3.14159265358979323;
double x,y,z,w;



void test01() // 基本类型 定义
{
    Eigen::Matrix<float,2,3> matrix_23; // Eigen 中所有向量和矩阵 都是 Eigen::Matrix  这是一个模板类 前三个参数 为 数据类型 行 列

    Eigen::Vector3d v_3d ;  // 同时,  Eigen 通过 typedf 提供了许多内置类型  底层依然是 Eigen::Matrix    列向量

    Eigen::Matrix<double,3,1> vd_3d; // v_3d yu vd_3d 等价    Eigen::Vector3d v_3d   是double 类型

    std::cout << "Vector3d " << std::endl << Eigen::Vector3d::UnitX() << std::endl; // (1,0,0) 列向量


    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Identity(); // 代表double 类型的 3*3 的矩阵    单位矩阵

    Eigen::MatrixXd matrix_xx; // 代表double 类型的 不确定维度的矩阵
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;

    std::cout << "Rotation Matrix" << std::endl << matrix_33 << std::endl;


    Eigen::Matrix3cd matrix_r; //旋转矩阵
    Eigen::AngleAxisd vector_rotation;  // 旋转向量

    
    Eigen::Vector3d translation; // 平移向量

    Eigen::Vector3d Eulerangler;  // 欧拉角
    Eigen::Quaterniond q1;  // 四元数

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();  //变换矩阵 定义与初始化
    std::cout << "Transform Matrix" << std::endl << T.matrix() << std::endl;  // 4*4 单位矩阵
}

void test02()  // 旋转向量
{
    // 旋转向量  旋转向量,是一个三维向量,其方向与旋转轴一致,长度等于旋转角; 任意的旋转都可以用一个旋转轴和绕轴的旋转角来描述,简称“轴角”(Axis-Angle);
    // https://zhuanlan.zhihu.com/p/93563218
    // https://blog.csdn.net/xuewend/article/details/84645213


    Eigen::AngleAxisd t_V(M_PI / 3, Eigen::Vector3d(0, 0, 1));  // 旋转向量 
    Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵              
    Eigen::Quaterniond t_Q(t_V);   //  四元数      
    Eigen::Vector3d eulerAngle(0, M_PI/6, M_PI); // 欧拉角 yaw pitch roll  z y x              


    //对旋转向量(轴角)赋值的三大种方法

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

        //2.使用旋转矩阵转旋转向量的方式
        Eigen::AngleAxisd V2(t_R);  //2.3 使用旋转矩阵来对旋转向量进行初始化
        V2.fromRotationMatrix(t_R); //2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值(注意此方法为旋转向量独有,四元数没有)
        V2 = t_R;                   //2.2 直接使用旋转矩阵来对旋转向量赋值
        std::cout << "Rotation_vector2 " << std::endl << V2.matrix() << std::endl;  

        //3. 使用四元数来对旋转向量进行赋值
        Eigen::AngleAxisd V3(t_Q); //3.2 使用四元数来对旋转向量进行初始化
        V3 = t_Q;                  //3.1 直接使用四元数来对旋转向量赋值
        std::cout << "Rotation_vector3 " << std::endl << V3.matrix() << std::endl;  

        //4. 欧拉角给旋转向量赋值
        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ())); 
        Eigen::AngleAxisd V4;
        V4 =yawAngle*pitchAngle*rollAngle;
        std::cout << "Rotation_vector4 " << std::endl << V4.matrix() << std::endl;  
}

void test03() // 四元数 的 赋值 及元素访问
{
    Eigen::AngleAxisd t_V(M_PI / 3, Eigen::Vector3d(0, 0, 1));  // 旋转向量 
    Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵              
    Eigen::Quaterniond t_Q(t_V);   //  四元数   
    Eigen::Vector3d eulerAngle(0, M_PI/6, M_PI); // 欧拉角 yaw pitch roll  z y x      
    
    // 对四元数赋值

    //1.使用旋转的角度A和旋转轴向量n(此向量为单位向量)来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
    double A = M_PI/4;
    Eigen::Quaterniond q1(cos(A/2),0*sin(A/2),0*sin(A/2),1*sin(A/2)); // Eigen::Quaterniond q1(w,x,y,z);  // 四元数的初始化
    std::cout << "Rotation_q1 " << std::endl << q1.coeffs() << std::endl;    // x y z w
    std::cout << "Rotation_q1 " << std::endl << q1.x() << ", " << q1.y() << ", " << q1.z() << ", " << q1.w() << std::endl;   

    //2. 使用旋转矩阵转四元數的方式
    Eigen::Quaterniond q2(t_R); // 2.2 使用旋转矩阵来对四元數进行初始化
    q2 = t_R; //2.1 直接使用旋转矩阵来对旋转向量赋值
    std::cout << "Rotation_q2 " << std::endl << q2.coeffs() << std::endl;   


    //3. 使用旋转向量对四元数来进行赋值
    Eigen::Quaterniond q3(t_V); // 2.2 使用旋转向量来对四元數进行初始化
    q3 = t_V; //2.1 直接使用旋转向量来对旋转向量赋值
    std::cout << "Rotation_q3 " << std::endl << q3.coeffs() << std::endl;   
    std::cout << "Rotation_q3 Inverse" << std::endl << q3.inverse().coeffs() << std::endl;   


    // 4. 欧拉角转四元数   借用 旋转向量
    Eigen::AngleAxisd rollangle(eulerAngle(2), Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchangle(eulerAngle(1), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawangle(eulerAngle(0), Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond q4 = yawangle*pitchangle*rollangle;
    q4.normalize();
    std::cout << "Rotation_q4 " << std::endl << q4.coeffs() << std::endl;   

    // Eigen::Quaterniond q1(w,x,y,z);  // 四元数的初始化
    // Eigen::Quaterniond q2(Eigen::Vector4d(w,x,y,z));  // 向量
    // Eigen::Quaterniond q3(Eigen::Matrix3d(t_R));
    // Eigen::Quaterniond q3(Eigen::AngleAxisd(t_R));

    // q1.toRotationMatrix(); // 转换成 旋转矩阵 
}

void test04() // 旋转矩阵 的 赋值
{
    Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1));  // 旋转向量 
    Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵              
    Eigen::Quaterniond t_Q(t_V);   //  四元数    
    Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角  yaw pitch roll  z y x   飞机的头部是x轴正方向

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

    R1<<1,2,3,4,5,6,7,8,9; // 由左至右 按行输入
    std::cout << "Rotation_matrix1" << std::endl << R1 << std::endl;  // 3*3 单位矩阵

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

    //3. 使用四元数转旋转矩阵来对旋转矩阵赋值
    Eigen::Matrix3d R3; 
    R3 = t_Q.matrix(); // 使用四元数的成员函数matrix()来对旋转矩阵赋值
    R3 = t_Q.toRotationMatrix(); // 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
    R3 = t_Q;
    std::cout << "Rotation_matrix3" << std::endl << R3 << std::endl;  // 

    std::cout << "Rotation_matrix3 Inverse" << std::endl << R3.inverse() << std::endl;  // 求旋转矩阵的逆变换 

    std::cout << "R3(0,0) " <<  R3(0,0)<< std::endl ;

    //4. 使用 欧拉角 来为旋转矩阵赋值
    Eigen::Matrix3d R4;
    Eigen::AngleAxisd rollAngle(eulerAngle(2),Eigen::Vector3d(1,0,0));
    Eigen::AngleAxisd pitchAngle(eulerAngle(1),Eigen::Vector3d(0,1,0));
    Eigen::AngleAxisd yawAngle(eulerAngle(0),Eigen::Vector3d(0,0,1));
    R4 = yawAngle*pitchAngle*rollAngle;
    std::cout << "Rotation_matrix4" << std::endl << R4 << std::endl;  // 
}

void test05() // 欧拉角
{
    Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1));  // 旋转向量 
    Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵              
    Eigen::Quaterniond t_Q(t_V);   //  四元数    
    Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角  yaw pitch roll  z y x   飞机的头部是x轴正方向

    // 旋转矩阵 转欧拉角
    Eigen::Vector3d eulerAngle1 = t_R.eulerAngles(2,1,0);
    std::cout << "Rotation_angle1 " << std::endl << eulerAngle1(0) << " " << eulerAngle1(1) << " " << eulerAngle1(2) << " " << std::endl;  

    // 旋转向量 转欧拉角
    Eigen::Vector3d eulerAngle2 = t_V.matrix().eulerAngles(2,1,0);
    std::cout << "Rotation_angle2 " << std::endl << eulerAngle2(0) << " " << eulerAngle2(1) << " " << eulerAngle2(2) << " " << std::endl;  

    // 四元数 转 欧拉角
    Eigen::Vector3d eulerAngle3 = t_Q.matrix().eulerAngles(2,1,0);
    std::cout << "Rotation_angle3 " << std::endl << eulerAngle3(0) << " " << eulerAngle3(1) << " " << eulerAngle3(2) << " " << std::endl;  

}

void test06() // 变换矩阵
{
    Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1));  // 旋转向量 
    Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵              
    Eigen::Quaterniond t_Q(t_V);   //  四元数    
    Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角  yaw pitch roll  z y x   飞机的头部是x轴正方向
    Eigen::Vector3d trans_p(0,2,2);

    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();  //变换矩阵 定义与初始化  虽然称为3d,实质上是4x4的矩阵(旋转R+平移t)
    std::cout << "Transform Matrix1" << std::endl << T1.matrix() << std::endl;  // 4*4 单位矩阵
    //T1.rotate(t_V);  // 通过旋转向量 为 变换矩阵的 旋转部分赋值    // 所有的旋转和平移都是以原坐标系为准
    //T1.rotate(t_R);  // 通过旋转矩阵 为 变换矩阵的 旋转部分赋值
    T1.rotate(t_Q); // 通过四元数 为 变换矩阵的 旋转部分赋值

    T1.pretranslate(trans_p); // 通过三维向量 为 变换矩阵的 平移部分赋值   // 顺序可以颠倒   

    std::cout << "Transform Matrix1" << std::endl << T1.matrix() << std::endl;  // 
    std::cout << "Transform Matrix1 Inverse" << std::endl << T1.inverse().matrix() << std::endl;  //求变换矩阵的逆,返回一个变换矩阵 对原矩阵无影响
 
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    //T2.linear() = t_R;//通过旋转矩阵 为 变换矩阵的 旋转部分赋值
    T2.linear() << 1,0,0,0,1,0,0,0,1;
    T2.translation() = trans_p;
    std::cout << "Transform Matrix2" << std::endl << T2.matrix() << std::endl;  // 
    std::cout << "Transform Matrix2 Rotation" << std::endl << T2.linear().matrix() << std::endl;  // 
    std::cout << "Transform Matrix2 translation" << std::endl << T2.translation().matrix() << std::endl;  // 
    std::cout << "T*(1,2,3) " << std::endl << (T2*Eigen::Vector3cd(1,2,3)) << std::endl;  // //相当于R21*v+t21,隐含齐次坐
}

void test07() // 已知 两个坐标系在全局坐标系的位姿 求其中一个在另一个坐标系下的位姿   求t1 在 t2 下的位姿
{
    Eigen::Vector3d eulerAngle(0,0,0);
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ())); 
    Eigen::Quaterniond q_1;
    q_1=yawAngle*pitchAngle*rollAngle;
    q_1.normalize();
    Eigen::Vector3d t_1 = Eigen::Vector3d(1,1,0);

    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    T1.rotate(q_1.toRotationMatrix());
    T1.pretranslate(t_1);
    //std::cout << "T1 " << std::endl <<  T1.matrix() << std::endl;

    Eigen::Vector3d eulerAngle2(M_PI,0,0);  // 第一个是z轴
    Eigen::AngleAxisd rollAngle2(Eigen::AngleAxisd(eulerAngle2(2),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle2(Eigen::AngleAxisd(eulerAngle2(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle2(Eigen::AngleAxisd(eulerAngle2(0),Eigen::Vector3d::UnitZ())); 
    Eigen::Quaterniond q_2;
    q_2=yawAngle2*pitchAngle2*rollAngle2;
    q_2.normalize();
    Eigen::Vector3d t_2 = Eigen::Vector3d(0,0, 0);
        
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate(q_2.toRotationMatrix());
    T2.pretranslate(t_2);
    //std::cout << "T " <<  T.matrix() << std::endl;
    Eigen::Isometry3d T_inv = T2.inverse();
    //std::cout << "T_inv " << std::endl <<  T_inv.matrix() << std::endl;


    Eigen::Isometry3d T =T_inv*T1;

    Eigen::Matrix3d t_Relate;
    t_Relate << T(0,0),T(0,1),T(0,2),
                T(1,0),T(1,1),T(1,2),
                T(2,0),T(2,1),T(2,2);

    std::cout << "T " << std::endl <<  T.matrix() << std::endl;

    std::cout << "t_Relate " << std::endl <<  t_Relate << std::endl;
}


void test08()  //  对 一个四元数 作 一个欧拉角的旋转变换
{
    Eigen::Vector3d eulerAngle(0,m_PI,0);// 创建 一个 欧拉角 zyx

    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));  // 将欧拉角 转换成 四元数
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ())); 
    Eigen::Quaterniond q_fix;
    q_fix=yawAngle*pitchAngle*rollAngle;
    q_fix.normalize();

    Eigen::Vector3d eulerAngle_tep = q_fix.matrix().eulerAngles(2,1,0);
    std::cout << eulerAngle_tep(2)*180/m_PI << " " << eulerAngle_tep(1)*180/m_PI << " " << eulerAngle_tep(0)*180/m_PI << " " << std::endl;

    
    Eigen::Quaterniond q(1,0,0,0);  // 旋转角度为 0 的四元数
    Eigen::Quaterniond q_new = q_fix*q; // q_fix*
    q_new.normalize();
    Eigen::Vector3d eulerAngle2 = q_new.matrix().eulerAngles(2,1,0);
    std::cout<< "@@@@@@@@@   " << eulerAngle2(2)*180/m_PI << " " << eulerAngle2(1)*180/m_PI << " " << eulerAngle2(0)*180/m_PI << " @@@" << std::endl;
}

void test09()// 常用运算
{
    // 旋转
    Eigen::Vector3d v1(1,1,0);  // 将 三维向量 绕z轴逆时针旋转 45度
    Eigen::AngleAxisd Angle_axis1(M_PI/4,Eigen::Vector3d(0,0,1));
    Eigen::Vector3d rotated_v1 = Angle_axis1.matrix().inverse()*v1;  // 3x3  * 3x1

    std::cout << "rotate_matrix1 " << std::endl << Angle_axis1.matrix() << std::endl;  // 
    std::cout << "rotated_v1 " << std::endl << rotated_v1 << std::endl;  // 

    Eigen::Vector3d v2(0,0,0);  // 将 三维向量 绕y轴逆时针旋转 45度
    v2.x() = 2;
    v2[2] = 2;
    Eigen::AngleAxisd Angle_axis2(M_PI/4,Eigen::Vector3d(0,1,0)); 
    Eigen::Vector3d rotated_v2 = Angle_axis2.matrix().inverse()*v2;  // 3x3  * 3x1

    std::cout << "rotate_matrix2 " << std::endl << Angle_axis2.matrix() << std::endl;  // 
    std::cout << "rotated_v2 " << std::endl << rotated_v2 << std::endl;  // 

    
    Eigen::Vector3d v3(2,2,0);
    std::cout << "v3 模长 " <<  v3.norm() << std::endl;  // 求模长
    std::cout << "v3 单位向量 " << std::endl <<  v3.normalized() << std::endl; // 求单位向量

    Eigen::Vector3d v4(1,0,0), v5(0,1,0);
    std::cout << "(1,0,0) 点乘 (0,1,0)" << std::endl <<  v4.dot(v5) << std::endl;  // 点乘 0
    std::cout << "(1,0,0) * (0,1,0)" << std::endl <<  v4.transpose()*v5 << std::endl;  // 乘 维数要对应 0
    std::cout << "(1,0,0) 叉乘 (0,1,0)" << std::endl <<  v4.cross(v5) << std::endl;  // 叉乘  (0,0,1)

    Eigen::Matrix<double,4,4> T;
    T.Identity();
    Eigen::AngleAxisd angle_axis(M_PI/4,Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d R(angle_axis);

    Eigen::Vector3d t;
    t.setOnes(); // 所有分量为1
    //t.setZero(); // 所有分量为0
    
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = t; // <3,1> 是形状  (0,3)是左上角位置

    std::cout << "T 的旋转 " << std::endl <<  T.topLeftCorner(3,3) << std::endl;  
    std::cout << "T 的平移 " << std::endl <<  T.topRightCorner(3,1) << std::endl;  
    std::cout << "T block " << std::endl <<  T.block<3,3>(0,1) << std::endl;  //  可以输出 block

}

void test10()  // 欧拉角的逆 借助四元数 验证四元数的逆
{
    Eigen::Vector3d eulerAngle(0,M_PI/6,M_PI);
    std::cout << "eulerAngle " << std::endl << eulerAngle(0) << " " << eulerAngle(1) << " " << eulerAngle(2) << " " << std::endl;  
    Eigen::AngleAxisd rollangle(eulerAngle(2), Eigen::Vector3d::UnitX());  // 欧拉角转旋转向量
    Eigen::AngleAxisd pitchangle(eulerAngle(1), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawangle(eulerAngle(0), Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond q = yawangle*pitchangle*rollangle; // 旋转向量相乘 再转四元数
    q.normalize();
    std::cout << "Rotation_q " << std::endl << q.coeffs() << std::endl;   

    Eigen::Quaterniond q_inversed = q.inverse();  // 四元数求逆
    std::cout << "Rotation_q_inversed " << std::endl << q_inversed.coeffs() << std::endl;   

    Eigen::Vector3d eulerAngle_inversed = q_inversed.matrix().eulerAngles(2,1,0); // 四元数转 旋转矩阵 转 欧拉角
    std::cout << "eulerAngle_inversed " << std::endl << eulerAngle_inversed(0) << " " << eulerAngle_inversed(1) << " " << eulerAngle_inversed(2) << " " << std::endl;  

}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"Eigen_test");
    ros::NodeHandle nh;
    ros::Rate rate(1);
    while(ros::ok())
    {
        //calrotation();
        //test01();
        //test02();
        //test03();
        //test04();
        //test05();
        //test06();
        //test09();
        test10();
        rate.sleep();

    }
    return 0;
}

 



这篇关于C++ Eigen 库的使用的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!


扫一扫关注最新编程教程