标签：std Matrix Vector3d Eigen C++ 使用 include 3d

#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;
}

标签：std,Matrix,Vector3d,Eigen,C++,使用,include,3d
来源： https://www.cnblogs.com/AI-ZZH/p/15861367.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。