Eigen实现欧拉角、四元数和旋转矩阵之间的变换

include相应的头文件

#include <Eigen/Geometry>

旋转矩阵和旋转向量的表示和声明及旋转

// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// 旋转向量使用 AngleAxis, 它底层不直接是 Matrix ,但运算可以当作矩阵(因为重载了运算符)
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); // 沿 Z 轴旋转 45 度
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用
matrix() 转换成矩阵
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

欧拉角:可以将旋转矩阵直接转换成欧拉角

Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX 顺序,即 yaw pitch roll
顺序
cou