三维坐标旋转变换——四元数与旋转矩阵的相互转换，python实现

最新推荐文章于 2024-02-04 15:59:18 发布

阿航626

最新推荐文章于 2024-02-04 15:59:18 发布

阅读量1.9k

点赞数

文章标签： python 矩阵 numpy 四元数旋转矩阵

本文链接：https://blog.youkuaiyun.com/weixin_53610475/article/details/131053640

版权

该文介绍了如何使用numpy库在Python中进行旋转矩阵到四元数以及四元数回转到旋转矩阵的转换。提供了两个函数，matrix2quaternion用于矩阵转四元数，quaternion2matrix则用于四元数转矩阵。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

0.公式参考

参考公式

1.代码

导包

import pandas as pd
import numpy as np

旋转矩阵转为四元数（输入为numpy数组）

def matrix2quaternion(m):
    #m:array
    w = ((np.trace(m) + 1) ** 0.5) / 2
    x = (m[2][1] - m[1][2]) / (4 * w)
    y = (m[0][2] - m[2][0]) / (4 * w)
    z = (m[1][0] - m[0][1]) / (4 * w)
    return w,x,y,z

四元数转为旋转矩阵（输入为一位列表或数组）

def quaternion2matrix(q):
    #q:list
    w,x,y,z = q
    return np.array([[1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w],
             [2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w],
             [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y]])