13 lines
397 B
Python
Executable File
13 lines
397 B
Python
Executable File
import numpy as np
|
|
from numpy import ndarray
|
|
|
|
|
|
def transform_coordinates_3d(coordinates: ndarray, sRT: ndarray):
|
|
assert coordinates.shape[0] == 3
|
|
coordinates = np.vstack(
|
|
[coordinates, np.ones((1, coordinates.shape[1]), dtype=np.float32)]
|
|
)
|
|
new_coordinates = sRT @ coordinates
|
|
new_coordinates = new_coordinates[:3, :] / new_coordinates[3, :]
|
|
return new_coordinates
|