def log(self, point, base_point):
"""
Compute the Riemannian logarithm at point base_point,
of point wrt the metric defined in
inner_product.
This gives a tangent vector at point base_point.
"""
point = vectorization.expand_dims(point, to_ndim=3)
n_points, _, _ = point.shape
base_point = vectorization.expand_dims(base_point, to_ndim=3)
n_base_points, mat_dim, _ = base_point.shape
assert (n_points == n_base_points
or n_points == 1
or n_base_points == 1)
sqrt_base_point = np.zeros((n_base_points,) + (mat_dim,) * 2)
for i in range(n_base_points):
sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i])
inv_sqrt_base_point = np.linalg.inv(sqrt_base_point)
point_near_id = np.matmul(inv_sqrt_base_point, point)
point_near_id = np.matmul(point_near_id, inv_sqrt_base_point)
log_at_id = group_log(point_near_id)
log = np.matmul(sqrt_base_point, log_at_id)
log = np.matmul(log, sqrt_base_point)
return log
def rotation_vector_from_quaternion(self, quaternion):
"""
Convert a unit quaternion into a rotation vector.
"""
quaternion = vectorization.expand_dims(quaternion, to_ndim=2)
n_quaternions, _ = quaternion.shape
cos_half_angle = quaternion[:, 0]
cos_half_angle = np.clip(cos_half_angle, -1, 1)
half_angle = np.arccos(cos_half_angle)
half_angle = vectorization.expand_dims(half_angle,
to_ndim=2, axis=1)
assert half_angle.shape == (n_quaternions, 1)
rot_vec = np.zeros_like(quaternion[:, 1:])
mask_0 = np.isclose(half_angle, 0)
mask_0 = np.squeeze(mask_0, axis=1)
mask_not_0 = ~mask_0
rotation_axis = (quaternion[mask_not_0, 1:]
/ np.sin(half_angle[mask_not_0]))
rot_vec[mask_not_0] = (2 * half_angle[mask_not_0]
* rotation_axis)
rot_vec = self.regularize(rot_vec)
return rot_vec
def dist(self, point_a, point_b):
"""
Compute the Riemannian distance between points
point_a and point_b.
"""
# TODO(nina): case np.dot(unit_vec, unit_vec) != 1
if np.all(point_a == point_b):
return 0.
point_a = vectorization.expand_dims(point_a, to_ndim=2)
point_b = vectorization.expand_dims(point_b, to_ndim=2)
n_points_a, _ = point_a.shape
n_points_b, _ = point_b.shape
assert (n_points_a == n_points_b or n_points_a == 1 or n_points_b == 1)
n_dists = np.maximum(n_points_a, n_points_b)
dist = np.zeros((n_dists, 1))
norm_a = self.embedding_metric.norm(point_a)
norm_b = self.embedding_metric.norm(point_b)
inner_prod = self.embedding_metric.inner_product(point_a, point_b)
cos_angle = inner_prod / (norm_a * norm_b)
mask_cos_greater_1 = np.greater_equal(cos_angle, 1.)
mask_cos_less_minus_1 = np.less_equal(cos_angle, -1.)
mask_else = ~mask_cos_greater_1 & ~mask_cos_less_minus_1
dist[mask_cos_greater_1] = 0.
dist[mask_cos_less_minus_1] = np.pi
dist[mask_else] = np.arccos(cos_angle[mask_else])
return dist
def dist(self, point_a, point_b):
"""
Compute the distance induced on the hyperbolic
space, from its embedding in the Minkowski space.
"""
if np.all(point_a == point_b):
return 0.
point_a = vectorization.expand_dims(point_a, to_ndim=2)
point_b = vectorization.expand_dims(point_b, to_ndim=2)
n_points_a, _ = point_a.shape
n_points_b, _ = point_b.shape
assert (n_points_a == n_points_b or n_points_a == 1 or n_points_b == 1)
n_dists = np.maximum(n_points_a, n_points_b)
dist = np.zeros((n_dists, 1))
sq_norm_a = self.embedding_metric.squared_norm(point_a)
sq_norm_b = self.embedding_metric.squared_norm(point_b)
inner_prod = self.embedding_metric.inner_product(point_a, point_b)
cosh_angle = -inner_prod / np.sqrt(sq_norm_a * sq_norm_b)
mask_cosh_less_1 = np.less_equal(cosh_angle, 1.)
mask_cosh_greater_1 = ~mask_cosh_less_1
dist[mask_cosh_less_1] = 0.
dist[mask_cosh_greater_1] = np.arccosh(cosh_angle[mask_cosh_greater_1])
return dist
def exp(self, tangent_vec, base_point):
"""
Compute the Riemannian exponential at point base_point
of tangent vector tangent_vec wrt the metric
defined in inner_product.
This gives a symmetric positive definite matrix.
"""
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=3)
n_tangent_vecs, _, _ = tangent_vec.shape
base_point = vectorization.expand_dims(base_point, to_ndim=3)
n_base_points, mat_dim, _ = base_point.shape
assert (n_tangent_vecs == n_base_points
or n_tangent_vecs == 1
or n_base_points == 1)
sqrt_base_point = np.zeros((n_base_points, mat_dim, mat_dim))
for i in range(n_base_points):
sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i])
inv_sqrt_base_point = np.linalg.inv(sqrt_base_point)
tangent_vec_at_id = np.matmul(inv_sqrt_base_point,
tangent_vec)
tangent_vec_at_id = np.matmul(tangent_vec_at_id,
inv_sqrt_base_point)
exp_from_id = group_exp(tangent_vec_at_id)
exp = np.matmul(exp_from_id, sqrt_base_point)
exp = np.matmul(sqrt_base_point, exp)
return exp
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
"""
Regularize a tangent_vector by getting its norm at the identity,
determined by the metric,
to be less than pi,
following the regularization convention
"""
if metric is None:
metric = self.left_canonical_metric
base_point = self.regularize(base_point)
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
jacobian = self.jacobian_translation(
point=base_point,
left_or_right=metric.left_or_right)
inv_jacobian = np.linalg.inv(jacobian)
tangent_vec_at_id = np.dot(tangent_vec,
np.transpose(inv_jacobian, axes=(0, 2, 1)))
tangent_vec_at_id = np.squeeze(tangent_vec_at_id, axis=1)
tangent_vec_at_id = self.regularize_tangent_vec_at_identity(
tangent_vec_at_id,
metric)
regularized_tangent_vec = np.dot(tangent_vec_at_id,
np.transpose(jacobian,
axes=(0, 2, 1)))
regularized_tangent_vec = np.squeeze(regularized_tangent_vec, axis=1)
return regularized_tangent_vec
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None):
if metric is None:
metric = self.left_canonical_metric
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
tangent_vec_metric_norm = metric.norm(tangent_vec)
tangent_vec_canonical_norm = np.linalg.norm(tangent_vec, axis=1)
if tangent_vec_canonical_norm.ndim == 1:
tangent_vec_canonical_norm = np.expand_dims(
tangent_vec_canonical_norm, axis=1)
mask_norm_0 = np.isclose(tangent_vec_metric_norm, 0)
mask_canonical_norm_0 = np.isclose(tangent_vec_canonical_norm, 0)
mask_0 = mask_norm_0 | mask_canonical_norm_0
mask_else = ~mask_0
mask_0 = np.squeeze(mask_0, axis=1)
mask_else = np.squeeze(mask_else, axis=1)
coef = np.empty_like(tangent_vec_metric_norm)
regularized_vec = tangent_vec
regularized_vec[mask_0] = tangent_vec[mask_0]
coef[mask_else] = (tangent_vec_metric_norm[mask_else]
/ tangent_vec_canonical_norm[mask_else])
regularized_vec[mask_else] = self.regularize(
coef[mask_else] * tangent_vec[mask_else])
regularized_vec[mask_else] = (regularized_vec[mask_else]
/ coef[mask_else])
return regularized_vec
def matrix_from_quaternion(self, quaternion):
"""
Convert a unit quaternion into a rotation vector.
"""
quaternion = vectorization.expand_dims(quaternion, to_ndim=2)
n_quaternions, _ = quaternion.shape
a, b, c, d = np.hsplit(quaternion, 4)
rot_mat = np.zeros((n_quaternions,) + (self.n,) * 2)
for i in range(n_quaternions):
# TODO(nina): vectorize by applying the composition of
# quaternions to the identity matrix
column_1 = [a[i] ** 2 + b[i] ** 2 - c[i] ** 2 - d[i] ** 2,
2 * b[i] * c[i] - 2 * a[i] * d[i],
2 * b[i] * d[i] + 2 * a[i] * c[i]]
column_2 = [2 * b[i] * c[i] + 2 * a[i] * d[i],
a[i] ** 2 - b[i] ** 2 + c[i] ** 2 - d[i] ** 2,
2 * c[i] * d[i] - 2 * a[i] * b[i]]
column_3 = [2 * b[i] * d[i] - 2 * a[i] * c[i],
2 * c[i] * d[i] + 2 * a[i] * b[i],
a[i] ** 2 - b[i] ** 2 - c[i] ** 2 + d[i] ** 2]
rot_mat[i] = np.hstack([column_1, column_2, column_3]).transpose()
assert rot_mat.ndim == 3
return rot_mat
def regularize(self, rot_vec):
"""
Regularize the norm of the rotation vector,
to be between 0 and pi, following the axis-angle
representation's convention.
If the angle angle is between pi and 2pi,
the function computes its complementary in 2pi and
inverts the direction of the rotation axis.
:param rot_vec: 3d vector
:returns self.regularized_rot_vec: 3d vector with: 0 < norm < pi
"""
rot_vec = vectorization.expand_dims(rot_vec, to_ndim=2)
assert self.belongs(rot_vec)
n_rot_vecs, _ = rot_vec.shape
angle = np.linalg.norm(rot_vec, axis=1)
regularized_rot_vec = rot_vec.astype('float64')
mask_not_0 = angle != 0
k = np.floor(angle / (2 * np.pi) + .5)
norms_ratio = np.zeros_like(angle).astype('float64')
norms_ratio[mask_not_0] = (
1. - 2. * np.pi * k[mask_not_0] / angle[mask_not_0])
norms_ratio[angle == 0] = 1
for i in range(n_rot_vecs):
regularized_rot_vec[i, :] = norms_ratio[i] * rot_vec[i]
assert regularized_rot_vec.ndim == 2
return regularized_rot_vec
def belongs(self, point):
"""
Check if point belongs to the Minkowski space.
"""
point = vectorization.expand_dims(point, to_ndim=2)
_, point_dim = point.shape
return point_dim == self.dimension
def closest_rotation_matrix(mat):
"""
Compute the closest - in terms of
the Frobenius norm - rotation matrix
of a given matrix M.
This avoids computational errors.
:param mat: 3x3 matrix
:returns rot_mat: 3x3 rotation matrix.
"""
mat = vectorization.expand_dims(mat, to_ndim=3)
_, mat_dim_1, mat_dim_2 = mat.shape
assert mat_dim_1 == mat_dim_2 == 3
mat_unitary_u, diag_s, mat_unitary_v = np.linalg.svd(mat)
rot_mat = np.matmul(mat_unitary_u, mat_unitary_v)
mask = np.where(np.linalg.det(rot_mat) < 0)
new_mat_diag_s = np.tile(np.diag([1, 1, -1]), sum(mask))
rot_mat[mask] = np.matmul(np.matmul(mat_unitary_u[mask],
new_mat_diag_s),
mat_unitary_v[mask])
assert rot_mat.ndim == 3
return rot_mat
def random_tangent_vec_uniform(self, n_samples=1, base_point=None):
if base_point is None:
base_point = np.eye(self.dimension)
base_point = vectorization.expand_dims(base_point, to_ndim=3)
n_base_points, _, _ = base_point.shape
assert n_base_points == n_samples or n_base_points == 1
sqrt_base_point = np.zeros_like(base_point)
for i in range(n_base_points):
sqrt_base_point[i] = scipy.linalg.sqrtm(base_point[i])
tangent_vec_at_id = (2 * np.random.rand(n_samples,
self.dimension,
self.dimension)
- 1)
tangent_vec_at_id = (tangent_vec_at_id
+ np.transpose(tangent_vec_at_id,
axes=(0, 2, 1)))
tangent_vec = np.matmul(sqrt_base_point, tangent_vec_at_id)
tangent_vec = np.matmul(tangent_vec, sqrt_base_point)
return tangent_vec
def belongs(self, point):
"""
Check that the transformation belongs to
the special euclidean group.
"""
point = vectorization.expand_dims(point, to_ndim=2)
_, point_dim = point.shape
return point_dim == self.dimension
def belongs(self, rot_vec):
"""
Check that a vector belongs to the
special orthogonal group.
"""
rot_vec = vectorization.expand_dims(rot_vec, to_ndim=2)
_, vec_dim = rot_vec.shape
return vec_dim == self.dimension
def point_on_geodesic(t):
t = vectorization.expand_dims(t, to_ndim=1)
t = vectorization.expand_dims(t, to_ndim=2, axis=1)
new_initial_point = vectorization.expand_dims(initial_point,
to_ndim=point_ndim +
1)
new_initial_tangent_vec = vectorization.expand_dims(
initial_tangent_vec, to_ndim=point_ndim + 1)
n_times_t, _ = t.shape
tangent_vec_shape = new_initial_tangent_vec.shape[1:]
tangent_vecs = np.zeros((n_times_t, ) + tangent_vec_shape)
for i in range(n_times_t):
tangent_vecs[i] = t[i] * new_initial_tangent_vec
point_at_time_t = self.exp(tangent_vec=tangent_vecs,
base_point=new_initial_point)
return point_at_time_t
def plot(points, ax=None, space=None, **point_draw_kwargs):
"""
Plot points in the 3D Special Euclidean Group,
by showing them as trihedrons.
"""
if space not in IMPLEMENTED:
raise NotImplementedError(
'The plot function is not implemented'
' for space {}. The spaces available for visualization'
' are: {}.'.format(space, IMPLEMENTED))
if points is None:
raise ValueError("No points given for plotting.")
points = vectorization.expand_dims(points, to_ndim=2)
if ax is None:
if space is 'SE3_GROUP':
ax_s = AX_SCALE * np.amax(np.abs(points[:, 3:6]))
elif space is 'SO3_GROUP':
ax_s = AX_SCALE * np.amax(np.abs(points[:, :3]))
else:
ax_s = AX_SCALE
if space is 'H2':
ax = plt.subplot(aspect='equal')
plt.setp(ax,
xlim=(-ax_s, ax_s),
ylim=(-ax_s, ax_s),
xlabel='X', ylabel='Y')
else:
ax = plt.subplot(111, projection='3d', aspect='equal')
plt.setp(ax,
xlim=(-ax_s, ax_s),
ylim=(-ax_s, ax_s),
zlim=(-ax_s, ax_s),
xlabel='X', ylabel='Y', zlabel='Z')
if space in ('SO3_GROUP', 'SE3_GROUP'):
trihedrons = convert_to_trihedron(points, space=space)
for t in trihedrons:
t.draw(ax, **point_draw_kwargs)
elif space is 'S2':
sphere = Sphere()
sphere.add_points(points)
sphere.draw(ax)
elif space is 'H2':
poincare_disk = PoincareDisk()
poincare_disk.add_points(points)
poincare_disk.draw(ax)
return ax
def quaternion_from_matrix(self, rot_mat):
"""
Convert a rotation matrix into a unit quaternion.
"""
rot_mat = vectorization.expand_dims(rot_mat, to_ndim=3)
rot_vec = self.rotation_vector_from_matrix(rot_mat)
quaternion = self.quaternion_from_rotation_vector(rot_vec)
assert quaternion.ndim == 2
return quaternion
def extrinsic_to_intrinsic_coords(self, point_extrinsic):
"""
From the extrinsic coordinates in Minkowski space,
to the extrinsic coordinates in Hyperbolic space.
"""
point_extrinsic = vectorization.expand_dims(point_extrinsic, to_ndim=2)
assert np.all(self.belongs(point_extrinsic))
point_intrinsic = point_extrinsic[:, 1:]
assert point_intrinsic.ndim == 2
return point_intrinsic
def is_symmetric(mat, tolerance=TOLERANCE):
"""Check if a matrix is symmetric."""
mat = vectorization.expand_dims(mat, to_ndim=3)
n_mats, _, _ = mat.shape
mask = np.zeros(n_mats, dtype=bool)
for i in range(n_mats):
mask[i] = np.allclose(mat[i], np.transpose(mat[i]),
atol=tolerance)
return mask
def extrinsic_to_intrinsic_coords(self, point_extrinsic):
"""
From the extrinsic coordinates in Euclidean space,
to some intrinsic coordinates in Hypersphere.
"""
point_extrinsic = vectorization.expand_dims(point_extrinsic, to_ndim=2)
assert np.all(self.belongs(point_extrinsic))
point_intrinsic = point_extrinsic[:, 1:]
assert point_intrinsic.ndim == 2
return point_intrinsic
def group_exp(self, tangent_vec, base_point=None):
"""
Compute the group exponential of vector tangent_vector.
"""
base_point = self.regularize(base_point)
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
point = super(SpecialOrthogonalGroup, self).group_exp(
tangent_vec=tangent_vec,
base_point=base_point)
point = self.regularize(point)
return point
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point=None):
"""
Inner product defined by the Riemannian metric at point base_point
between tangent vectors tangent_vec_a and tangent_vec_b.
"""
tangent_vec_a = vectorization.expand_dims(tangent_vec_a, to_ndim=2)
tangent_vec_b = vectorization.expand_dims(tangent_vec_b, to_ndim=2)
inner_prod_mat = self.inner_product_matrix(base_point)
inner_prod_mat = vectorization.expand_dims(inner_prod_mat, to_ndim=3)
n_tangent_vecs_a = tangent_vec_a.shape[0]
n_tangent_vecs_b = tangent_vec_b.shape[0]
n_inner_prod_mats = inner_prod_mat.shape[0]
bool_all_same_n = (n_tangent_vecs_a == n_tangent_vecs_b ==
n_inner_prod_mats)
bool_a = n_tangent_vecs_a == 1
bool_b = n_tangent_vecs_b == 1
bool_inner_prod = n_inner_prod_mats == 1
assert (bool_all_same_n
or n_tangent_vecs_a == n_tangent_vecs_b and bool_inner_prod
or n_tangent_vecs_a == n_inner_prod_mats and bool_b
or n_tangent_vecs_b == n_inner_prod_mats and bool_a
or bool_a and bool_b or bool_a and bool_inner_prod
or bool_b and bool_inner_prod)
n_inner_prods = np.amax(
[n_tangent_vecs_a, n_tangent_vecs_b, n_inner_prod_mats], axis=0)
inner_prod = np.zeros((n_inner_prods, 1))
for i in range(n_inner_prods):
tangent_vec_a_i = (tangent_vec_a[0]
if n_tangent_vecs_a == 1 else tangent_vec_a[i])
tangent_vec_b_i = (tangent_vec_b[0]
if n_tangent_vecs_b == 1 else tangent_vec_b[i])
inner_prod_mat_i = (inner_prod_mat[0] if n_inner_prod_mats == 1
else inner_prod_mat[i])
inner_prod[i] = np.dot(np.dot(tangent_vec_a_i, inner_prod_mat_i),
tangent_vec_b_i.transpose())
return inner_prod
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point):
"""
Compute the inner product of tangent_vec_a and tangent_vec_b
at point base_point using the affine invariant Riemannian metric.
"""
inv_base_point = np.linalg.inv(base_point)
aux_a = np.matmul(inv_base_point, tangent_vec_a)
aux_b = np.matmul(inv_base_point, tangent_vec_b)
inner_product = np.trace(np.matmul(aux_a, aux_b), axis1=1, axis2=2)
inner_product = vectorization.expand_dims(inner_product,
to_ndim=2, axis=1)
return inner_product
def exponential_matrix(self, rot_vec):
"""
Compute the exponential of the rotation matrix
represented by rot_vec.
:param rot_vec: 3D rotation vector
:returns exponential_mat: 3x3 matrix
"""
rot_vec = self.rotations.regularize(rot_vec)
n_rot_vecs, _ = rot_vec.shape
angle = np.linalg.norm(rot_vec, axis=1)
angle = vectorization.expand_dims(angle, to_ndim=2, axis=1)
skew_rot_vec = so_group.skew_matrix_from_vector(rot_vec)
coef_1 = np.empty_like(angle)
coef_2 = np.empty_like(coef_1)
mask_0 = np.equal(angle, 0)
mask_0 = np.squeeze(mask_0, axis=1)
mask_close_to_0 = np.isclose(angle, 0)
mask_close_to_0 = np.squeeze(mask_close_to_0, axis=1)
mask_else = ~mask_0 & ~mask_close_to_0
coef_1[mask_close_to_0] = (1. / 2.
- angle[mask_close_to_0] ** 2 / 24.)
coef_2[mask_close_to_0] = (1. / 6.
- angle[mask_close_to_0] ** 3 / 120.)
# TODO(nina): check if the discountinuity as 0 is expected.
coef_1[mask_0] = 0
coef_2[mask_0] = 0
coef_1[mask_else] = (angle[mask_else] ** (-2)
* (1. - np.cos(angle[mask_else])))
coef_2[mask_else] = (angle[mask_else] ** (-2)
* (1. - (np.sin(angle[mask_else])
/ angle[mask_else])))
term_1 = np.zeros((n_rot_vecs, self.n, self.n))
term_2 = np.zeros_like(term_1)
for i in range(n_rot_vecs):
term_1[i] = np.eye(self.n) + skew_rot_vec[i] * coef_1[i]
term_2[i] = np.matmul(skew_rot_vec[i], skew_rot_vec[i]) * coef_2[i]
exponential_mat = term_1 + term_2
assert exponential_mat.ndim == 3
return exponential_mat
def log(self, point, base_point=None):
"""
Riemannian logarithm at point base_point
of tangent vector tangent_vec wrt the Riemannian metric.
"""
point = vectorization.expand_dims(point, to_ndim=2)
base_point = vectorization.expand_dims(base_point, to_ndim=2)
n_points, _ = point.shape
n_base_points, point_dim = base_point.shape
assert (n_points == n_base_points or n_points == 1
or n_base_points == 1)
n_logs = np.maximum(n_points, n_base_points)
log = np.zeros((n_logs, point_dim))
for i in range(n_logs):
base_point_i = (base_point[0]
if n_base_points == 1 else base_point[i])
point_i = (point[0] if n_points == 1 else point[i])
log[i] = self.log_basis(point_i, base_point_i)
return log
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
"""
Regularize an element of the group SE(3),
by extracting the rotation vector r from the input [r t]
and using self.rotations.regularize.
:param point: 6d vector, element in SE(3) represented as [r t].
:returns self.regularized_point: 6d vector, element in SE(3)
with self.regularized rotation.
"""
if metric is None:
metric = self.left_canonical_metric
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
base_point = vectorization.expand_dims(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_tangent_vec = tangent_vec[:, :dim_rotations]
rot_base_point = base_point[:, :dim_rotations]
metric_mat = metric.inner_product_mat_at_identity
rot_metric_mat = metric_mat[:dim_rotations, :dim_rotations]
rot_metric = InvariantMetric(
group=rotations,
inner_product_mat_at_identity=rot_metric_mat,
left_or_right=metric.left_or_right)
regularized_vec = np.zeros_like(tangent_vec)
regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric)
regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:]
return regularized_vec
def belongs(self, mat, tolerance=TOLERANCE):
"""
Check if a matrix belongs to the manifold of
symmetric positive definite matrices.
"""
mat = vectorization.expand_dims(mat, to_ndim=3)
n_mats, mat_dim, _ = mat.shape
mask_is_symmetric = is_symmetric(mat, tolerance=tolerance)
eigenvalues = np.zeros((n_mats, mat_dim))
eigenvalues[mask_is_symmetric] = np.linalg.eigvalsh(
mat[mask_is_symmetric])
mask_pos_eigenvalues = np.all(eigenvalues > 0)
return mask_is_symmetric & mask_pos_eigenvalues
def exp_from_identity(self, tangent_vec):
"""
Compute the Riemannian exponential from the identity of the
Lie group of tangent vector tangent_vec.
"""
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
if self.left_or_right == 'left':
exp = self.left_exp_from_identity(tangent_vec)
else:
opp_left_exp = self.left_exp_from_identity(-tangent_vec)
exp = self.group.inverse(opp_left_exp)
exp = self.group.regularize(exp)
return exp
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""
From the intrinsic coordinates in the hyperbolic space,
to the extrinsic coordinates in Minkowski space.
"""
point_intrinsic = vectorization.expand_dims(point_intrinsic, to_ndim=2)
n_points, _ = point_intrinsic.shape
dimension = self.dimension
point_extrinsic = np.zeros((n_points, dimension + 1))
point_extrinsic[:, 1:dimension + 1] = point_intrinsic[:, 0:dimension]
point_extrinsic[:, 0] = np.sqrt(
1. + np.linalg.norm(point_intrinsic, axis=1)**2)
assert np.all(self.belongs(point_extrinsic))
assert point_extrinsic.ndim == 2
return point_extrinsic
def belongs(self, mat):
"""
Check if mat belongs to the Matrix Lie group.
Note:
- By default, check that the matrix is invertible.
- Need override for any matrix Lie group
that is not GL(n).
"""
mat = vectorization.expand_dims(mat, to_ndim=3)
n_mats, _, _ = mat.shape
mat_rank = np.zeros((n_mats, 1))
for i in range(n_mats):
mat_rank[i] = np.linalg.matrix_rank(mat[i])
return mat_rank == self.n
