计算 N*4*4 位姿形状的逆变换，在N*3*4位姿后补充 [0,0,0,1]

针对 [N,4,4] shape 的 poses，函数 ComputeInversePoses 返回 相同 shape，但是每个 pose 都是前面的 逆 pose。

针对 [N,3,4] shape 的 poses，函数 AddIdentityToPoses 返回 在每个 [3,4] pose下加上 [0,0,0,1] 后的pose，返回的 shape [N,4,4]

def ComputeInversePoses(poses):
    if isinstance(poses, torch.Tensor):
        # Convert torch tensor to numpy array
        poses = poses.numpy()

    # Check if poses is a numpy array
    if not isinstance(poses, np.ndarray):
        raise ValueError("Input poses must be a numpy array")

    # Check if poses is 3-dimensional
    if len(poses.shape) != 3 or poses.shape[1:] != (4, 4):
        raise ValueError("Input poses must be a 3-dimensional array with shape (N, 4, 4)")

    # Create an array to store the inverse poses
    inverse_poses = np.zeros_like(poses)

    # Compute the inverse for each 4x4 matrix
    for i in range(poses.shape[0]):
        inverse_poses[i] = np.linalg.inv(poses[i])

    return inverse_poses.astype(np.float32)


def AddIdentityToPoses(poses):
    # Check if poses is a torch tensor
    if isinstance(poses, torch.Tensor):
        # Convert torch tensor to numpy array
        poses = poses.numpy()

    # Check if poses is 3-dimensional
    if len(poses.shape) != 3 or poses.shape[2] != 4:
        raise ValueError("Input poses must be a 3-dimensional array with shape (N, 3, 4)")

    # Create poses_with_identity array
    poses_with_identity = np.zeros((poses.shape[0], 4, 4), dtype=np.float32)
    poses_with_identity[:, :3, :4] = poses
    poses_with_identity[:, 3, :] = [0, 0, 0, 1]

    return poses_with_identity.astype(np.float32)