2021-11-08 Python欧拉角 四元数 旋转矩阵 旋转向量 齐次矩阵 转换与齐次矩阵求逆

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