python将四元数变换为旋转矩阵的实例

所属分类: 脚本专栏 / python 阅读数: 1637
收藏 0 赞 0 分享

如下所示:

import numpy as np
from autolab_core import RigidTransform

# 写上用四元数表示的orientation和xyz表示的position
orientation = {'y': -0.6971278819736084, 'x': -0.716556549511624, 'z': -0.010016582945017661, 'w': 0.02142651612120239}
position = {'y': -0.26022684372145516, 'x': 0.6453529828252734, 'z': 1.179122068068349}

rotation_quaternion = np.asarray([orientation['w'], orientation['x'], orientation['y'], orientation['z']])
translation = np.asarray([position['x'], position['y'], position['z']])
# 这里用的是UC Berkeley的autolab_core,比较方便吧,当然可以自己写一个fuction来计算,计算公式在https://www.cnblogs.com/flyinggod/p/8144100.html
T_qua2rota = RigidTransform(rotation_quaternion, translation)

print(T_qua2rota)
 
# 以下是打印的结果
Tra: [ 0.64535298 -0.26022684 1.17912207]
   Rot: [[ 0.02782477 0.99949234 -0.01551915]
   [ 0.99863386 -0.02710724 0.0446723 ]
   [ 0.04422894 -0.01674094 -0.99888114]]
   Qtn: [-0.02142652 0.71655655 0.69712788 0.01001658]
   from unassigned to world

自己写的话

def quaternion_to_rotation_matrix(quat):
  q = quat.copy()
  n = np.dot(q, q)
  if n < np.finfo(q.dtype).eps:
    return np.identity(4)
  q = q * np.sqrt(2.0 / n)
  q = np.outer(q, q)
  rot_matrix = np.array(
    [[1.0 - q[2, 2] - q[3, 3], q[1, 2] + q[3, 0], q[1, 3] - q[2, 0], 0.0],
     [q[1, 2] - q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] + q[1, 0], 0.0],
     [q[1, 3] + q[2, 0], q[2, 3] - q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
     [0.0, 0.0, 0.0, 1.0]],
    dtype=q.dtype)
  return rot_matrix

描述有两种方式,即XYZABC和XYZ+quaternion:

https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format

以上这篇python将四元数变换为旋转矩阵的实例就是小编分享给大家的全部内容了,希望能给大家一个参考,也希望大家多多支持脚本之家。

更多精彩内容其他人还在看

Python实现图像几何变换

这篇文章主要介绍了Python实现图像几何变换的方法,实例分析了Python基于Image模块实现图像翻转、旋转、改变大小等操作的相关技巧,非常简单实用,需要的朋友可以参考下
收藏 0 赞 0 分享

Python中的urllib模块使用详解

这篇文章主要介绍了Python中的urllib模块使用详解,是Python入门学习中的基础知识,需要的朋友可以参考下
收藏 0 赞 0 分享

Python的多态性实例分析

这篇文章主要介绍了Python的多态性,以实例形式深入浅出的分析了Python在面向对象编程中多态性的原理与实现方法,需要的朋友可以参考下
收藏 0 赞 0 分享

python生成IP段的方法

这篇文章主要介绍了python生成IP段的方法,涉及Python文件读写及随机数操作的相关技巧,具有一定参考借鉴价值,需要的朋友可以参考下
收藏 0 赞 0 分享

python操作redis的方法

这篇文章主要介绍了python操作redis的方法,包括Python针对redis的连接、设置、获取、删除等常用技巧,具有一定参考借鉴价值,需要的朋友可以参考下
收藏 0 赞 0 分享

python妹子图简单爬虫实例

这篇文章主要介绍了python妹子图简单爬虫,实例分析了Python爬虫程序所涉及的页面源码获取、进度显示、正则匹配等技巧,需要的朋友可以参考下
收藏 0 赞 0 分享

分析用Python脚本关闭文件操作的机制

这篇文章主要介绍了分析用Python脚本关闭文件操作的机制,作者分Python2.x版本和3.x版本两种情况进行了阐述,需要的朋友可以参考下
收藏 0 赞 0 分享

python实现搜索指定目录下文件及文件内搜索指定关键词的方法

这篇文章主要介绍了python实现搜索指定目录下文件及文件内搜索指定关键词的方法,可实现针对文件夹及文件内关键词的搜索功能,需要的朋友可以参考下
收藏 0 赞 0 分享

python中getaddrinfo()基本用法实例分析

这篇文章主要介绍了python中getaddrinfo()基本用法,实例分析了Python中使用getaddrinfo方法进行IP地址解析的基本技巧,需要的朋友可以参考下
收藏 0 赞 0 分享

python查找指定具有相同内容文件的方法

这篇文章主要介绍了python查找指定具有相同内容文件的方法,涉及Python针对文件操作的相关技巧,需要的朋友可以参考下
收藏 0 赞 0 分享
查看更多