使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换

使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换 - 走看看

转载记录学习

一、旋转向量

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<

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=yawAngle*pitchAngle*rollAngle;

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=yawAngle*pitchAngle*rollAngle;

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=yawAngle*pitchAngle*rollAngle;

四、四元数

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);

绕固定坐标系转和绕当前坐标系旋转

//
// Created by qian on 2021/3/7.
//
#include "iostream"
#include 
#include 
#include 

using namespace Eigen;
using namespace std;
int main(int argc, char **argv){
    Vector3d t0(1,2,3);// 初始向量
    Vector3d t(3,2,1); // 平移向量
    const float angle_x=30.0, angle_y=20.0, angle_z=10.0;
    // *************绕XYZ为当前坐标系旋转轴************
    AngleAxisd rotation_X=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());
    //固定坐标系的 Y 轴先投影成 X 旋转后的旋转轴,再进行旋转
    AngleAxisd rotation_Y=AngleAxisd(angle_y/180.0*M_PI,rotation_X.inverse()*Vector3d::UnitY());
    //固定坐标系的 Z 轴先投影成 XY 旋转后的旋转轴,再进行旋转
    AngleAxisd rotation_Z=AngleAxisd(angle_z/180.0*M_PI,rotation_Y.inverse()*rotation_X.inverse()*Vector3d::UnitZ());
    // 旋转向量
    AngleAxisd rotation_vector1;
    rotation_vector1=rotation_X*rotation_Y*rotation_Z;
    cout<<"旋转角："<