欧拉角、旋转矩阵、四元数

参考的博客：
https://blog.csdn.net/yang__jing/article/details/82316093
https://blog.csdn.net/u011092188/article/details/77430988

Eigen库变换

旋转矩阵（3X3）:Eigen::Matrix3d
旋转向量（3X1）:Eigen::AngleAxisd
四元数（4X1）: Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
变换矩阵（4X4）:Eigen::Isometry3d

一.旋转矩阵

1.初始赋值：

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2.旋转矩阵转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

3.旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

二.欧拉角

1.初始化欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

2.欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

三.四元数

1. 初始化四元数

Eigen::Quaterniond quaternion(w,x,y,z);

2. 四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

3. 四元数转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);