四元数，欧拉角，旋转矩阵相互转换

#include <TransForms3d/TransForms.h>/*---------------------------------------角度弧度转换----------------------------------------*/
/*** @description: 角度转为弧度* @param {double} angle 角度值* @return 返回对应弧度值,一般在-3.14~3.14之间*/
double TransForms::Degrees(double angle)
{return angle / 180 * M_PI;
}/*** @description: 弧度转为角度* @param {double} degrees 弧度值* @return 返回对应的角度值，一般在-180~180之间*/
double TransForms::Angle(double degrees)
{return degrees / M_PI * 180;
}/*---------------------------------------欧拉角部分---------------------------*/
/*** @description: 角度制欧拉角转旋转矩阵,此函数默认的旋转顺序是x-y-z.* @param {double} rx 绕x轴的旋转.* @param {double} ry 绕y轴的旋转.* @param {double} rz 绕z轴的旋转.* @return {Matrix3d}  返回3✖3的旋转矩阵.*/
Matrix3d TransForms::EulerAngle2Mat(double rx, double ry, double rz)
{rx = Degrees(rx);ry = Degrees(ry);rz = Degrees(rz);AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()));AngleAxisd pitchAngle(AngleAxisd(ry, Vector3d::UnitY()));AngleAxisd yawAngle(AngleAxisd(rz, Vector3d::UnitZ()));Matrix3d rotation_matrix;rotation_matrix = yawAngle * pitchAngle * rollAngle;return rotation_matrix;
}
/*** @description: 欧拉角转旋转矩阵* @param {Vector3d} eular 欧拉角rx,ry,rz * @return {Matrix3d} 返回3✖3的旋转矩阵.*/
Matrix3d TransForms::Euler2Mat(double rx, double ry, double rz)
{AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()));AngleAxisd pitchAngle(AngleAxisd(ry, Vector3d::UnitY()));AngleAxisd yawAngle(AngleAxisd(rz, Vector3d::UnitZ()));Matrix3d rotation_matrix;rotation_matrix = yawAngle * pitchAngle * rollAngle;return rotation_matrix;
}/*** @description:欧拉角转四元数* @param {double} rx 绕x轴的旋转* @param {double} ry 绕y轴的旋转* @param {double} rz 绕z轴的旋转* @return {Quaterniond} 返回对应的四元数*/
Quaterniond TransForms::Euler2Quat(double rx, double ry, double rz)
{return Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ());
}/*** @description: 角度制欧拉角转四元数* @param {double} rx 绕x轴的旋转* @param {double} ry 绕y轴的旋转* @param {double} rz 绕z轴的旋转* @return {Quaterniond} 返回对应的四元数*/
Quaterniond TransForms::EulerAngle2Quat(double rx, double ry, double rz)
{rx = Degrees(rx);ry = Degrees(ry);rz = Degrees(rz);return Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ());
}
/*** @description: 旋转矩阵转欧拉角(弧度制)* @param {Matrix3d} 3✖3的旋转矩阵* @return {Vector3d} 欧拉角*/
Vector3d TransForms::Mat2Euler(Matrix3d mat)
{return mat.eulerAngles(2, 1, 0);
}/*** @description: 欧拉角转旋转矩阵* @param {double} rx 绕x轴的旋转* @param {double} ry 绕y轴的旋转* @param {double} rz 绕z轴的旋转* @return {*}*/
Matrix3d TransForms::EulerAngle2Mat(Vector3d eular)
{return EulerAngle2Mat(eular.x(), eular.y(), eular.z());
}/*** @description: 旋转矩阵转角度制欧拉角(角度制)* @param {Matrix3d} 3✖3的旋转矩阵* @return {Vector3d}  欧拉角 */
Vector3d TransForms::Mat2EulerAngle(Matrix3d mat)
{Vector3d rot = mat.eulerAngles(0, 1, 2);rot = rot / M_PI * 180;return rot;
}/*---------------------------------------四元数部分----------------------------------------*/
/*** @description: 四元数转旋转矩阵* @param {Quaterniond} 四元数* @return {Matrix3d} 对应的旋转矩阵*/
Matrix3d TransForms::Quat2Mat(Quaterniond quat)
{return quat.matrix();
}/*** @description: 四元数转欧拉角* @param {Quaterniond} 四元数* @return {Vector3d} 对应的欧拉角*/
Vector3d TransForms::Quat2Eular(Quaterniond quat)
{return Mat2Euler(quat.matrix());
}/*** @description: 四元数转弧度制欧拉角(角度制)* @param {Quaterniond} 四元数* @return {Vector3d} 对应的欧拉角*/
Vector3d TransForms::Quat2EularAngle(Quaterniond quat)
{return Mat2EulerAngle(quat.matrix());
}/*** @description: 旋转矩阵转四元数* @param {Matrix3d} 3✖3的旋转矩阵* @return {Quaterniond} 对应的四元数*/
Quaterniond TransForms::Mat2Quat(Matrix3d mat)
{return Quaterniond(mat);
}/*---------------------------------------齐次矩阵部分----------------------------------------*/
/*** @description: 通过位置和欧拉角合成一个齐次矩阵* @param {Vector3d} positon 平移位置* @param {Vector3d} rotEular  旋转变换(欧拉角形式)* @return {*}*/
Matrix4d TransForms::Compose(Vector3d positon, Vector3d rotEular)
{Matrix3d rot = TransForms::EulerAngle2Mat(rotEular);// std::cout<<Mat2EulerAngle(rot);Matrix4d t;t.setIdentity();t.block<3, 3>(0, 0) = rot;t.block<3, 1>(0, 3) = positon;return t;
}/*** @description: 通过位置和四元数合成一个齐次矩阵* @param {Vector3d} positon 平移位置* @param {Quaterniond} quat 四元数* @return {Matrix4d} 齐次矩阵*/
Matrix4d TransForms::Compose(Vector3d positon, Quaterniond quat)
{return Compose(positon, Quat2Eular(quat));
}/*** @description: 通过三个位置和三个欧拉角合成一个齐次矩阵* @param {double} x 沿x轴的平移* @param {double} y 沿y轴的平移* @param {double} z 沿z轴的平移* @param {double} rx 绕x轴的旋转* @param {double} ry 绕y轴的旋转* @param {double} rz 绕z轴的旋转* @return {Matrix4d} 返回4✖4的齐次变换矩阵*/
Matrix4d TransForms::ComposeEuler(const double x, const double y, const double z, const double rx, const double ry, const double rz)
{Eigen::Vector3d rot(rx, ry, rz);Eigen::Vector3d pos(x, y, z);return TransForms::Compose(pos, rot);
}/*** @description:  将齐次矩阵转换成平移和欧拉角形式，方便理解* @param {Matrix4d} 4✖4的齐次变换矩阵* @return {VectorXd} x,y,z,rx,ry,rz*/
VectorXd TransForms::H2EulerAngle(Matrix4d t)
{VectorXd pose = VectorXd(6);Matrix3d mt = t.block<3, 3>(0, 0);Vector3d p3 = t.block<3, 1>(0, 3).col(0);pose(0, 0) = p3.x();pose(1, 0) = p3.y();pose(2, 0) = p3.z();Vector3d eular = Mat2EulerAngle(mt);pose(3, 0) = eular.x();pose(4, 0) = eular.y();pose(5, 0) = eular.z();return pose;
}

