/*
* create 2021/12/30
* 四元素 欧拉角 旋转矩阵 旋转向量之间的转换
* 其中四元素需要单独读出 x y z w 输出,其他都可以直接cout
*/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
#define PI (3.1415926535897932346f)
int main(int argc, char **argv)
{
// 定义一个四元素
Eigen::Quaterniond q(0.8, 0.6, 0.0, 0.0);
// 定义一个欧拉角表示RPY
Eigen::Vector3d eulerAngle(1,0, 0.0, 0.0);
// 定义一个旋转矩阵R
Eigen::Matrix3d R;
R << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
// 定义一个旋转向量(旋转角度和旋转轴)
Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0, 0, 1));
//【四元素】==========================================
//--1. 四元素转欧拉角
Eigen::Vector3d eulerAngle1;
eulerAngle1 = q.matrix().eulerAngles(2,1,0);
//--2. 四元素转旋转矩阵
Eigen::Matrix3d R1;
R1 = q.matrix();
//--3. 四元素转旋转向量
Eigen::AngleAxisd rotation_vector1(q);
//【欧拉角】==========================================
//--1. 欧拉角转四元素
Eigen::Quaterniond q1;
q1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
//--2. 欧拉角转旋转矩阵
Eigen::Matrix3d R1;
R1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
//--3. 欧拉角转旋转向量
Eigen::AngleAxisd rotation_vector2;
rotation_vector2 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
//【旋转矩阵】==========================================
//--1. 旋转矩阵转欧拉角
Eigen::Vector3d eulerAngle2;
eulerAngle2 = R.eulerAngles(2,1,0);
//--2. 旋转矩阵转四元素
// 直接赋值操作
Eigen::Quaterniond q2(R);
//--3. 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector3;
rotation_vector3.fromRotationMatrix(R);
//【旋转向量】==========================================
//--1. 旋转向量转欧拉角
Eigen::Vector3d eulerAngle3;
eulerAngle2 = rotation_vector.matrix().eulerAngles(2,1,0);
//--2. 旋转向量转四元素
// 直接赋值操作
Eigen::Quaterniond q3(rotation_vector);
//--3. 旋转向量转旋转矩阵
Eigen::Matrix3d R3;
R3 = rotation_vector.matrix();
R3 = rotation_vector.toRotationMatrix();
return 0;
}
【四元素 欧拉角 旋转矩阵 旋转向量】四种表示方法间的转换关系
于 2021-12-30 17:33:58 首次发布