四元数,旋转矩阵,欧拉角互转(python)

这篇具有很好参考价值的文章主要介绍了四元数,旋转矩阵,欧拉角互转(python)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

运行代码之前需要安装pyquaternion和scipy。

pip install pyquaternion

pip install scipy

代码之前放下面,main函数有使用的方式文章来源地址https://www.toymoban.com/news/detail-517709.html

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import numpy as np
import math
#旋转矩阵转四元数需要pyquaternion包
from pyquaternion import Quaternion
#四元数转旋转矩阵需要scipy
from scipy.spatial.transform import Rotation as R

def isRotationMatrix(R) :
  Rt = np.transpose(R)
  shouldBeIdentity = np.dot(Rt, R)
  I = np.identity(3, dtype = R.dtype)
  n = np.linalg.norm(I - shouldBeIdentity)
  return n < 1e-6
#rotationMatrixToEulerAngles 用于旋转矩阵转欧拉角
def rotationMatrixToEulerAngles(R) :
 
  assert(isRotationMatrix(R))
   
  sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
   
  singular = sy < 1e-6
 
  if not singular :
    x = math.atan2(R[2,1] , R[2,2])
    y = math.atan2(-R[2,0], sy)
    z = math.atan2(R[1,0], R[0,0])
  else :
    x = math.atan2(-R[1,2], R[1,1])
    y = math.atan2(-R[2,0], sy)
    z = 0
 
  return np.array([x, y, z])
#eulerAnglesToRotationMatrix欧拉角转旋转矩阵
def eulerAnglesToRotationMatrix(theta) :
  R_x = np.array([[1,     0,         0          ],
          [0,     math.cos(theta[0]), -math.sin(theta[0]) ],
          [0,     math.sin(theta[0]), math.cos(theta[0]) ]
          ])
  R_y = np.array([[math.cos(theta[1]),  0,   math.sin(theta[1]) ],
          [0,           1,   0          ],
          [-math.sin(theta[1]),  0,   math.cos(theta[1]) ]
          ])
         
  R_z = np.array([[math.cos(theta[2]),  -math.sin(theta[2]),  0],
          [math.sin(theta[2]),  math.cos(theta[2]),   0],
          [0,           0,           1]
          ])
  R = np.dot(R_z, np.dot( R_y, R_x ))
  return R
#旋转矩阵转四元数
def rotateToQuaternion(rotateMatrix):
    q = Quaternion(matrix=rotateMatrix)
    return q

if __name__ == '__main__' :
  #初始化旋转矩阵
  rotationMat = np.array([[ -0.90748313, -0.30075654, -0.29329146],
                 [-0.05041803, -0.61514386, 0.78680115],
                 [ -0.41705203, 0.72879595, 0.54306912]])


  #旋转矩阵转欧拉角
  EulerAngles = rotationMatrixToEulerAngles(rotationMat)
  print("\nOutput Euler angles :\n{0}".format(EulerAngles))


  #欧拉角转旋转矩阵
  rotationMat_1 = eulerAnglesToRotationMatrix(EulerAngles)
  print ("\nR1 :\n{0}".format(rotationMat_1))

  #旋转矩阵转四元数
  Quaternion = rotateToQuaternion(rotationMat)
  print("四元数x为: ", Quaternion.x, "\n四元数y为: ", Quaternion.y, "\n四元数z为: ", Quaternion.z, "\n四元数w为: ", Quaternion.w)


  #四元数转旋转矩阵
  Rq = [Quaternion.x.astype(float), Quaternion.y.astype(float), Quaternion.z.astype(float), Quaternion.w.astype(float)]
  Rm = R.from_quat(Rq)
  rotation_matrix = Rm.as_matrix()
  print('rotation:\n', rotation_matrix)

