转载注明出处。
直接放代码
/*
* 本程序演示旋转向量,欧拉角,旋转矩阵,四元数之间的相互转换
* Created By OuyangZizhou on 2019.5.7
*/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
int main()
{
/*************分割线******************/
// 初始化旋转向量,绕z轴旋转45°
Eigen::AngleAxisd Rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1));
// 将旋转向量转化为旋转矩阵,用罗德里格斯公式
Eigen::Matrix3d Rotation_matrix = Rotation_vector.matrix();
// 旋转向量转换为四元数
Eigen::Quaterniond q(Rotation_vector);
// Eigen::Quaterniond q;
// q = Rotation_vector;
// 旋转向量转为欧拉角,顺序为z-y-x,也即yaw pitch roll
Eigen::Vector3d eulerAngle = Rotation_vector.matrix().eulerAngles(2,1,0);
/*************分割线******************/
// 初始化旋转矩阵
Eigen::Matrix3d R_matrix;
// 以下为一种初始化方法,先直接采用上面的旋转矩阵
// R_matrix << 1, 0, 0, 0, 1, 0, 0, 0, 1;
R_matrix = Rotation_matrix;
// 旋转矩阵转化为旋转向量
Eigen::AngleAxisd R_vector(R_matrix);
// R_vector = R_matrix;
// R_vector.fromRotationMatrix(R_matrix);
// 旋转矩阵转化为四元数
Eigen::Quaterniond q2(R_matrix);
// q2 = R_matrix;
// 旋转矩阵转化为欧拉角表示,欧拉角只能通过matrix表示出来
Eigen::Vector3d eulerAngle1 = R_matrix.eulerAngles(2,1,0);
/*************分割线******************/
// 初始化欧拉角, yaw, pitch, roll
Eigen::Vector3d eulerAngle2(M_PI/4,M_PI/5, M_PI/6);
// 欧拉角转换为旋转向量,分别计算三个分量,然后将其相乘起来
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vectorX;
rotation_vectorX = yawAngle*pitchAngle*rollAngle;
// 欧拉角转换为旋转矩阵,和旋转向量的计算同理
// Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
// Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
// Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
Eigen::Matrix3d R_matrix1;
R_matrix1 = yawAngle*pitchAngle*rollAngle;
// 欧拉角转换为四元数,和旋转向量的计算同理
// Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle2(2), Eigen::Vector3d::UnitX()));
// Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle2(1), Eigen::Vector3d::UnitY()));
// Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle2(0), Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond q3;
q3 = yawAngle*pitchAngle*rollAngle;
/*************分割线******************/
//初始化四元数 (w,x,y,z)
Eigen::Quaterniond q4(0.1, 0.2, 0.3, 0.4);
q4.normalize();// 要做这一步归一化,才能用来表示旋转
// 四元数到旋转向量
Eigen::AngleAxisd rotation_vector3(q4);
// rotation_vector3 = q4;
// cout << rotation_vector3.matrix() << endl;
// 四元数到旋转矩阵
Eigen::Matrix3d R_matrix4(q4);
R_matrix4 = q4;
// cout << R_matrix4;
// R_matrix4 = q4.matrix();
// R_matrix4 = q4.toRotationMatrix();
// 四元数到欧拉角(Z-Y-X)
Eigen::Vector3d eulerAngle3 = q4.matrix().eulerAngles(2,1,0);
return 0;
}
参考:https://blog.csdn.net/u011092188/article/details/77430988