def is_transformation_matrix(mat):
"""Check if a matrix is a transformation matrix"""
assert_shape(mat, (4, 4))
val = np.all(mat[3, :3] == 0)
c = mat[3, 3] == 1
R = mat[:3, :3]
# check if orthogonal
orthogonal = np.allclose(R.dot(R.T), np.eye(3))
return val and c and orthogonal
def cartesian_to_homogeneous_vectors(cartesian_vector, matrix_type="numpy"):
"""Converts a cartesian vector to an homogenous vector"""
assert_shape(cartesian_vector, (None, ))
dimension_x = cartesian_vector.shape[0]
# Vector
if matrix_type == "numpy":
homogeneous_vector = np.zeros(dimension_x + 1)
# Last item is a 1
homogeneous_vector[-1] = 1
homogeneous_vector[:-1] = cartesian_vector
return homogeneous_vector
def convert_points_to_homogeneous(pts):
""" Convert a list of points to homogeneous coordinate system
Pads a list of points (2D or 3D) by 1's
Args:
pts: a numpy array of points [N, 2] or [N, 3]
Returns:
a list of points in homogenous coordinate system [N, 3] or [N, 4]
"""
assert_shape(pts, [None, None])
padding = np.ones(pts.shape[0], dtype=pts.dtype)
return np.concatenate([pts, padding[:, None]], axis=1)
def to_transformation_matrix(R, T):
"""Compose a 4x4 transformation matrix
Args:
R: a [3, 3] rotation matrix
T: a [3] translation matrix
Returns:
a [4, 4] transformation matrix
"""
assert_shape(R, (3, 3))
assert_shape(T, (3, ))
RT = np.eye(4, dtype=R.dtype)
RT[:3, :3] = R
RT[:3, 3] = T
return RT
def cartesian_to_homogeneous(cartesian_matrix, matrix_type="numpy"):
"""Converts a cartesian matrix to an homogenous matrix"""
assert_shape(cartesian_matrix, (None, None))
dimension_x, dimension_y = cartesian_matrix.shape
# Square matrix
# Manage different types fo input matrixes
if matrix_type == "numpy":
homogeneous_matrix = np.eye(dimension_x + 1)
elif matrix_type == "sympy":
import sympy
homogeneous_matrix = sympy.eye(dimension_x + 1)
# Add a column filled with 0 and finishing with 1 to the cartesian
# matrix to transform it into an homogeneous one.
homogeneous_matrix[:-1, :-1] = cartesian_matrix
return homogeneous_matrix
def transform_points_3d(pts, t):
"""Homogeneous transformation using a 4x4 matrix.
Args:
pts: a numpy array of 3D points [N, 3]
t: a 4x4 transformation matrix
Returns:
a numpy array of transformed points
"""
assert (is_transformation_matrix(t))
assert_shape(pts, (None, 3))
pts = pts.copy()
# convert to homogeneous coordinate system
pts = convert_points_to_homogeneous(pts)
tpts = t.dot(pts.T).T
tpts = tpts[:, :3] / tpts[:, 3, None] # dehomogenize
return tpts
def homogeneous_to_cartesian(homogeneous_matrix):
"""Converts a cartesian matrix to an homogenous matrix"""
# Remove the last column
assert_shape(homogeneous_matrix, (None, None))
return homogeneous_matrix[:-1, :-1]
def homogeneous_to_cartesian_vectors(homogeneous_vector):
"""Converts a cartesian vector to an homogenous vector"""
assert_shape(homogeneous_vector, (None, ))
return homogeneous_vector[:-1]