到了这里,关于四元数,旋转矩阵,欧拉角互转(python)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 欧拉角,轴角,四元数与旋转矩阵详解

    入门小菜鸟,希望像做笔记记录自己学的东西,也希望能帮助到同样入门的人,更希望大佬们帮忙纠错啦~侵权立删。 目录 一、欧拉角 1、静态定义 2、欧拉角的表示  3、欧拉角表示的优缺点  4、欧拉角的万向节死锁(静态不存在万向锁的问题) 二、四元数 1、提出意义和定

    2024年01月17日
    浏览(43)
  • 【Eigen库使用】角轴、旋转矩阵、欧拉角、四元数转换

    在slam中经常用到的四种描述机器人orientation的变量,他们之间可以相互转化,使用Eigen库可以很容易的做到这一点, 需要特别关注的是:欧拉角与其余量之间的转换关系 : 1)首先要明确的是, 必须要明确欧拉角的旋转次序 ,你可以选择RPY、YPR等方式,在相同的orientation下,

    2024年01月18日
    浏览(49)
  • 《动手学机器人学》7.2.4姿态之间的互相转换,Python&&C++支持四元数,欧拉角旋转矩阵、轴角

    本系列教程作者:小鱼 公众号:鱼香ROS QQ交流群:139707339 教学视频地址:小鱼的B站 完整文档地址:鱼香ROS官网 版权声明:如非允许禁止转载与商业用途。 上一节小鱼带你一起学习了四种姿态表示方式,这节课我们就利用相关的开源库,来完成姿态的不同表示方式之间的转

    2024年02月02日
    浏览(47)
  • 持之以恒(一)位姿转换:姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵

    姿态的几种表示形式, 姿态角 、 四元数 、 欧拉角 、 旋转矩阵 、 位姿矩阵 。 姿态 说明 表示形式 Eigen 姿态角 指的是机体坐标系与地理坐标系的夹角,即旋转向量 rx,ry,rz Eigen::Vector3f(Degrees) 四元数 四元素不存在万向节死锁问题、利用球面插值可以获得均匀的转速 w,x,y,z

    2024年02月15日
    浏览(52)
  • ROS系列——使用python的transforms3d、numpy库实现四元数、旋转矩阵、欧拉角、轴角等的相互转换

    pip3 install transforms3d 四元数模块在transforms3d.quaternions里,直接导入即可使用 2.1.1四元数转旋转矩阵 2.1.2 旋转矩阵转四元数 2.2.1 四元数转轴角 2.2.2 轴角转四元数 四元数模块在transforms3d.euler里,直接导入即可使用 3.1.1 固定轴欧拉角转四元数 3.1.2 四元数转固定轴欧拉角 3.2.1 固定

    2024年02月07日
    浏览(92)
  • ABB机器人欧拉角与四元数的相互转化以及旋转矩阵的求法

    做项目时用到ABB机器人,直接通过ABB内置的函数可以轻松实现四元数读数与欧拉角的相互转化。但实际项目需要从示教器读出相关位置并自行计算,尤其需要计算旋转矩阵。 本文以 ABB IRB120机器人 (不确定其他机器人是否与ABB机器人一致)为例如下姿态为例来描述上述几个量

    2024年02月03日
    浏览(53)
  • 车辆姿态表达:旋转矩阵、欧拉角、四元数的转换以及eigen、matlab、pathon方法实现

    旋转矩阵、欧拉角、四元数主要用于表示坐标系中的旋转关系,通过三者之间的转换可以减小一些算法的复杂度。 本文主要概述旋转矩阵、欧拉角、四元数的基本理论、三者之间的转换关系以及三者转换在eigen、matlab和pathon上的方法实现。 对于两个三维点 p1 、 p2 : p 1 ( x

    2023年04月11日
    浏览(45)
  • 【数理知识】三维空间旋转矩阵的欧拉角表示法,四元数表示法,两者之间的转换,Matlab 代码实现

    序号 内容 1 【数理知识】自由度 degree of freedom 及自由度的计算方法 2 【数理知识】刚体 rigid body 及刚体的运动 3 【数理知识】刚体基本运动,平动,转动 4 【数理知识】向量数乘,内积,外积,matlab代码实现 5 【数理知识】最小二乘法,从线性回归出发,数值举例并用最小

    2024年02月12日
    浏览(49)
  • Unity旋转 欧拉角和四元数

    Unity中的旋转最为常知的是Transform.rotation,但其内部实现是由 Quaternion (四元数)进行计算处理,而Inspactor中显示的旋转值是由 EulerAngles (欧拉角)处理。 Unity使用四元数对实际的旋转值进行计算和存储,使用欧拉角对基于世界空间坐标的旋转进行描述和显示,而Inspacetor中显示

    2024年02月13日
    浏览(47)
  • 游戏引擎中为什么要用四元数表示旋转而不用欧拉角旋转?

    个人观点,仅供参考,如有错误可太刺激了 四元数的简单概念和使用 欧拉角通常用于表示一个物体的 旋转状态 ,而不是表示 旋转过程 。 欧拉角描述的是物体相对于某个参考坐标系的朝向或旋转状态,通常以不同的轴(例如,绕X轴、Y轴和Z轴)的旋转角度来表示。这可以让

    2024年02月06日
    浏览(54)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包