针对 [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)