【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）

矩阵的使用可参考系列博客：点击此处

原文链接：基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换。

也可以参考另一篇博客：eigen 中四元数、欧拉角、旋转矩阵、旋转向量。

在机器人学中经常会涉及到欧拉角,旋转矩阵,旋转向量,四元数之间的转换,因此基于eigen对变换关系进行实现,以便参考:
注意，代码中Eigen::AngleAxisd中使用的是必须是弧度radian，不是角度angle。所以，不要直接输入角度angle，需要先把角度angle转化为弧度radian。

#include <iostream>#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;#define PI (3.1415926535897932346f)int main(int argc, char **argv)
{/**** 1. 旋转向量 ****/cout << endl << "********** AngleAxis **********" << endl;//1.0 初始化旋转向量,沿Z轴旋转45度的旋转向量Eigen::AngleAxisd rotation_vector1 (M_PI/4, Eigen::Vector3d(0, 0, 1)); //1.1 旋转向量转换为旋转矩阵//旋转向量用matrix()转换成旋转矩阵Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity(); //单位矩阵初始化rotation_matrix1 = rotation_vector1.matrix();cout << "rotation matrix1 =\n" << rotation_matrix1 << endl;                //或者由罗德里格公式进行转换rotation_matrix1 = rotation_vector1.toRotationMatrix();cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; /*1.2 旋转向量转换为欧拉角*///将旋转向量转换为旋转矩阵,再由旋转矩阵转换为欧拉角,详见旋转矩阵转换为欧拉角Eigen::Vector3d eulerAngle1 = rotation_vector1.matrix().eulerAngles(2,1,0);cout << "eulerAngle1, z y x: " << eulerAngle1 << endl;/*1.3 旋转向量转四元数*/Eigen::Quaterniond quaternion1(rotation_vector1);//或者Eigen::Quaterniond quaternion1_1;quaternion1_1 = rotation_vector1;cout << "quaternion1 x: " << quaternion1.x() << endl;cout << "quaternion1 y: " << quaternion1.y() << endl;cout << "quaternion1 z: " << quaternion1.z() << endl;cout << "quaternion1 w: " << quaternion1.w() << endl;cout << "quaternion1_1 x: " << quaternion1_1.x() << endl;cout << "quaternion1_1 y: " << quaternion1_1.y() << endl;cout << "quaternion1_1 z: " << quaternion1_1.z() << endl;cout << "quaternion1_1 w: " << quaternion1_1.w() << endl;/**** 2. 旋转矩阵 *****/cout << endl << "********** RotationMatrix **********" << endl;//2.0 旋转矩阵初始化Eigen::Matrix3d rotation_matrix2;rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
;//或直接单位矩阵初始化Eigen::Matrix3d rotation_matrix2_1 = Eigen::Matrix3d::Identity();//2.1 旋转矩阵转换为欧拉角//ZYX顺序，即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0); cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl;//2.2 旋转矩阵转换为旋转向量Eigen::AngleAxisd rotation_vector2;rotation_vector2.fromRotationMatrix(rotation_matrix2);//或者Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2);cout << "rotation_vector2 " << "angle is: " << rotation_vector2.angle() * (180 / M_PI) << " axis is: " << rotation_vector2.axis().transpose() << endl;cout << "rotation_vector2_1 " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector2_1.axis().transpose() << endl;//2.3 旋转矩阵转换为四元数Eigen::Quaterniond quaternion2(rotation_matrix2);//或者Eigen::Quaterniond quaternion2_1;quaternion2_1 = rotation_matrix2;cout << "quaternion2 x: " << quaternion2.x() << endl;cout << "quaternion2 y: " << quaternion2.y() << endl;cout << "quaternion2 z: " << quaternion2.z() << endl;cout << "quaternion2 w: " << quaternion2.w() << endl;cout << "quaternion2_1 x: " << quaternion2_1.x() << endl;cout << "quaternion2_1 y: " << quaternion2_1.y() << endl;cout << "quaternion2_1 z: " << quaternion2_1.z() << endl;cout << "quaternion2_1 w: " << quaternion2_1.w() << endl;/**** 3. 欧拉角 ****/cout << endl << "********** EulerAngle **********" << endl;//3.0 初始化欧拉角(Z-Y-X，即RPY, 先绕x轴roll,再绕y轴pitch,最后绕z轴yaw)Eigen::Vector3d ea(0.785398, -0, 0);//3.1 欧拉角转换为旋转矩阵Eigen::Matrix3d rotation_matrix3;rotation_matrix3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());cout << "rotation matrix3 =\n" << rotation_matrix3 << endl;   //3.2 欧拉角转换为四元数,Eigen::Quaterniond quaternion3;quaternion3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());cout << "quaternion3 x: " << quaternion3.x() << endl;cout << "quaternion3 y: " << quaternion3.y() << endl;cout << "quaternion3 z: " << quaternion3.z() << endl;cout << "quaternion3 w: " << quaternion3.w() << endl;//3.3 欧拉角转换为旋转向量Eigen::AngleAxisd rotation_vector3;rotation_vector3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());  cout << "rotation_vector3 " << "angle is: " << rotation_vector3.angle() * (180 / M_PI) << " axis is: " << rotation_vector3.axis().transpose() << endl;/**** 4.四元数 ****/cout << endl << "********** Quaternion **********" << endl;//4.0 初始化四元素,注意eigen Quaterniond类四元数初始化参数顺序为w,x,y,zEigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683);//4.1 四元数转换为旋转向量Eigen::AngleAxisd rotation_vector4(quaternion4);//或者Eigen::AngleAxisd rotation_vector4_1;rotation_vector4_1 = quaternion4;cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI) << " axis is: " << rotation_vector4.axis().transpose() << endl;cout << "rotation_vector4_1 " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector4_1.axis().transpose() << endl;//4.2 四元数转换为旋转矩阵Eigen::Matrix3d rotation_matrix4;rotation_matrix4 = quaternion4.matrix();Eigen::Matrix3d rotation_matrix4_1;rotation_matrix4_1 = quaternion4.toRotationMatrix();cout << "rotation matrix4 =\n" << rotation_matrix4 << endl;cout << "rotation matrix4_1 =\n" << rotation_matrix4_1 << endl;      //4.4 四元数转欧拉角(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle4 = quaternion4.matrix().eulerAngles(2,1,0);cout << "yaw(z) pitch(y) roll(x) = " << eulerAngle4.transpose() << endl;return 0;
}

