def exp(self, tangent_vec, base_point):
"""Compute the Log-Euclidean exponential map.
Compute the Riemannian exponential at point base_point
of tangent vector tangent_vec wrt the Log-Euclidean metric.
This gives a symmetric positive definite matrix.
Parameters
----------
tangent_vec : array-like, shape=[n_samples, n, n]
base_point : array-like, shape={n_samples, n, n]
Returns
-------
exp : array-like, shape=[n_samples, n, n]
"""
ndim = gs.maximum(gs.ndim(tangent_vec), gs.ndim(base_point))
log_base_point = gs.linalg.logm(base_point)
dlog_tangent_vec = self.space.differential_log(tangent_vec, base_point)
exp = gs.linalg.expm(log_base_point + dlog_tangent_vec)
if ndim == 2:
return exp[0]
return exp
def log_from_identity(self, point):
"""
Riemannian logarithm of a point wrt the identity.
"""
point = self.group.regularize(point)
if self.left_or_right == 'left':
log = self.left_log_from_identity(point)
else:
inv_point = self.group.inverse(point)
left_log = self.left_log_from_identity(inv_point)
log = -left_log
assert gs.ndim(log) == 2
return log
def rotation_matrix(theta):
"""Construct the rotation matrix associated to the angle theta.
Parameters
----------
theta : float
Rotation angle.
Returns
-------
rot : array-like, shape=[2, 2]
2D rotation matrix of angle theta.
"""
if gs.ndim(gs.array(theta)) <= 1:
theta = gs.array([theta])
return Localization.group.rotations.matrix_from_rotation_vector(theta)
def transpose(mat):
"""Return the transpose of matrices.
Parameters
----------
mat : array-like, shape=[..., n, n]
Matrix.
Returns
-------
transpose : array-like, shape=[..., n, n]
Transposed matrix.
"""
is_vectorized = (gs.ndim(gs.array(mat)) == 3)
axes = (0, 2, 1) if is_vectorized else (1, 0)
return gs.transpose(mat, axes)
def regularize(self, point, point_type=None):
"""
In 3D, 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.
"""
if point_type is None:
point_type = self.default_point_type
if point_type == 'vector':
point = gs.to_ndarray(point, to_ndim=2)
assert self.belongs(point, point_type)
n_points, _ = point.shape
regularized_point = gs.copy(point)
if self.n == 3:
angle = gs.linalg.norm(regularized_point, axis=1)
mask_0 = gs.isclose(angle, 0)
mask_not_0 = ~mask_0
mask_pi = gs.isclose(angle, gs.pi)
k = gs.floor(angle / (2 * gs.pi) + .5)
norms_ratio = gs.zeros_like(angle)
norms_ratio[mask_not_0] = (
1. - 2. * gs.pi * k[mask_not_0] / angle[mask_not_0])
norms_ratio[mask_0] = 1
norms_ratio[mask_pi] = gs.pi / angle[mask_pi]
for i in range(n_points):
regularized_point[i, :] = (norms_ratio[i] *
regularized_point[i, :])
else:
# TODO(nina): regularization needed in nD?
regularized_point = gs.copy(point)
assert gs.ndim(regularized_point) == 2
elif point_type == 'matrix':
point = gs.to_ndarray(point, to_ndim=3)
# TODO(nina): regularization for matrices?
regularized_point = gs.copy(point)
return regularized_point
def log(self, point, base_point=None):
"""Compute Riemannian logarithm of a point wrt a base point.
Parameters
----------
point
base_point
Returns
-------
log
"""
if base_point is None:
base_point = self.group.identity
base_point = self.group.regularize(base_point)
if base_point is self.group.identity:
return self.log_from_identity(point)
point = self.group.regularize(point)
n_points, _ = point.shape
n_base_points, _ = base_point.shape
if self.left_or_right == 'left':
point_near_id = self.group.compose(self.group.inverse(base_point),
point)
else:
point_near_id = self.group.compose(point,
self.group.inverse(base_point))
log_from_id = self.log_from_identity(point_near_id)
jacobian = self.group.jacobian_translation(
base_point, left_or_right=self.left_or_right)
n_logs, _ = log_from_id.shape
n_jacobians, _, _ = jacobian.shape
if n_logs == 1:
log_from_id = gs.tile(log_from_id, (n_jacobians, 1))
if n_jacobians == 1:
jacobian = gs.tile(jacobian, (n_logs, 1, 1))
log = gs.einsum('ij,ijk->ik', log_from_id,
gs.transpose(jacobian, axes=(0, 2, 1)))
assert gs.ndim(log) == 2
return log
def test_geodesic_output_shape(self, metric, start_point, end_point, t):
geodesic = metric.geodesic(start_point, end_point)
is_list = type(start_point) is list or type(end_point) is list
n_geo = max(
len(start_point) if type(start_point) is list else 1,
len(end_point) if type(end_point) is list else 1,
)
pt = start_point[0] if type(start_point) is list else start_point
d_array = gs.ndim(pt.to_array())
n_t = len(t) if type(t) is list else 1
results = self._convert_to_gs_array(geodesic(t), is_list)
self.assertTrue(results.ndim == d_array + 1 + int(is_list))
self.assertTrue(results.shape[-d_array - 1] == n_t)
if is_list:
self.assertTrue(results.shape[-d_array - 2] == n_geo)
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point=None):
"""
Inner product between two tangent vectors at a base point.
Parameters
----------
tangent_vec_a: array-like, shape=[n_samples, dimension]
or shape=[1, dimension]
tangent_vec_b: array-like, shape=[n_samples, dimension]
or shape=[1, dimension]
base_point: array-like, shape=[n_samples, dimension]
or shape=[1, dimension]
"""
tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=2)
tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=2)
n_tangent_vec_a = gs.shape(tangent_vec_a)[0]
n_tangent_vec_b = gs.shape(tangent_vec_b)[0]
inner_prod_mat = self.inner_product_matrix(base_point)
inner_prod_mat = gs.to_ndarray(inner_prod_mat, to_ndim=3)
n_mats = gs.shape(inner_prod_mat)[0]
n_inner_prod = gs.maximum(n_tangent_vec_a, n_tangent_vec_b)
n_inner_prod = gs.maximum(n_inner_prod, n_mats)
n_tiles_a = gs.divide(n_inner_prod, n_tangent_vec_a)
n_tiles_a = gs.cast(n_tiles_a, gs.int32)
tangent_vec_a = gs.tile(tangent_vec_a, [n_tiles_a, 1])
n_tiles_b = gs.divide(n_inner_prod, n_tangent_vec_b)
n_tiles_b = gs.cast(n_tiles_b, gs.int32)
tangent_vec_b = gs.tile(tangent_vec_b, [n_tiles_b, 1])
n_tiles_mat = gs.divide(n_inner_prod, n_mats)
n_tiles_mat = gs.cast(n_tiles_mat, gs.int32)
inner_prod_mat = gs.tile(inner_prod_mat, [n_tiles_mat, 1, 1])
aux = gs.einsum('nj,njk->nk', tangent_vec_a, inner_prod_mat)
inner_prod = gs.einsum('nk,nk->n', aux, tangent_vec_b)
inner_prod = gs.to_ndarray(inner_prod, to_ndim=2, axis=1)
assert gs.ndim(inner_prod) == 2, inner_prod.shape
return inner_prod
def belongs(self, point):
"""Evaluate if a point belongs to the Minkowski space.
Parameters
----------
point : array-like, shape=[n_samples, dim]
Input points.
Returns
-------
belongs : array-like, shape=[n_samples,]
"""
point_dim = point.shape[-1]
belongs = point_dim == self.dim
if gs.ndim(point) == 2:
belongs = gs.tile([belongs], (point.shape[0], ))
return belongs
def belongs(self, point):
"""Evaluate if a point belongs to the Euclidean space.
Parameters
----------
point : array-like, shape=[..., dim]
Point to evaluate.
Returns
-------
belongs : array-like, shape=[...,]
"""
point_dim = point.shape[-1]
belongs = point_dim == self.dim
if gs.ndim(point) == 2:
belongs = gs.tile([belongs], (point.shape[0], ))
return belongs
def christoffels(self, point, point_type="spherical"):
"""Compute the Christoffel symbols at a point.
Only implemented in dimension 2 and for spherical coordinates.
Parameters
----------
point : array-like, shape=[..., dim]
Point on hypersphere where the Christoffel symbols are computed.
point_type: str, {'spherical', 'intrinsic', 'extrinsic'}
Coordinates in which to express the Christoffel symbols.
Optional, default: 'spherical'.
Returns
-------
christoffel : array-like, shape=[..., contravariant index, 1st
covariant index, 2nd covariant index]
Christoffel symbols at point.
"""
if self.dim != 2 or point_type != "spherical":
raise NotImplementedError(
"The Christoffel symbols are only implemented"
" for spherical coordinates in the 2-sphere"
)
point = gs.to_ndarray(point, to_ndim=2)
christoffel = []
for sample in point:
gamma_0 = gs.array([[0, 0], [0, -gs.sin(sample[0]) * gs.cos(sample[0])]])
gamma_1 = gs.array(
[
[0, gs.cos(sample[0]) / gs.sin(sample[0])],
[gs.cos(sample[0]) / gs.sin(sample[0]), 0],
]
)
christoffel.append(gs.stack([gamma_0, gamma_1]))
christoffel = gs.stack(christoffel)
if gs.ndim(christoffel) == 4 and gs.shape(christoffel)[0] == 1:
christoffel = gs.squeeze(christoffel, axis=0)
return christoffel
def is_symmetric(cls, mat, atol=TOLERANCE):
"""Check if a matrix is symmetric.
Parameters
----------
mat : array-like, shape=[..., n, n]
Matrix.
atol : float
Absolute tolerance.
Optional, default: 1e-5.
Returns
-------
is_sym : array-like, shape=[...,]
Boolean evaluating if the matrix is symmetric.
"""
is_square = cls.is_square(mat)
if not is_square:
is_vectorized = (gs.ndim(gs.array(mat)) == 3)
return gs.array([False] * len(mat)) if is_vectorized else False
return cls.equal(mat, cls.transpose(mat), atol)
def is_strictly_upper_triangular(cls, mat, atol=gs.atol):
"""Check if a matrix is strictly upper triangular.
Parameters
----------
mat : array-like, shape=[..., n, n]
Matrix.
atol : float
Absolute tolerance.
Optional, default : backend atol.
Returns
-------
is_strictly_triu : array-like, shape=[...,]
Boolean evaluating if the matrix is strictly upper triangular
"""
is_square = cls.is_square(mat)
if not is_square:
is_vectorized = gs.ndim(gs.array(mat)) == 3
return gs.array([False] * len(mat)) if is_vectorized else False
return cls.equal(mat, gs.triu(mat, k=1))
def left_log_from_identity(self, point):
"""
Riemannian logarithm of a point wrt the identity associated
to the left-invariant metric.
If the method is called by a right-invariant metric, it uses the
left-invariant metric associated to the same inner-product matrix
at the identity.
"""
point = self.group.regularize(point)
inner_prod_mat = self.inner_product_mat_at_identity
inv_inner_prod_mat = gs.linalg.inv(inner_prod_mat)
sqrt_inv_inner_prod_mat = gs.linalg.sqrtm(inv_inner_prod_mat)
assert sqrt_inv_inner_prod_mat.shape == ((1, ) +
(self.group.dimension, ) * 2)
aux = gs.squeeze(sqrt_inv_inner_prod_mat, axis=0)
log = gs.matmul(point, aux)
log = self.group.regularize_tangent_vec_at_identity(tangent_vec=log,
metric=self)
assert gs.ndim(log) == 2
return log
def belongs(self, point, atol=gs.atol):
"""Evaluate if a point belongs to the Euclidean space.
Parameters
----------
point : array-like, shape=[..., dim]
Point to evaluate.
atol : float
Unused.
Returns
-------
belongs : array-like, shape=[...,]
Boolean evaluating if point belongs to the Euclidean space.
"""
point_dim = point.shape[-1]
belongs = point_dim == self.dim
if gs.ndim(point) == 2:
belongs = gs.tile([belongs], (point.shape[0], ))
return belongs
def matching(self, base_graph, graph_to_permute):
"""Match graphs.
Parameters
----------
base_graph : list of Graph or array-like, shape=[..., n, n].
Base graph.
graph_to_permute : list of Graph or array-like, shape=[..., n, n].
Graph to align.
"""
base_graph, graph_to_permute = gs.broadcast_arrays(
base_graph, graph_to_permute)
is_single = gs.ndim(base_graph) == 2
if is_single:
base_graph = gs.expand_dims(base_graph, 0)
graph_to_permute = gs.expand_dims(graph_to_permute, 0)
perm = self.matcher.match(base_graph, graph_to_permute)
self.perm_ = gs.array(perm[0]) if is_single else gs.array(perm)
return self.perm_
def belongs(self, point, atol=gs.atol):
"""Give a one-liner description of the method.
For example: Evaluate if a point belongs to MyManifold.
The signature of the method should match the signature of the parent
method, in this case the method `belongs` from the class `Manifold`.
List the parameters of the method.
In what follows, the ellipsis ... indicate either nothing
or any number n of elements, i.e. shape=[..., dim] means
shape=[dim] or shape=[n, dim] for any n.
All functions/methods of geomstats should work for any number
of inputs. In the case of the method `belongs`, it means:
for any number of input points.
For example:
Parameters
----------
point : array-like, shape=[..., dim]
Point to evaluate.
atol : float
Tolerance, unused.
Optional, default: backend atol
List the outputs of the method.
For example:
Returns
-------
belongs : array-like, shape=[...,]
Boolean evaluating if point belongs to the manifold.
"""
# Perform operations to check if point belongs
# to the manifold, for example:
belongs = point.shape[-1] == self.dim
if gs.ndim(point) == 2:
belongs = gs.tile([belongs], (point.shape[0], ))
return belongs
def predict(self, X):
"""Compute closest neighbor according to riemannian_metric.
Parameters
----------
X : array-like, shape=[n_samples, dim]
if point_type='vector'
shape=[n_samples, n, n]
if point_type='matrix'
Test data, where n_samples is the number of samples
and n_features is the number of features.
Returns
-------
y : array-like, shape=[n_samples,]
Predicted labels.
"""
indices = self.riemannian_metric.closest_neighbor_index(X, self.mean_estimates_)
if gs.ndim(indices) == 0:
indices = gs.expand_dims(indices, 0)
return gs.take(self.classes_, indices)
def matrix_from_quaternion(self, quaternion):
"""
Convert a unit quaternion into a rotation vector.
"""
assert self.n == 3, ('The quaternion representation does not exist'
' for rotations in %d dimensions.' % self.n)
quaternion = gs.to_ndarray(quaternion, to_ndim=2)
n_quaternions, _ = quaternion.shape
w, x, y, z = gs.hsplit(quaternion, 4)
rot_mat = gs.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 = [
w[i]**2 + x[i]**2 - y[i]**2 - z[i]**2,
2 * x[i] * y[i] - 2 * w[i] * z[i],
2 * x[i] * z[i] + 2 * w[i] * y[i]
]
column_2 = [
2 * x[i] * y[i] + 2 * w[i] * z[i],
w[i]**2 - x[i]**2 + y[i]**2 - z[i]**2,
2 * y[i] * z[i] - 2 * w[i] * x[i]
]
column_3 = [
2 * x[i] * z[i] - 2 * w[i] * y[i],
2 * y[i] * z[i] + 2 * w[i] * x[i],
w[i]**2 - x[i]**2 - y[i]**2 + z[i]**2
]
rot_mat[i] = gs.hstack([column_1, column_2, column_3]).transpose()
assert gs.ndim(rot_mat) == 3
return rot_mat
def upper_triangular_matrix_from_vector(point):
"""Compute the upper triangular matrix representation of the vector.
The 3D Heisenberg group can also be represented as 3x3 upper triangular
matrices. This function computes this representation of the vector
'point'.
Parameters
----------
point : array-like, shape=[..., 3]
Point in the vector-represention.
Returns
-------
upper_triangular_mat : array-like, shape=[..., 3, 3]
Upper triangular matrix.
"""
n_points = gs.ndim(point)
element_02 = point[..., 2] + 1 / 2 * point[..., 0] * point[..., 1]
if n_points == 1:
modified_point = gs.array(
[1, point[0], element_02, 1, point[1], 1])
else:
modified_point = gs.stack(
(
gs.ones(n_points),
point[..., 0],
element_02,
gs.ones(n_points),
point[..., 1],
gs.ones(n_points),
),
axis=1,
)
return gs.triu(SymmetricMatrices.from_vector(modified_point))
def inner_product_matrix(self, base_point=None):
"""Compute inner product matrix at the tangent space at a base point.
Parameters
----------
base_point : array-like, shape=[n_samples, dimension], optional
Point in the group (the default is identity).
Returns
-------
metric_mat : array-like, shape=[n_samples, dimension, dimension]
The metric matrix at base_point.
"""
if self.group.default_point_type == 'matrix':
raise NotImplementedError(
'inner_product_matrix not implemented for Lie groups'
' whose elements are represented as matrices.')
if base_point is None:
base_point = self.group.identity
base_point = self.group.regularize(base_point)
jacobian = self.group.jacobian_translation(
point=base_point,
left_or_right=self.left_or_right)
assert gs.ndim(jacobian) == 3
inv_jacobian = gs.linalg.inv(jacobian)
inv_jacobian_transposed = gs.transpose(inv_jacobian, axes=(0, 2, 1))
n_base_points = base_point.shape[0]
inner_product_mat_at_id = gs.array(
[self.inner_product_mat_at_identity[0]] * n_base_points)
metric_mat = gs.matmul(
inv_jacobian_transposed, inner_product_mat_at_id)
metric_mat = gs.matmul(metric_mat, inv_jacobian)
return metric_mat
def from_vector_to_diagonal_matrix(vector, num_diag=0):
"""Create diagonal matrices from rows of a matrix.
Parameters
----------
vector : array-like, shape=[m, n]
num_diag : int
number of diagonal in result matrix. If 0, the result matrix is a
diagonal matrix; if positive, the result matrix has an upper-right
non-zero diagonal; if negative, the result matrix has a lower-left
non-zero diagonal.
Optional, Default: 0.
Returns
-------
diagonals : array-like, shape=[m, n, n]
3-dimensional array where the `i`-th n-by-n array `diagonals[i, :, :]`
is a diagonal matrix containing the `i`-th row of `vector`.
"""
num_columns = gs.shape(vector)[-1]
identity = gs.eye(num_columns)
identity = gs.cast(identity, vector.dtype)
diagonals = gs.einsum("...i,ij->...ij", vector, identity)
diagonals = gs.to_ndarray(diagonals, to_ndim=3)
num_lines = diagonals.shape[0]
if num_diag > 0:
left_zeros = gs.zeros((num_lines, num_columns, num_diag))
lower_zeros = gs.zeros((num_lines, num_diag, num_columns + num_diag))
diagonals = gs.concatenate((left_zeros, diagonals), axis=2)
diagonals = gs.concatenate((diagonals, lower_zeros), axis=1)
elif num_diag < 0:
num_diag = gs.abs(num_diag)
right_zeros = gs.zeros((num_lines, num_columns, num_diag))
upper_zeros = gs.zeros((num_lines, num_diag, num_columns + num_diag))
diagonals = gs.concatenate((diagonals, right_zeros), axis=2)
diagonals = gs.concatenate((upper_zeros, diagonals), axis=1)
return gs.squeeze(diagonals) if gs.ndim(vector) == 1 else diagonals
def flatten(self, mat):
"""Return a flattened form of the matrix.
Flatten a matrix (compatible with vectorization on data axis 0).
The reverse operation is reshape. These operations are often called
matrix vectorization / matricization in mathematics
(https://en.wikipedia.org/wiki/Tensor_reshaping).
The names flatten / reshape were chosen to avoid confusion with
vectorization on data axis 0.
Parameters
----------
mat : array-like, shape=[..., m, n]
Matrix.
Returns
-------
vec : array-like, shape=[..., m * n]
Flatten copy of mat.
"""
is_data_vectorized = gs.ndim(gs.array(mat)) == 3
shape = ((mat.shape[0], self.m * self.n) if is_data_vectorized else
(self.m * self.n, ))
return gs.reshape(mat, shape)
def point_on_landmarks(tangent_vec):
assert gs.ndim(tangent_vec) >= 2
exp = self.exp(
tangent_vec=tangent_vec,
base_landmarks=new_initial_landmarks)
return exp
def point_on_landmarks(tangent_vec):
if gs.ndim(tangent_vec) < 2:
raise RuntimeError
exp = self.exp(tangent_vec=tangent_vec,
base_point=new_initial_landmarks)
return exp
def grad(y_pred,
y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""Closed-form for the gradient of pose_loss.
Parameters
----------
y_pred : array-like
Prediction on SE(3).
y_true : array-like
Ground-truth on SE(3).
metric : RiemannianMetric
Metric used to compute the loss and gradient.
representation : str, {'vector', 'matrix'}
Representation chosen for points in SE(3).
Returns
-------
lie_grad : array-like
Tangent vector at point y_pred.
"""
if gs.ndim(y_pred) == 1:
y_pred = gs.expand_dims(y_pred, axis=0)
if gs.ndim(y_true) == 1:
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'vector':
lie_grad = lie_group.grad(y_pred, y_true, SE3, metric)
if representation == 'quaternion':
y_pred_rot_vec = SO3.rotation_vector_from_quaternion(y_pred[:, :4])
y_pred_pose = gs.hstack([y_pred_rot_vec, y_pred[:, 4:]])
y_true_rot_vec = SO3.rotation_vector_from_quaternion(y_true[:, :4])
y_true_pose = gs.hstack([y_true_rot_vec, y_true[:, 4:]])
lie_grad = lie_group.grad(y_pred_pose, y_true_pose, SE3, metric)
quat_scalar = y_pred[:, :1]
quat_vec = y_pred[:, 1:4]
quat_vec_norm = gs.linalg.norm(quat_vec, axis=1)
quat_sq_norm = quat_vec_norm**2 + quat_scalar**2
quat_arctan2 = gs.arctan2(quat_vec_norm, quat_scalar)
differential_scalar = -2 * quat_vec / (quat_sq_norm)
differential_vec = (
2 *
(quat_scalar / quat_sq_norm - 2 * quat_arctan2 / quat_vec_norm) *
(gs.einsum('ni,nj->nij', quat_vec, quat_vec) / quat_vec_norm *
quat_vec_norm) + 2 * quat_arctan2 / quat_vec_norm * gs.eye(3))
differential_scalar_t = gs.transpose(differential_scalar, axes=(1, 0))
upper_left_block = gs.hstack(
(differential_scalar_t, differential_vec[0]))
upper_right_block = gs.zeros((3, 3))
lower_right_block = gs.eye(3)
lower_left_block = gs.zeros((3, 4))
top = gs.hstack((upper_left_block, upper_right_block))
bottom = gs.hstack((lower_left_block, lower_right_block))
differential = gs.vstack((top, bottom))
differential = gs.expand_dims(differential, axis=0)
lie_grad = gs.einsum('ni,nij->ni', lie_grad, differential)
lie_grad = gs.squeeze(lie_grad, axis=0)
return lie_grad
def is_scalar_vectorized(scalar):
is_scalar_vec = gs.ndim(scalar) == 2
has_dim_1 = gs.shape(scalar)[-1] == 1
result = is_scalar_vec and has_dim_1
result = helper.to_scalar(result)
return result
def is_matrix_vectorized_with_point_type(obj, point, point_type=None):
is_matrix_vec = gs.ndim(point) == 3
is_matrix_vec = helper.to_scalar(is_matrix_vec)
return is_matrix_vec
def is_matrix_vectorized(matrix):
is_matrix_vec = gs.ndim(matrix) == 3
is_matrix_vec = helper.to_scalar(is_matrix_vec)
return is_matrix_vec
def is_vector_vectorized(vector):
is_vector_vec = gs.ndim(vector) == 2
is_vector_vec = helper.to_scalar(is_vector_vec)
return is_vector_vec
