Eigen基础函数

参考

​​Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换​​

注意

1. Eigen中 四元数最后一个数为实部
2. 注意输出欧拉角的次序 2,1,0代表绕x-y-z轴旋转的角度

代码

旋转

原始固定坐标系下的旋转次序是x-y-z，也就是z-y-x*P;

现在做当前坐标系下的旋转，次序为P*x-y-z;

根据性质

当前坐标系下的旋转Px-y-z就等价于固定坐标系下的反方向的旋转x-y-zP
所以也就有这段程序，

//    当前坐标系下的旋转，往后乘的方向是P*x-y-z，等价于以一个相反的方向往前乘以x-y-z*P，以四元数形式做运算
cout<<"(1,2,3)在当前坐标系下的旋转后的点\n"<<(q_x*q_y*q_z*P).transpose()<<endl;
Eigen::Vector3d P3 = (q_x*q_y*q_z*P)+t;
cout<<"(1,2,3)旋转+平移 \n"<<P3.transpose()<<endl;

完整代码

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

using namespace std;


int main(int argc, char const *argv[])
{
    cout<<"---------------start-------------"<<endl;

    // 初始化欧拉角 按照Z-Y-X排序，题目中给定的是xyz排序
    Eigen::Vector3d eulerAngle(M_PI/18,M_PI/9,M_PI/6);


    // 欧拉角转轴角
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
    // 对应的四元数
    Eigen::Quaterniond q_x=Eigen::Quaterniond (rollAngle);
    cout<<"q_x: \n"<<q_x.coeffs().transpose()<<endl;
    cout<<"q_x Matrix: \n"<<q_x.matrix()<<endl;  // 四元数转旋转矩阵


//    cout<<"eulerAngle(2)" <<eulerAngle(2)<<endl;
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::Quaterniond q_y=Eigen::Quaterniond (pitchAngle);
    cout<<"q_y: \n"<<q_y.coeffs().transpose()<<endl;
    cout<<"q_y Matrix: \n"<<q_y.matrix()<<endl;  // 四元数转旋转矩阵



    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond q_z=Eigen::Quaterniond (yawAngle);
    cout<<"q_z: \n"<<q_z.coeffs().transpose()<<endl;
    cout<<"q_z Matrix: \n"<<q_z.matrix()<<endl;  // 四元数转旋转矩阵


    Eigen::AngleAxisd rotation_vector;
    rotation_vector = yawAngle*pitchAngle*rollAngle;
    cout<<"rotation matrix"<<endl;
        cout<<rotation_vector.matrix()<<endl;

    //    axisAngle
    cout<<"---------------------"<<endl;
    cout<<"axis"<<endl;
    cout<<rotation_vector.axis()<<endl;
    cout<<"angle"<<endl;
    cout<<rotation_vector.angle()/3.1415*180<<endl;

    //    四元数 last number is w.
    Eigen::Quaterniond q=Eigen::Quaterniond (rotation_vector);
    cout<<"---------------------"<<endl;
    cout<<"Quaternion"<<endl;
    cout<<q.coeffs().transpose()<<endl;

    // Quaternion to rotation matrix
    cout<<"---------------------"<<endl;
    Eigen::Matrix3d rotation_matrix(q);
    cout<<"rotation matrix"<<"\n"<<rotation_matrix<<endl;

    //    Quaternion to eulerAngle
    cout<<"---------------------"<<endl;
    Eigen::Vector3d euler_angle=q.matrix().eulerAngles(2,1,0);  // x y z
//    cout<<"euler_angle"<<"\n"<<euler_angle/3.1415*180<<endl;
    cout<<"euler_angle Z-Y-X"<<"\n"<<euler_angle.transpose()/3.1415*180<<endl;


    // Rotation with P(1,2,3)
    cout<<"---------------------"<<endl;
    Eigen::Vector3d P(1,2,3);
    cout<<"P:"<<"\n"<<P.transpose()<<endl;
    cout<<"(1,2,3) after rotation with rotation_matrix:"<<"\n"<<(rotation_matrix*P).transpose()<<endl;
    cout<<"(1,2,3) after rotation with axisAngle:"<<"\n"<<(rotation_vector*P).transpose()<<endl;

    // Transfrom with P  固定坐标系下，往前乘
    cout<<"---------------------"<<endl;
    Eigen::Isometry3d T(Eigen::Isometry3d::Identity());
//    cout<<"T \n"<<T.matrix()<<endl;

    T.rotate(rotation_matrix);
    Eigen::Vector3d t=Eigen::Vector3d(3,2,1);  // 平移向量
    T.pretranslate(t);

    cout<<"new T\n"<<T.matrix()<<endl;

    Eigen::Vector3d P2 = T*P;
    cout<<"(1,2,3) after Transfrom with T:\n"<<P2.transpose()<<endl;


    //    当前坐标系下的旋转，往后乘的方向是P*x-y-z，等价于以一个相反的方向往前乘以x-y-z*P，以四元数形式做运算
    cout<<"(1,2,3)在当前坐标系下的旋转后的点\n"<<(q_x*q_y*q_z*P).transpose()<<endl;
    Eigen::Vector3d P3 = (q_x*q_y*q_z*P)+t;
    cout<<"(1,2,3)旋转+平移 \n"<<P3.transpose()<<endl;


    return 0;
}