四元数，欧拉角，旋转矩阵相互转换相关推荐

matlab和eigen在旋转向量，欧拉角，四元数，旋转矩阵转换的对比（一旋转矩阵转其他）
1. 参考: eigen安装:clion使用Eigen_gxt_kt的博客-CSDN博客_clion eigen matlab 角度转四元数_四元数的两种写法与转换_女王丁丁的博客-CSDN博客机械 ...
欧拉角、四元数和旋转矩阵
旋转变换旋转变换最为直观的表示方法是"轴-角":绕着某一个过原点轴,旋转某一角度. 轴可以用一个单位长度的点[w1,w2,w3][w_1,w_2,w_3][w1,w2,w3 ...
Eigen 欧拉角的说明，及四元数和旋转矩阵的变换
本文说明eulerAngles(0, 1, 2),和eulerAngles(2, 1, 0)的差异,并顺便将欧拉角.旋转矩阵.四元数一块的联系写了一下,也结合了一些有趣的博客内容. 1. 欧拉角旋转方 ...
unity3d四元数和旋转矩阵
一四元数 Quaternion中存放了x,y,z,w四个数据成员,可以用下标来进行访问,对应的下标分别是0,1,2,3. 主要介绍几个函数 <1> 根据两个向量计算出旋转量,计算出来的旋 ...
OpenGL相机自由移动旋转缩放，四元数,欧拉角，LookAt
OpenGL相机自由移动旋转缩放,四元数,欧拉角,LookAt 定义相机摄像机位置右轴上轴 Look At 自由移动相机左右移动移动速度视角移动欧拉角通过欧拉角计算实际的方向向量缩放 ...
【游戏课】技术片段之——四元数与旋转矩阵的关系
什么是四元数四元数将三维空间的旋转拓展到思维空间,在旋转和方向的变化方面,优于欧拉角和变换矩阵.因为四元数把三维空间中的绕三个轴向的旋转变换为四维空间中绕一个轴向的旋转,从而使其比欧拉角的直接插值能 ...
python中将四元数转换为旋转矩阵
在制作bundlefusion时,想测试TUM数据集,并且将groundtruth写入到数据集中,TUM中给定的groundtruth中的旋转是使用四元数表示的,而bundlefusion中需要SE3 ...
【机器人】四元数与旋转矩阵的转换关系
导语:最近在搭建电机与机械臂的联合仿真,闲暇之余顺便看了一下姿态轨迹规划的文章,对姿态的插补有点感兴趣,发现用四元数表示姿态然后来进行姿态的插补非常方便,且不会出现奇异.这里对四元数与常规的旋转矩阵的 ...
欧拉角的概念理解和欧拉角旋转矩阵推导
欧拉角用来计算空间中刚体的旋转位置,目的是改变刚体的朝向. 具体来说,空间中有一个点p和一根轴k,点p绕轴k旋转θ角度到p',求p'的坐标.这就是欧拉角要解决的问题. 只不过,欧拉角将1个点绕1根轴旋 ...

四元数，欧拉角，旋转矩阵相互转换

四元数，欧拉角，旋转矩阵相互转换相关推荐

最新文章

热门文章