使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换 Eigen::Affine3f和Eigen::Matrix4f的转换 以及float 和 double类型转换
2024-05-19 03:02:52
转自https://www.cnblogs.com/long5683/p/14373627.html
文章目录
- 前言
- 一、旋转向量
- 1.1 初始化旋转向量
- 1.2 旋转向量转旋转矩阵
- 1.3 旋转向量转欧拉角(xyz，即RPY)
- 1.4 旋转向量转四元数
- 二、旋转矩阵
- 2.1 初始化旋转矩阵
- 2.2 旋转矩阵转旋转向量
- 2.3 旋转矩阵转欧拉角(xyz，即RPY)
- 2.4 旋转矩阵转四元数
- 三、欧拉角
- 3.1 初始化欧拉角(xyz，即RPY)
- 3.2 欧拉角转旋转向量
- 3.3 欧拉角转旋转矩阵
- 3.4 欧拉角转四元数
- 四、四元数
- 4.1 初始化四元数
- 4.2 四元数转旋转向量
- 4.3 四元数转旋转矩阵
- 4.4 四元数转欧拉角(xyz，即RPY)
- 五、Eigen::Affine3f和Eigen::Matrix4f的转换
- 六、float 和 double类型转换
- 七、绕固定坐标系转和绕当前坐标系旋转
Vector3.normalized的特点是当前向量是不改变的并且返回一个新的规范化的向量；
Vector3.Normalize的特点是改变当前向量，也就是当前向量长度是1
前言
国内复制粘贴太严重了，都不验证一下，欧拉角部分全网都是2,1,0的排序但经过自己验证0,1,2才是正确的（如有错误请指正）。
一、旋转向量
1.1 初始化旋转向量
旋转角为alpha(顺时针)，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());
1.2 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
1.3 旋转向量转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);
1.4 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
Quaterniond quaternion;Eigen::Quaterniond quaternion;
quaternion=rotation_vector;
二、旋转矩阵
2.1 初始化旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
2.2 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);
2.3 旋转矩阵转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);
2.4 旋转矩阵转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
三、欧拉角
3.1 初始化欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle(roll,pitch,yaw);
3.2 欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;
3.3 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.4 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;
四、四元数
4.1 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
4.2 四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;
4.3 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
4.4 四元数转欧拉角(xyz，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);
五、Eigen::Affine3f和Eigen::Matrix4f的转换
Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;
六、float 和 double类型转换
Eigen::MatrixXd matrix_d；
Eigen::MatrixXf matrix_f；
matrix_f = matrix_d.cast<float>();
七、绕固定坐标系转和绕当前坐标系旋转
//
// Created by qian on 2021/3/7.
//
#include "iostream"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>using namespace Eigen;
using namespace std;
int main(int argc, char **argv){Vector3d t0(1,2,3);// 初始向量Vector3d t(3,2,1); // 平移向量const float angle_x=30.0, angle_y=20.0, angle_z=10.0;// *************绕XYZ为当前坐标系旋转轴************AngleAxisd rotation_X=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());//固定坐标系的 Y 轴先投影成 X 旋转后的旋转轴,再进行旋转AngleAxisd rotation_Y=AngleAxisd(angle_y/180.0*M_PI,rotation_X.inverse()*Vector3d::UnitY());//固定坐标系的 Z 轴先投影成 XY 旋转后的旋转轴,再进行旋转AngleAxisd rotation_Z=AngleAxisd(angle_z/180.0*M_PI,rotation_Y.inverse()*rotation_X.inverse()*Vector3d::UnitZ());// 旋转向量AngleAxisd rotation_vector1;rotation_vector1=rotation_X*rotation_Y*rotation_Z;cout<<"旋转角："<<rotation_vector1.angle()*180.0/M_PI<<", 旋转轴"<<rotation_vector1.axis().transpose()<<endl;// 旋转矩阵Matrix3d rotation_matrix1(rotation_vector1);cout<<"旋转矩阵：\n"<<rotation_matrix1<<endl;// 四元数Quaterniond q1(rotation_matrix1);cout<<"四元数：\n"<<q1.coeffs()<<endl;// 齐次欧式变换Isometry3d T=Isometry3d::Identity();T.rotate(rotation_vector1);T.pretranslate(t);cout<<"齐次欧式变换：\n"<<T.matrix()<<endl;cout<<"旋转平移变换后的向量"<<T*t0<<endl;// **********绕XYZ为世界固定坐标系旋转轴**********AngleAxisd rotation_X2=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());AngleAxisd rotation_Y2=AngleAxisd(angle_y/180.0*M_PI,Vector3d::UnitY());AngleAxisd rotation_Z2=AngleAxisd(angle_z/180.0*M_PI,Vector3d::UnitZ());// 旋转向量AngleAxisd rotation_vector2;rotation_vector2=rotation_Z2*rotation_Y2*rotation_X2;cout<<"旋转角："<<rotation_vector2.angle()*180.0/M_PI<<", 旋转轴"<<rotation_vector2.axis().transpose()<<endl;// 旋转矩阵Matrix3d rotation_matrix2(rotation_vector2);cout<<"旋转矩阵：\n"<<rotation_matrix2<<endl;// 四元数Quaterniond q2(rotation_matrix2);cout<<"四元数：\n"<<q2.coeffs()<<endl;// 齐次欧式变换Isometry3d T2=Isometry3d::Identity();T2.rotate(rotation_vector2);T2.pretranslate(t);cout<<"齐次欧式变换：\n"<<T2.matrix()<<endl;cout<<"旋转平移变换后的向量"<<q2*t0+t<<endl;cout<<rotation_matrix1*rotation_matrix2.transpose()<<endl;return 0;
}
使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换 Eigen::Affine3f和Eigen::Matrix4f的转换 以及float 和 double类型转换相关推荐
- 【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）
矩阵的使用可参考系列博客:点击此处 原文链接:基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换. 也可以参考另一篇博客:eigen 中四元数.欧拉角.旋转矩阵.旋转向量. ...
- 【自动驾驶】24.欧拉角、旋转向量、四元数、万向锁
原文链接:https://blog.csdn.net/shenxiaolu1984/article/details/50639298 原文链接:https://blog.csdn.net/lql071 ...
- Sophus降维、升维与欧拉角、旋转向量的爱恨情仇
0. 简介 在面对二维与三维之间的转换时,我们常常会困惑该如何去转换,在G2O中存在有理想的坐标转换工具,但是在Sophus中却缺乏这样的手段.之前在Sophus处简要的介绍了一下SE(2)与SE(3 ...
- 罗德里格斯公式推导，以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
- [转] 罗德里格斯公式推导，以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
- 三维坐标变换（旋转矩阵旋转向量）
矩阵运算显然是计算机三维坐标变换最简单方便的计算方法,因此在 opencv.opengl.工业机器人等开发中,提到位姿旋转变换,多半用的是旋转矩阵.然而,用矩阵来表示一个旋转关系有两个缺点: 首先,通 ...
- python二郎成长笔记（三）（matlab标定工具箱详解，旋转矩阵旋转向量，matlab标定数据传入opencv）
这一章由来:二郎之前研究matlab的双目立体视觉,已经得到了需要的信息,可是,二郎想要对代码进行修改使其更适宜自己的应用目的. 修改:标定不用修改--matlab内置的已经很强大了,而且没有必要用p ...
- eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
- 旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)...
1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/C ...
- 方向向量转欧拉角_【姿态表示】旋转向量、旋转矩阵、欧拉角、四元数
1. 旋转矩阵与旋转向量旋转矩阵(Rotation Matrix)用 9 个量描述旋转的3个自由度,有冗余: 9 个量是有约束的:必须是正交矩阵,且行列式为 1 旋转向量(Rotation Vecto ...
最新文章
- 基于LODOP的打印
- PostgreSQL何以支持丰富的NoSQL特性?
- Python基础（偏函数）
- 小体积神器！这款不到500KB的电脑软件，功能也太赞了！
- 看起来很美 VS. 是你来检阅我的忧伤了吗?
- java将图片保存进mysql_Java存储图片到Mysql
- 7 useLayoutEffect、useDebugValue
- OSGi Testsuite：引入类名过滤器
- 前端学习（570）：margin负值下的等高布局
- Java局域网对战游戏、天气预报项目
- [C++]二维数组还是一维数组？
- java中BorderLayout的使用方法
- html中复选框如何添加,Word 怎么添加复选框 怎么在word文档中插入复选框
- 【王者荣耀】入门战斗经验
- Android启动之BOOT_COMPLETED广播
- macOS 安装 aircrack-ng 破解 wifi，hashcat ，OnlineHashCrack，在线破解握手包
- stick棍子by Ye
- 为什么ps因为计算机限制,photoshop为什么经常崩溃 ps防止崩溃解决办法
- python 占位符（百分号方式、Format 方式）
- [活动预告] Substrate 中的 IBC 跨链模块技术分享 Substrate-ibc
热门文章
- 教学反思计算机专业,职业中学计算机专业教学反思
- 大学英语计算机开学考试试题,2017大学计算机一级考试试题（含答案）
- Html转PDF（使用html2canvas ）跨域图片PDF文件上无法显示问题记录
- 还在论坛找IAR资料？IAR原厂免费培训了解一下！
- 全国计算机二级c 考纲,全国计算机等级考试二级C考试大纲考纲.doc
- 智能经济时代，百度“服务”的升维
- ‘vue-cli-service‘ 不是内部或外部命令，也不是可运行的程序或批处理文件。（解决办法）
- 小程序 coolui-scroller 下拉刷新 上拉加载 组件 使用 以及遇到的一些坑
- 7. limit实现分页、RowBounds分页【不推荐使用】、PageHelper插件
- python入门 -文件读写和数据清洗