Eigen中旋转矩阵、四元数、欧拉角_eigen 四元素角度-程序员宅基地

技术标签: 定位&地图&标定  

#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,z
    Eigen::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;
}
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/shanxihqls/article/details/106809633

智能推荐

配置JDK和JBOSS_jboss指定jdk-程序员宅基地

文章浏览阅读642次。 Normal 0 7.8 磅 0 2 false false false MicrosoftInternetExplorer4

Java异步非阻塞编程的几种方式_ava实现异步非阻塞-程序员宅基地

文章浏览阅读1.8k次,点赞2次,收藏6次。文章目录方式一:同步调用方式二:Future方式异步调用方式三:Callback回调方式方式四:Java9 Reactive Streams今天读到一篇博客,发现对Java异步非阻塞编程概念有些模糊了,趁此整理了一下常用的几种编程方式。方式一:同步调用当主线程发起IO操作时,会被阻塞,一直等到数据返回,此时主线程状态是Runnable状态。产生的问题是:主线程在IO等待的过程中,线程资源没有得到充分的利用,对于大量IO场景的业务吞吐量会有一定限制。如下图:方式二:Future方式异步调用使用fu_ava实现异步非阻塞

TS : typescript 类型注解_ts 注释-程序员宅基地

文章浏览阅读4k次,点赞2次,收藏9次。TS : typescript 类型注解在ts中,类型注解是一种为函数或者变量添加约束的方式。1. 基础的类型boolean,number,string2. 数组ts可以想js一样操作数组元素,定义数字有两种方式。let list : number[] = [1,2,3];//orlet list : Array<number> = [1,2,3];3. 元组元组类型表示允许数组内的类型各不相同,但是在赋值时需要按照类型顺序来赋值。let list :_ts 注释

thinkcmf5默认后台模版文件资源管理修改_thinkcmf默认模板 怎么修改-程序员宅基地

文章浏览阅读4k次。 直接修改这个两个文件即可,如果没效果,将data\runtime里面的缓存文件删除即可模板文件路径:目录\public\themes\admin_simpleboot3\user\admin_asset\index.html&lt;include file="public@header"/&gt;&lt;/head&gt;&lt;body&gt;&lt;div class..._thinkcmf默认模板 怎么修改

python五行代码解决滑块验证的缺口距离识别,破解滑块验证_滑块缺口距离计算python-程序员宅基地

文章浏览阅读5.5k次,点赞6次,收藏42次。目前网上关于滑块的缺口识别的方法很多,但是都不极简,看起来繁杂,各种算法的都有,有遍历的有二分法的,今天写个最简单,准确率最高的。直接看代码:def FindPic(target, template): """ 找出图像中最佳匹配位置 :param target: 目标即背景图 :param template: 模板即需要找到的图 :return:..._滑块缺口距离计算python

.NET AWS S3存储操作类_netcore amazons3helper-程序员宅基地

文章浏览阅读533次。背景:我们是公司内部搭建的存储 并不是使用公网云,可供参考1、Nuget包AWSSDK.S32、创建一个单利模式 操作类using Amazon.S3;using Amazon.S3.Model;using System;using System.Collections.Generic;using System.IO;using System.Linq;using System.Text;using System.Threading;using System.Threadi.._netcore amazons3helper

随便推点

c语言常见拼写错误_c语言拼写错误怎么办-程序员宅基地

文章浏览阅读1.3k次。1、 拼写错误,尤其是include,main,void,float等词。C语言的32个关键字与基本含义如下:double :声明双精度变量或函数int:声明整型变量或函数char :声明字符型变量或函数float:声明浮点型变量或函数void :声明函数无返回值或无参数,声明无类型指针(基本上就这三个作用)auto :声明自动变量一般不使用struct:声明结构_c语言拼写错误怎么办

Qt 5.15.0编译指南_qt 5.15 编译-程序员宅基地

文章浏览阅读2.1w次,点赞15次,收藏85次。从Qt5.15.0起,对于开源用户,Qt官方不再提供独立安装文件,且不再有bug修复版本(比如Qt5.15.1)如果想体验Qt5.15及之后的版本,就要培养一个新技能——编译Qt源码从编译到发布大概需要这么几个步骤:1. 从Qt官网下载源码;2. 编译源码;3. 将编译后的qmake.exe导入QtCreator或者Visual Studio;4. 使用QtCreator或者Visual Studio编译Qt项目;5. 项目发布可仍然使用windeployqt.exe。下面记_qt 5.15 编译

基于SSM整合案例-简易图书管理系统_一个基于ssm jsp mysql mubatis简单的图书管理系统-程序员宅基地

文章浏览阅读841次。Spring、Spring-MVC、Mybatis(SSM)经典整合框架!!!用Spring整合Mybatis框架和Spring-MVC做一个图书管理系统!给出详细的开发流程,开发工具是IDEA、MySQL、Maven、Navicate Premium1、建立Maven工程、导入依赖<?xml version="1.0" encoding="UTF-8"?><project xmlns="http://maven.apache.org/POM/4.0.0" xml_一个基于ssm jsp mysql mubatis简单的图书管理系统

计算机专业教授推荐信,计算机系教授推荐信样本-程序员宅基地

文章浏览阅读52次。At the request of Mr. Xizhen Chen, my former student in the Department of Computer Science, Beijing Univ. of Sciences, I am glad to write this letter furnishing my evaluation of his academic aptitude ...

Ubuntu使用记录_ubuntu系统安装 xmltodict-程序员宅基地

文章浏览阅读1k次。ubuntu使用_ubuntu系统安装 xmltodict

如何在Qt中使用boost库_qt使用boost-程序员宅基地

文章浏览阅读1.4w次,点赞13次,收藏42次。如何在Qt中使用boost库、c++11 using 类型别名用法、c++11 auto 关键字类型推导_qt使用boost