python代码，两个4*4旋转矩阵之间的位姿变化，相对旋转矩阵

python代码，两个4*4旋转矩阵之间的位姿变化，也就是求两个旋转矩阵之间的相对旋转矩阵

import numpy as np

def get_transform_matrix(rot_mat1, rot_mat2):
    """Calculate the transformation matrix that transforms points or vectors from the coordinate frame
    represented by rot_mat1 to the coordinate frame represented by rot_mat2."""
    rot_mat_diff = np.dot(rot_mat2, np.linalg.inv(rot_mat1))
    transform_matrix = np.eye(4)
    transform_matrix[:3, :3] = rot_mat_diff
    return transform_matrix

# Example usage:
rot_mat1 = np.array([[1, 0, 0, 0],
                     [0, 1, 0, 0],
                     [0, 0, 1, 0],
                     [0, 0, 0, 1]])  # initial rotation matrix
rot_mat2 = np.array([[0, 0, 1, 0],
                     [0, 1, 0, 0],
                     [-1, 0, 0, 0],
                     [0, 0, 0, 1]])  # final rotation matrix
transform_matrix = get_transform_matrix(rot_mat1, rot_mat2)  # transformation matrix from initial to final pose
print(transform_matrix)