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

如下所示:

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将四元数变换为旋转矩阵的实例就是小编分享给大家的全部内容了,希望能给大家一个参考,也希望大家多多支持我们。

(0)

相关推荐

  • 对Python的zip函数妙用,旋转矩阵详解

    Python的zip函数 示例1: x = [1, 2, 3] y = [4, 5, 6] z = [7, 8, 9] xyz = zip(x, y, z) print xyz 运行的结果是: [(1, 4, 7), (2, 5, 8), (3, 6, 9)] 从这个结果可以看出zip函数的基本运作方式. 示例2: x = [1, 2, 3] y = [4, 5, 6] z = [7, 8, 9] xyz = zip(x, y, z) u = zip(*xyz) print u 运行的结果是:

  • Python3实现的旋转矩阵图像算法示例

    本文实例讲述了Python3实现的旋转矩阵图像算法.分享给大家供大家参考,具体如下: 问题: 给定一个 n × n 的二维矩阵表示一个图像. 将图像顺时针旋转 90 度. 方案一:先按X轴对称旋转, 再用zip()解压,最后用list重组. # -*- coding:utf-8 -*- #! python3 class Solution: def rotate(self, matrix): """ :type matrix: List[List[int]] :rtype: v

  • python 和c++实现旋转矩阵到欧拉角的变换方式

    在摄影测量学科中,国际摄影测量遵循OPK系统,即是xyz转角系统,而工业中往往使用zyx转角系统. 旋转矩阵的意义:描述相对地面的旋转情况,yaw-pitch-roll对应zyx对应k,p,w #include <iostream> #include<stdlib.h> #include<eigen3/Eigen/Core> #include<eigen3/Eigen/Dense> #include<stdlib.h> using namespa

  • Python根据欧拉角求旋转矩阵的实例

    利用numpy和scipy,我们可以很容易根据欧拉角求出旋转矩阵,这里的旋转轴我们你理解成四元数里面的旋转轴 import numpy as np import scipy.linalg as linalg import math #参数分别是旋转轴和旋转弧度值 def rotate_mat(self, axis, radian): rot_matrix = linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian)) a

  • python实现回旋矩阵方式(旋转矩阵)

    我们知道Python中是没有数组 这种数据结构的,所以要想实现回旋矩阵,需要先导入一个numpy包, 它是一个由多维数组对象和用于处理数组的例程集合组成的python扩充程序库,可以用来做数组算数和逻辑运算 思路: 首先创建一个n*m全为零的矩阵,然后按照旋转规律依次替换里面的值 最外层的循环代表替换一圈数据,这一圈数据在数组中的形状是一个矩形,我们分析可以知道,最后一轮循环有两种情况: 1.替换一圈(矩形)数据 2.替换一个数据 如果是第一种情况,我们让循环正常运行就可以了,但是如果是第二种,

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

    如下所示: 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.2602268

  • python opencv实现任意角度的透视变换实例代码

    本文主要分享的是一则python+opencv实现任意角度的透视变换的实例,具体如下: # -*- coding:utf-8 -*- import cv2 import numpy as np def rad(x): return x * np.pi / 180 img = cv2.imread("6.jfif") cv2.imshow("original", img) # 扩展图像,保证内容不超出可视范围 img = cv2.copyMakeBorder(img,

  • python 简单备份文件脚本v1.0的实例

    整体思路 将要备份的目录列为一个列表,通过执行系统命令,进行压缩.备份. 这样关键在于构造命令并使用 os.system( )来执行,一开始使用zip 命令始终没有成功,后来发现Windows下并没有这个命令,还要安装GnuWin32项目,后来安装了7z,实现了使用系统命令进行压缩. 压缩命令 通过下载7z压缩,将7z.exe 7z,dll 加入系统环境变量目录,通过以下命令进行压缩.解压7z a test.zip a.txt b.txt # 指定若干文件 7z a test.zip f:/te

  • python里使用正则表达式的组嵌套实例详解

    python里使用正则表达式的组嵌套实例详解 由于组本身是一个完整的正则表达式,所以可以将组嵌套在其他组中,以构建更复杂的表达式.下面的例子,就是进行组嵌套的例子: #python 3.6 #蔡军生 #http://blog.csdn.net/caimouse/article/details/51749579 # import re def test_patterns(text, patterns): """Given source text and a list of pa

  • python 文件操作删除某行的实例

    使用continue跳过本次写循环就可以了 #文本内容 Yesterday when I was young 昨日当我年少轻狂 The tasting of life was sweet 生命的滋味是甜的 As rain upon my tongue tasting I lived by night and shunned the naked light of day tasting123 And only now I see how the time ran away tasting tast

  • Python 模拟员工信息数据库操作的实例

    1.功能简介 此程序模拟员工信息数据库操作,按照语法输入指令即能实现员工信息的增.删.改.查功能. 2.实现方法 • 架构: 本程序采用python语言编写,关键在于指令的解析和执行:其中指令解析主要运用了正则表达式来高效匹配有效信息:指令执行通过一个commd_exe主执行函数和增.删.改.查4个子执行函数来实现,操作方法主要是运用面向对象方法将员工信息对象化,从而使各项操作都能方便高效实现.程序主要函数如下: (1)command_exe(command) 指令执行主函数,根据指令第一个字段

  • python 循环while和for in简单实例

    python 循环while和for in简单实例 #!/uer/bin/env python # _*_ coding: utf-8 _*_ lucknumber = 5 b = 0 while b <3: print('guss count:',b) a = int(input('you guse number')) if a > lucknumber: print ('youaerbiger') elif a == lucknumber: print ('youare righet')

  • python 读取excel文件生成sql文件实例详解

    python 读取excel文件生成sql文件实例详解 学了python这么久,总算是在工作中用到一次.这次是为了从excel文件中读取数据然后写入到数据库中.这个逻辑用java来写的话就太重了,所以这次考虑通过python脚本来实现. 在此之前需要给python添加一个xlrd模块,这个模块是专门用来操作excel文件的. 在mac中可以通过easy_install xlrd命令实现自动安装模块 import xdrlib ,sys import xlrd def open_excel(fil

  • Python中Selenium模拟JQuery滑动解锁实例

    本文介绍了Python中Selenium模拟JQuery滑动解锁实例,分享给大家,也给自己留个笔记 滑动解锁一直做UI自动化的难点之一,我补一篇滑动解锁的例子,希望能给初做Web UI自动化测试的同学一些思路. 首先先看个例子. 当我手动点击滑块时,改变的只是样式: 1.slide-to-unlock-handle 表示滑块,滑块的左边距在变大(因为它在向右移动嘛!) 2.Slide-tounlock-progress 表示滑过之后的背景黄色,黄色的宽度在增加,因为滑动经过的地方都变黄了. 除些

随机推荐