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

#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;
}

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/509955.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

node.js事件

// 1.普通事件的使用var EventEmitterrequire(events).EventEmitter;//声明事件对象 var eventnew EventEmitter();//实例化对象 event.on(some_event,function(){//注册事件并给事件取个别名some_eventconsole.log(这是一个自定义的事件); }); setTimeout(function(){//触发事…

Phoenix 关联hbase表历史数据

Phoenix 基本使用 进入Phoenix phoenix-sqlline.py 10.248.161.18:2181:/hbase 或者进入目录下执行 cd /opt/cloudera/parcels/APACHE_PHOENIX/bin 关联Hbase原有表 创建Hbase表&#xff0c;列簇为 cf1 , cf2 create phoenix_hbase_test,cf1,cf2 插入测试数据 put phoenix…

C++面试(二)

笔试题&#xff1a; Char * const p,char const *p,const char *p分别表示什么意思? 面向对象的三个特性是什么&#xff0c;有什么意义&#xff1f; 编程实现字符串连接函数 char * strcat(char *strDest,const char *strsrc);不用C/C字符串处理库函数。 ISO七层参考模型是什…

C++面试(三)

笔试题&#xff1a; 1 写出按一个字节的位对其方式的命令 #pragma pack(n) 2 写出UpdateData(BOOL values)函数的作用 3 写字符串拷贝函数&#xff0c;不能用系统的strcpy; Char *Strcpy(char *str, const char *Srcstr); 4 对两个有序的链表head1,head2&#xff0c;把他们合并…

Phoenix 关联映射 Hbase表 获取不到数据,upsert hbase 列名为16进制字符

创建Hbase表 create phoenix_hbase_test,cf1,cf2 put phoenix_hbase_test, key1,cf1:name,zhangsan put phoenix_hbase_test, key1,cf2:age,18 put phoenix_hbase_test, key2,cf1:name,lisi put phoenix_hbase_test, key2,cf2:age,26 put phoenix_hbase_test, key3,cf1:name,c…

node.js模块和包

概念&#xff1a;模块(Module)和包(Package)是Node.js最重要的支柱。开发一个具有一定规模的程序不可能只用一个文件&#xff0c;通常需要把各个功能拆分、分装、然后组合起来。模块正式为了实现这种方式而诞生&#xff0c;在浏览器JavaScript中&#xff0c;脚本模块的拆分和组…

C++面试/技巧(四)

我们老师那个时候就唧唧歪歪的说不要写精通……这了那了的。但是经过我的个人实践才发现&#xff0c;不管你写什么&#xff0c;别人问到你简历上你提到的东西时你能回答上来。别你写着精通某个技术&#xff0c;别人问你时你却说我不是很了解只是知道一点或干脆说不会&#xff0…

Ensure that config phoenix.schema.isNamespaceMappingEnabled is consistent on client and server

Phoenix链接异常&#xff0c;报错如下 0: jdbc:phoenix:xxx:2181:/hbase> Error: ERROR 726 (43M10): Inconsistent namespace mapping properties. Ensure that config phoenix.schema.isNamespaceMappingEnabled is consistent on client and server. (state43M10,code7…

node.js包管理器和代码调式

一、如何使用包管理器 Node.js包管理器&#xff0c;即npm是Node.js官方提供的包管理工具&#xff0c;它已经成了Node.js包的标准发布平台&#xff0c;用于Node.js包的发布、传播、依赖控制。(1)获取一个包 npm [install/i] [package_name] 例如安装express包&#xff1a;…

C++面试(五)

一. 笔试题. 有考关于SIZEOFC多态.虚函数内存泄露.把一串字符串.>0放左边,小于0的放右边.EXIT() 跟 _EXIT()的区别C怎样才能实现一个类只能创建一个实例有一串字符串.将其中的小写字母转为大写字母简述下进程和线程之间的区别. 二、面试题 根据简历过一遍.里面提到的相关技…

phoenix创建索引报错“ Mutable secondary indexes must have the hbase.regionserver.wal.codec property”

phoenix 创建hbase表索引时异常&#xff0c;报错如下 Error: ERROR 1029 (42Y88): Mutable secondary indexes must have the hbase.regionserver.wal.codec property set to org.apache.hadoop.hbase.regionserver.wal.IndexedWALEditCodec in the hbase-sites.xml of every …

node.js全局对象和全局变量

概念&#xff1a;所有属性都可以在程序的任何地方访问&#xff0c;即全局变量。在JavaScript中&#xff0c;通常window是全局对象&#xff0c;而Node.js的全局对象是global&#xff0c;所有全局变量都是global对象的属性&#xff0c;如&#xff1a;console、process等。一、全局…

软件项目开发流程

需求分析 1.相关系统分析员向用户初步了解需求&#xff0c;然后用相关的工具软件列出要开发的系统的大功能模块&#xff0c;每个大功能模块有哪些小功能模块&#xff0c;对于有些需求比较明确相关的界面时&#xff0c;在这一步里面可以初步定义好少量的界面。[1]2.系统分析员深…

node.js util全局变量和事件驱动events

一、util全局变量 1.util.inherits(constructor,superConstructor)此方法是一个实现对象间原型继承的函数。javaScript通过原型赋值来实现继承&#xff0c;细节可参考云哥JS高级视频(原型)。案例看 util/inherits.js定义了一个基础对象Base&#xff0c;原型方法为showName&…

Phoenix 关联查询异常 , MaxServerCacheSizeExceededException phoenix.query.maxServerCacheBytes

参数配置说明&#xff1a;http://phoenix.apache.org/tuning.html Join&#xff1a;http://phoenix.apache.org/joins.html 从设计角度来讲&#xff0c;尽可能的不使用phoenix进行关联查询&#xff0c;速度比直接根据条件查询慢很多很多 测试关联&#xff0c;异常如下 Error:…

面试时,你会问面试官哪些问题?

明天又要去参加一次面试。每次面试的时候&#xff0c;面试官都会在最后给面试者一些时间&#xff0c;来问问题。这是个非常好的机会&#xff0c;能按照自己的思路&#xff0c;来了解职位、技术、企业文化、福利待遇、企业状况和前景等情况&#xff0c;以弥补前面面试过程中没有…

Phoenix 原理 以及 Phoenix在HBase中的应用

一、前言 业务使用HBase已经有一段时间了&#xff0c;期间也反馈了很多问题&#xff0c;其中反馈最多的是HBase是否支持SQL查询和二级索引&#xff0c;由于HBase在这两块上目前暂不支持&#xff0c;导致业务在使用时无法更好的利用现有的经验来查询HBase。虽然HBase本身不支持…

node.js文件操作

介绍&#xff1a;fs模块是文件操作的封装&#xff0c;它提供了文件的读取&#xff0c;写入&#xff0c;更名&#xff0c;删除&#xff0c;遍历目录&#xff0c;链接POSIX文件系统操作。与其他模块不同的是&#xff0c;fs模块中所有的操作都提供了异步和同步两个版本&#xff0c…

Undo/Redo框架实现

http://www.cnblogs.com/wanghui9072229/archive/2011/08/29/2158960.html

phoenix 使用详细介绍 创建二级索引

phoenix 关联hbase 基本使用参考&#xff1a;https://blog.csdn.net/zhangshenghang/article/details/97491597 Phoenix 创建二级索引 hbase中有表test_article ,在表空间 test_ns ,列簇 fn 下有字段 url , text , uid ,name 连接phoenixphoenix-sqlline.py hostname:2181:/hb…