【Eigen库使用】角轴、旋转矩阵、欧拉角、四元数转换

零、前言

在slam中经常用到的四种描述机器人orientation的变量，他们之间可以相互转化，使用Eigen库可以很容易的做到这一点，需要特别关注的是：欧拉角与其余量之间的转换关系：

1）首先要明确的是，必须要明确欧拉角的旋转次序，你可以选择RPY、YPR等方式，在相同的orientation下，旋转次序会影响欧拉角三分量的数值，也会影响欧拉角与其他旋转表示的转换关系，但是不会影响转换结果；也就是说，旋转次序是你自己根据习惯选择的，在把欧拉角转换为其他形式时，也要根据选择的次序使用对应的转换关系；

2）欧拉角转换为其他表示，一般使用旋转向量（角轴）作为过渡，即，首先将欧拉角三个分量变成角轴，再将三个角轴乘在一起（乘的顺序要和旋转次序相匹配）得到角轴，使用角轴可以很方便的过渡到旋转矩阵或四元数；

3）其他表示转换为欧拉角，使用的是eulerAngles(0,1,2)方法，其中012表示转换得到的欧拉角向量的旋转次序是XYZ即RPY，如果想得到YPR格式的欧拉角，使用eulerAngles(2,1,0);即可。

一、旋转向量

1.1 初始化旋转向量
旋转角为alpha(顺时针)，旋转轴为(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

1.2 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.3 旋转向量转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

1.4 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
Quaterniond quaternion;

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

二、旋转矩阵

2.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.2 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.3 旋转矩阵转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

2.4 旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

三、欧拉角

3.1 初始化欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle(roll,pitch,yaw);

3.2 欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));

Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAnglepitchAnglerollAngle;

3.3 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));

Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAnglepitchAnglerollAngle;

3.4 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));

Eigen::Quaterniond quaternion;
quaternion=yawAnglepitchAnglerollAngle;

四、四元数

4.1 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);

4.2 四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.3 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

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

4.4 四元数转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);