97 lines
2.8 KiB
Python
97 lines
2.8 KiB
Python
# -- coding: utf-8 --
|
||
import numpy as np
|
||
from scipy.spatial.transform import Rotation as rot
|
||
|
||
import cv2
|
||
import numpy as np
|
||
import math
|
||
|
||
# 旋转矩阵转旋转向量（Rxyz->Rvector，按特定轴旋转）
|
||
def rotvector2rot(rotvector):
|
||
Rm = cv2.Rodrigues(rotvector)[0]
|
||
return Rm
|
||
|
||
# 旋转矩阵转欧拉角
|
||
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])
|
||
def rot2eulerpi(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])
|
||
def rot_to_ypr_zyx(R):
|
||
"""
|
||
将旋转矩阵R转换为Yaw-Pitch-Roll角度（Z-Y-X顺序）
|
||
"""
|
||
sin_p = 2 * (R[2, 0] ** 2 - R[0, 0] ** 2 + R[1, 0] ** 2) ** 0.5
|
||
if sin_p < 1e-6:
|
||
sin_p = 1e-6
|
||
if R[2, 0] > 0:
|
||
sin_p = -sin_p
|
||
|
||
roll = np.arctan2(R[2, 0], R[2, 2])
|
||
pitch = np.arcsin(-R[2, 1])
|
||
yaw = np.arctan2(R[1, 0], R[0, 0])
|
||
|
||
return yaw, pitch, roll
|
||
def rot_to_ypr_xyz(R):
|
||
# 计算R的trace值
|
||
tr = np.trace(R)
|
||
# 计算sqrt(R.T.R) - 1的模长，即平方根trace值
|
||
sqrt_tr_m1 = np.sqrt(0.25 * ((tr - 3) ** 2) + (R[2, 2] - 1) ** 2)
|
||
|
||
# 根据平方根trace值判断Yaw-Pitch-Roll角的计算方法
|
||
yaw = np.arctan2(R[2, 1], R[2, 2]) # 使用arctan2计算Yaw角
|
||
pitch = np.arcsin(-R[2, 0]) # 使用arcsin计算Pitch角
|
||
roll = np.arctan2(R[1, 0], R[0, 0]) # 使用arctan2计算Roll角
|
||
|
||
return yaw, pitch, roll
|
||
|
||
# 四元数转欧拉角
|
||
def quaternion2euler(quaternion):
|
||
r = rot.from_quat(quaternion)
|
||
euler = r.as_euler('xyz', degrees=True)
|
||
return euler
|
||
|
||
# 四元数转旋转矩阵
|
||
def quaternion2rot(quaternion):
|
||
r = rot.from_quat(quaternion)
|
||
rot = r.as_matrix()
|
||
return rot
|
||
|
||
# 欧拉角转四元数
|
||
def euler2quaternion(euler):
|
||
r = rot.from_euler('xyz', euler, degrees=True)
|
||
quaternion = r.as_quat()
|
||
return quaternion
|
||
|
||
# 欧拉角转旋转矩阵
|
||
def euler2rot(euler):
|
||
r = rot.from_euler('xyz', euler, degrees=True)
|
||
rotation_matrix = r.as_matrix()
|
||
return rotation_matrix |