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

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