Eigen集合模块学习（角轴，位姿，欧拉角，四元数）--参考SLAM十四讲3.6

#include <iostream>
using namespace std;
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <ctime>
#include <cmath>//欧拉角
#include <eigen3/Eigen/Geometry>
int main(int argc, char **argv)
{
    Eigen::Matrix3d rotation_matrix3d=Eigen::Matrix3d::Identity();//单位阵
    cout<<"rotation_matrix3d="<<rotation_matrix3d<<endl;
    Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
    cout.precision(5);
    //角轴表达
    rotation_matrix3d=rotation_vector.toRotationMatrix();//将角轴转换为矩阵
    cout<<"rotation_vector.matrix()="<<rotation_vector.matrix()<<endl;
    cout<<"rotation_matrix3d="<<rotation_matrix3d<<endl;
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated=rotation_vector*v;//旋转过后的向量
    cout<<"v_rotated="<<v_rotated.transpose()<<endl;
    //欧拉角
    Eigen::Vector3d euler_angles=rotation_matrix3d.eulerAngles(2,1,0);//ZYX顺序
    cout<<"euler_angles="<<180/M_PI*euler_angles.transpose()<<endl; //角度表示,本身是弧度表示
    //位姿变换
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//需要赋值为单位阵，后续好做旋转
    cout<<"T(Identity)="<<T.matrix()<<endl;
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"T="<<T.matrix()<<endl;
    
    Eigen::Vector3d v_transformed=T*v;//虽然T是四维的，但有运算符重载按照三维计算
    cout<<"v_transformed="<<v_transformed.transpose()<<endl;
    //四元数
    Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
    cout<<"q(四元数,传vector)="<<q.coeffs()<<endl;//顺序[nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2),cos(theta/2)]
    //Quaterniond也可以传rotationmatrix
    q=Eigen::Quaterniond(rotation_matrix3d);
    cout<<"q(传matrix)="<<q.coeffs()<<endl;
    v_rotated=q*v;//数学上是qvq',所以乘法有重载
    cout<<"v_rotated="<<v_rotated.transpose()<<endl;
    return 0;
}