终端输出:

【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）相关推荐

基于蝙蝠算法优化BP神经网络的数据分类算法及其MATLAB实现-附代码
基于蝙蝠算法优化BP神经网络的数据分类算法及其MATLAB实现-附代码文章目录基于蝙蝠算法优化BP神经网络的数据分类算法及其MATLAB实现-附代码 1 蝙蝠算法与BP神经网络分类模型 1.1 蝙 ...
自动驾驶前沿综述：基于深度强化学习的自动驾驶算法
©作者 | 陈道明学校 | 布里斯托尔大学研究方向 | 自动驾驶方向这是 21 年的一篇综述文章,可以算得上是最前沿的自动驾驶技术综述.这几年随着深度表征学习的发展,强化学习领域也得到了加强.本 ...
面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)...
背景介绍 3D检测用于获取物体在三维空间中的位置和类别信息,主要基于点云.双目.单目和多模态数据等方式.其中,点云数据由于具有较为丰富的几何信息,相比于其它单模态数据更为稳定,基于激光雷达点云数据的3 ...
分布式消息总线，基于.NET Socket Tcp的发布-订阅框架之离线支持，附代码下载
一.分布式消息总线以及基于Socket的实现在前面的分享一个分布式消息总线,基于.NET Socket Tcp的发布-订阅框架,附代码下载一文之中给大家分享和介绍了一个极其简单也非常容易上的基于.N ...
自动驾驶仿真：Carsim基于车辆后轴中心输出参数
文章目录前言一.Motion Sensors详解 1.创建Motion Sensors 2.配置Motion Sensors参数 3. 生成后轴中心相关变量前言已知Carsim的车辆中心点位于 ...
《数字语音处理》- 实验4. 基于MATLAB与VQ的特定人孤立词语音识别研究（附代码）
声明本文仅在CSDN发布,未经允许请勿转载或引用! 正版链接: https://blog.csdn.net/meenr/article/details/117629850 MATLAB基于VQ的特定 ...
基于蜜蜂优化算法优化的卷积神经网络（CNN）图像分类——附代码
目录摘要: 1.蜜蜂优化算法: 2. 卷积神经网络(CNN) 输入层隐含层输出层 3. 本文Matlab代码: 摘要: 本文通过蜜蜂优化算法,优化了卷积神经网络(CNN)中的超参数,主要是网 ...
带你了解基于Ploto构建自动驾驶平台
摘要:华为云Solution as Code推出基于Ploto构建自动驾驶平台解决方案. 本文分享自华为云社区<基于Ploto构建自动驾驶平台>,作者:阿米托福 . 2022年6月15日, ...
基于 Ploto 华为云 Solution as Code 推出构建自动驾驶平台
在自动驾驶专场中,华为云携手合作伙伴联合发布 "乐高式" 自动驾驶研发平台解决方案,实现自动驾驶研发效率提升. 联合发布 "乐高式" 自动驾驶研发平台解决方案 ...

【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）

【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）相关推荐

最新文章

热门文章