File size: 3,255 Bytes
2e4e201 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 |
import math
import numpy as np
from scipy.spatial.transform import Rotation as R
def create_perspective_matrix(aspect_ratio):
kDegreesToRadians = np.pi / 180.
near = 1
far = 10000
perspective_matrix = np.zeros(16, dtype=np.float32)
# Standard perspective projection matrix calculations.
f = 1.0 / np.tan(kDegreesToRadians * 63 / 2.)
denom = 1.0 / (near - far)
perspective_matrix[0] = f / aspect_ratio
perspective_matrix[5] = f
perspective_matrix[10] = (near + far) * denom
perspective_matrix[11] = -1.
perspective_matrix[14] = 1. * far * near * denom
# If the environment's origin point location is in the top left corner,
# then skip additional flip along Y-axis is required to render correctly.
perspective_matrix[5] *= -1.
return perspective_matrix
def project_points(points_3d, transformation_matrix, pose_vectors, image_shape):
P = create_perspective_matrix(image_shape[1] / image_shape[0]).reshape(4, 4).T
L, N, _ = points_3d.shape
projected_points = np.zeros((L, N, 2))
for i in range(L):
points_3d_frame = points_3d[i]
ones = np.ones((points_3d_frame.shape[0], 1))
points_3d_homogeneous = np.hstack([points_3d_frame, ones])
transformed_points = points_3d_homogeneous @ (transformation_matrix @ euler_and_translation_to_matrix(pose_vectors[i][:3], pose_vectors[i][3:])).T @ P
projected_points_frame = transformed_points[:, :2] / transformed_points[:, 3, np.newaxis] # -1 ~ 1
projected_points_frame[:, 0] = (projected_points_frame[:, 0] + 1) * 0.5 * image_shape[1]
projected_points_frame[:, 1] = (projected_points_frame[:, 1] + 1) * 0.5 * image_shape[0]
projected_points[i] = projected_points_frame
return projected_points
def project_points_with_trans(points_3d, transformation_matrix, image_shape):
P = create_perspective_matrix(image_shape[1] / image_shape[0]).reshape(4, 4).T
L, N, _ = points_3d.shape
projected_points = np.zeros((L, N, 2))
for i in range(L):
points_3d_frame = points_3d[i]
ones = np.ones((points_3d_frame.shape[0], 1))
points_3d_homogeneous = np.hstack([points_3d_frame, ones])
transformed_points = points_3d_homogeneous @ transformation_matrix[i].T @ P
projected_points_frame = transformed_points[:, :2] / transformed_points[:, 3, np.newaxis] # -1 ~ 1
projected_points_frame[:, 0] = (projected_points_frame[:, 0] + 1) * 0.5 * image_shape[1]
projected_points_frame[:, 1] = (projected_points_frame[:, 1] + 1) * 0.5 * image_shape[0]
projected_points[i] = projected_points_frame
return projected_points
def euler_and_translation_to_matrix(euler_angles, translation_vector):
rotation = R.from_euler('xyz', euler_angles, degrees=True)
rotation_matrix = rotation.as_matrix()
matrix = np.eye(4)
matrix[:3, :3] = rotation_matrix
matrix[:3, 3] = translation_vector
return matrix
def matrix_to_euler_and_translation(matrix):
rotation_matrix = matrix[:3, :3]
translation_vector = matrix[:3, 3]
rotation = R.from_matrix(rotation_matrix)
euler_angles = rotation.as_euler('xyz', degrees=True)
return euler_angles, translation_vector |