from scipy.spatial.transform import Rotation as R
import numpy as np
import math
# Checks if a matrix is a valid rotation matrix.
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
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(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])
# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(theta):
R_x = np.array([[1, 0, 0],
[0, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0])]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot(R_y, R_x))
return R
def siyuanshu2rotation_matrix(Rq):
# Rq = [-0.35, 1.23e-06, 4.18e-08, 0.39]
Rm = R.from_quat(Rq)
rotation_matrix = Rm.as_matrix()
return rotation_matrix
def Rm_t2T(Rm, t):
return np.array([list(Rm[0]) + [t[0]],
list(Rm[1]) + [t[1]],
list(Rm[2]) + [t[2]],
[0, 0, 0, 1]])
def inv_T(T):
R=np.array([[T[0][0],T[0][1],T[0][2]],
[T[1][0],T[1][1],T[1][2]],
[T[2][0],T[2][1],T[2][2]]])
t=np.array([[T[0][3]],[T[1][3]],[T[2][3]]])
RT=R.T
t_=-RT@t
T_ = np.array([[RT[0][0], RT[0][1], RT[0][2], t_[0][0]],
[RT[1][0], RT[1][1],RT[1][2], t_[1][0]],
[RT[2][0], RT[2][1], RT[2][2], t_[2][0]],
[0,0,0,1]])
return T_
if __name__ == "__main__":
Rq_imu2left = [-0.000742524745874, 0.00278630666435, -0.00573470676318, 0.999979376793]
Rm_imu2left = siyuanshu2rotation_matrix(Rq_imu2left)
t_imu2left=[-0.00200000009499, -0.02300000377, 0.000220000030822]
print("Rm_imu2left")
print(Rm_imu2left)
T_imu2left=Rm_t2T(Rm_imu2left, t_imu2left)
print(T_imu2left)
Rv_left2right = [0.00344811, -0.00121974, -0.000160861]
Rm_left2right = eulerAnglesToRotationMatrix(Rv_left2right)
t_left2right=[0.119879, -0.000293239, -0.00010727]
print("Rm_left2right")
print(Rm_left2right)
T_left2right = Rm_t2T(Rm_left2right, t_left2right)
print(T_left2right)
T_imu2right=T_imu2left@T_left2right
print("T_imu2right")
print(T_imu2right)
print("T_right2imu2")
print(inv_T(T_imu2right))
print("T_left2imu")
print(inv_T(T_imu2left))
#需要cam02imu cam12imu
2021-11-08 Python欧拉角 四元数 旋转矩阵 旋转向量 齐次矩阵 转换与齐次矩阵求逆
最新推荐文章于 2024-09-09 09:33:30 发布