python实现四元数、欧拉角、旋转矩阵、旋转向量的相互转换
python实现四元数、欧拉角、旋转矩阵、旋转向量的相互转换
文章共595字 · 阅读需要大约2分钟
一键AI生成摘要,助你高效阅读
问答
·
在实际项目中,经常会遇到各种转换,比如我遇到的就是四元数转旋转矩阵、欧拉角转旋转矩阵、旋转向量转旋转矩阵等等。。时不时还需要看一下自己的算法计算的角度是否正确,所以需要转换到欧拉角。。每次都会忘记对应的API是啥,在哪,之类的问题。。这次索性就整理到一起去算了。
1.旋转向量转旋转矩阵
import cv2
import numpy as np
def rotvector2rot(rotvector):
Rm = cv2.Rodrigues(rotvector)[0]
return Rm
rotvector = np.array([[0.223680285784755, 0.240347886848190, 0.176566110650535]])
print(rotvector2rot(rotvector))
# 输出
# [[ 0.95604131 -0.14593404 0.2543389 ]
# [ 0.19907538 0.95986385 -0.19756111]
# [-0.21529982 0.23950919 0.94672136]]
2.四元数转欧拉角
from scipy.spatial.transform import Rotation as R
def quaternion2euler(quaternion):
r = R.from_quat(quaternion)
euler = r.as_euler('xyz', degrees=True)
return euler
quaternion = [0.03551,0.21960,-0.96928, 0.10494]
print(quaternion2euler(quaternion))
# 输出
# [ -24.90053735 6.599459 -169.1003646 ]
3.欧拉角转四元数
from scipy.spatial.transform import Rotation as R
def euler2quaternion(euler):
r = R.from_euler('xyz', euler, degrees=True)
quaternion = r.as_quat()
return quaternion
euler = [-24.90053735, 6.599459, -169.1003646]
print(euler2quaternion(euler))
# 输出
# [ 0.03550998 0.21959986 -0.9692794 0.10493993]
4.欧拉角转旋转矩阵
from scipy.spatial.transform import Rotation as R
def euler2rot(euler):
r = R.from_euler('xyz', euler, degrees=True)
rotation_matrix = r.as_matrix()
return rotation_matrix
euler = [-24.90053735, 6.599459, -169.1003646]
print(euler2rot(euler))
# 输出
# [[-0.9754533 0.21902821 -0.02274859]
# [-0.18783626 -0.88152702 -0.43316008]
# [-0.11492777 -0.41825442 0.90102988]]
5.旋转矩阵转欧拉角
import numpy as np
import math
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
def rot2euler(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]) * 180 / np.pi
y = math.atan2(-R[2, 0], sy) * 180 / np.pi
z = math.atan2(R[1, 0], R[0, 0]) * 180 / np.pi
else:
x = math.atan2(-R[1, 2], R[1, 1]) * 180 / np.pi
y = math.atan2(-R[2, 0], sy) * 180 / np.pi
z = 0
return np.array([x, y, z])
rot = np.array([[-1.01749712e-02, 9.99670705e-01, -2.35574076e-02],
[-9.99890780e-01, -1.04241019e-02, -1.04769347e-02],
[-1.07190495e-02, 2.34482322e-02, 9.99667586e-01]])
print(rot2euler(rot))
# 输出
# [ 1.34368509 0.61416806 -90.58302646]
6.四元数转旋转矩阵
from scipy.spatial.transform import Rotation as R
def quaternion2rot(quaternion):
r = R.from_quat(quaternion)
rot = r.as_matrix()
return rot
quaternion = [0.03551, 0.21960, -0.96928, 0.10494]
print(quaternion2rot(quaternion))
# 输出
# [[-0.9754533 0.21902821 -0.02274859]
# [-0.18783626 -0.88152702 -0.43316008]
# [-0.11492777 -0.41825442 0.90102988]]
更多推荐
已为社区贡献1条内容
所有评论(0)