计算 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 arrayposes = poses.numpy()# Check if poses is a numpy arrayif not isinstance(poses, np.ndarray):raise ValueError("Input poses must be a numpy array")# Check if poses is 3-dimensionalif 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 posesinverse_poses = np.zeros_like(poses)# Compute the inverse for each 4x4 matrixfor 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 tensorif isinstance(poses, torch.Tensor):# Convert torch tensor to numpy arrayposes = poses.numpy()# Check if poses is 3-dimensionalif 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 arrayposes_with_identity = np.zeros((poses.shape[0], 4, 4), dtype=np.float32)poses_with_identity[:, :3, :4] = posesposes_with_identity[:, 3, :] = [0, 0, 0, 1]return poses_with_identity.astype(np.float32)