Eigen 四元数、欧拉角、旋转矩阵、旋转向量相互转换

一、旋转向量

1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)

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

1.1 旋转向量转旋转矩阵

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

inline Eigen::Matrix3d rotationVectorToRotationMatrix(const Eigen::Vector3d& rotation_vector)
{
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix << 1, -rotation_vector(2), rotation_vector(1), 
                       rotation_vector(2), 1, -rotation_vector(0),
                       -rotation_vector(1), rotation_vector(0), 1;
    return rotation_matrix;
}

Eigen::Vector3d delta_theta = Eigen::Vector3d::Zero();
Eigen::Matrix3d rotation_delta_base_imu = Eigen::Matrix3d::Identity();
rotation_delta_base_imu = rotationVectorToRotationMatrix(delta_theta);

Eigen::AngleAxisd angle_axisd_delta_theta(delta_theta.norm(), delta_theta.normalized());
rotation_delta_base_imu = angle_axisd_delta_theta.matrix();

1.2 旋转向量转欧拉角(Z-Y-X，即RPY)

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

1.3 旋转向量转四元数

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


Eigen::AngleAxisd angle_axisd(v.norm(),v / v.norm());
Eigen::Quaterniond quaternion(angle_axisd);

二、旋转矩阵

2.0 初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix<

2.1 旋转矩阵转旋转向量

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

2.2 旋转向量转欧拉角(Z-Y-X，即RPY)

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

2.3 旋转向量转四元数

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

三、欧拉角

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

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

3.1 欧拉角转旋转向量

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

3.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.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;

四、四元数

4.0 初始化四元数

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

4.1 四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.2 四元数转旋转矩阵

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

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

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

eigen 中四元数、欧拉角、旋转矩阵、旋转向量