def _exp_translation_transform(self, rot_vec):
base_1 = gs.eye(2)
base_2 = self.rotations.skew_matrix_from_vector(gs.ones(1))
cos_coef = rot_vec * utils.taylor_exp_even_func(
rot_vec**2, utils.cosc_close_0, order=3)
sin_coef = utils.taylor_exp_even_func(rot_vec**2,
utils.sinc_close_0,
order=3)
sin_term = gs.einsum('...i,...jk->...jk', sin_coef, base_1)
cos_term = gs.einsum('...i,...jk->...jk', cos_coef, base_2)
transform = sin_term + cos_term
return transform
def propagation_jacobian(state, sensor_input):
r"""Compute the Jacobian associated to the affine propagation..
The Jacobian is given by :math:`\begin{bmatrix} 1 & dt \\ & 1
\end{bmatrix}`.
Parameters
----------
state : unused
sensor_input : array-like, shape=[2]
Vector representing the information from the accelerometer.
Returns
-------
jacobian : array-like, shape=[dim, dim]
Jacobian of the propagation.
"""
dt, _ = sensor_input
dim = LocalizationLinear.dim
position_line = gs.hstack((gs.eye(dim // 2), dt * gs.eye(dim // 2)))
speed_line = gs.hstack((gs.zeros((dim // 2, dim // 2)), gs.eye(dim // 2)))
jac = gs.vstack((position_line, speed_line))
return jac
def inner_product_shape_test_data(self):
space = NFoldManifold(SpecialOrthogonal(3), 2)
n_samples = 4
point = gs.stack([gs.eye(3)] * space.n_copies * n_samples)
point = gs.reshape(point, (n_samples, *space.shape))
tangent_vec = space.to_tangent(gs.zeros((n_samples, *space.shape)),
point)
smoke_data = [
dict(space=space,
n_samples=4,
point=point,
tangent_vec=tangent_vec)
]
return self.generate_tests(smoke_data)
def inner_product_matrix(self, base_point=None):
"""Compute inner product matrix, independent of the base point.
Parameters
----------
base_point: array-like, shape=[n_samples, dimension]
Returns
-------
inner_prod_mat: array-like, shape=[n_samples, dimension, dimension]
"""
mat = gs.eye(self.dimension)
mat = gs.to_ndarray(mat, to_ndim=3)
return mat
def inner_product_matrix(self, base_point=None):
"""
Inner product matrix, independent of the base point.
"""
inner_prod_mat = gs.eye(self.dimension - 1, self.dimension - 1)
first_row = gs.array([0.] * (self.dimension - 1))
first_row = gs.to_ndarray(first_row, to_ndim=2, axis=1)
inner_prod_mat = gs.vstack([gs.transpose(first_row), inner_prod_mat])
first_column = gs.array([-1.] + [0.] * (self.dimension - 1))
first_column = gs.to_ndarray(first_column, to_ndim=2, axis=1)
inner_prod_mat = gs.hstack([first_column, inner_prod_mat])
return inner_prod_mat
def squared_dist_is_symmetric_test_data(self):
smoke_data = []
for angle_type_1, angle_type_2, left_or_right in zip(
elements, elements, ["left", "right"]):
smoke_data += [
dict(
metric_mat_at_identity=9 *
gs.eye(SpecialOrthogonal(3, "vector").dim),
left_or_right=left_or_right,
point_1=elements[angle_type_1],
point_2=elements[angle_type_2],
)
]
return self.generate_tests(smoke_data)
def matrix_from_rotation_vector(self, rot_vec):
"""Convert rotation vector to rotation matrix.
Parameters
----------
rot_vec: array-like, shape=[..., 3]
Returns
-------
rot_mat: array-like, shape=[..., 3]
"""
rot_vec = self.regularize(rot_vec)
angle = gs.linalg.norm(rot_vec, axis=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
skew_rot_vec = self.skew_matrix_from_vector(rot_vec)
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
# This avoids dividing by 0.
mask_0 = gs.isclose(angle, 0.)
mask_0_float = gs.cast(mask_0, gs.float32) + self.epsilon
coef_1 += mask_0_float * (1. - (angle ** 2) / 6.)
coef_2 += mask_0_float * (1. / 2. - angle ** 2)
# This avoids dividing by 0.
mask_else = ~mask_0
mask_else_float = gs.cast(mask_else, gs.float32) + self.epsilon
angle += mask_0_float
coef_1 += mask_else_float * (gs.sin(angle) / angle)
coef_2 += mask_else_float * (
(1. - gs.cos(angle)) / (angle ** 2))
coef_1 = gs.squeeze(coef_1, axis=1)
coef_2 = gs.squeeze(coef_2, axis=1)
term_1 = (gs.eye(self.dim)
+ gs.einsum('n,njk->njk', coef_1, skew_rot_vec))
squared_skew_rot_vec = gs.einsum(
'nij,njk->nik', skew_rot_vec, skew_rot_vec)
term_2 = gs.einsum('n,njk->njk', coef_2, squared_skew_rot_vec)
return term_1 + term_2
def test_belongs(self):
mat = gs.eye(3)
result = self.group.belongs(mat)
expected = True
self.assertAllClose(result, expected)
mat = gs.ones((3, 3))
result = self.group.belongs(mat)
expected = False
self.assertAllClose(result, expected)
mat = gs.ones(3)
result = self.group.belongs(mat)
expected = False
self.assertAllClose(result, expected)
def __init__(self,
dim,
shape,
default_point_type="vector",
lie_algebra=None,
**kwargs):
super(LieGroup, self).__init__(dim=dim,
shape=shape,
default_point_type=default_point_type,
**kwargs)
self.lie_algebra = lie_algebra
self.left_canonical_metric = InvariantMetric(
group=self,
metric_mat_at_identity=gs.eye(self.dim),
left_or_right="left")
self.right_canonical_metric = InvariantMetric(
group=self,
metric_mat_at_identity=gs.eye(self.dim),
left_or_right="right")
self.metric = self.left_canonical_metric
self.metrics = []
def metric_matrix(self, base_point=None):
"""Metric matrix at the tangent space at a base point.
Parameters
----------
base_point : array-like, shape=[..., dim + 1]
Base point.
Optional, default: None.
Returns
-------
mat : array-like, shape=[..., dim + 1, dim + 1]
Inner-product matrix.
"""
return gs.eye(self.dim + 1)
def __init__(self, group,
metric_mat_at_identity=None,
left_or_right='left', **kwargs):
super(_InvariantMetricMatrix, self).__init__(dim=group.dim, **kwargs)
self.group = group
self.lie_algebra = group.lie_algebra
if metric_mat_at_identity is None:
metric_mat_at_identity = gs.eye(self.group.dim)
geomstats.errors.check_parameter_accepted_values(
left_or_right, 'left_or_right', ['left', 'right'])
self.metric_mat_at_identity = metric_mat_at_identity
self.left_or_right = left_or_right
def exponential_matrix(self, rot_vec):
"""
Compute the exponential of the rotation matrix represented by rot_vec.
"""
rot_vec = self.rotations.regularize(rot_vec)
n_rot_vecs, _ = rot_vec.shape
angle = gs.linalg.norm(rot_vec, axis=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
skew_rot_vec = so_group.skew_matrix_from_vector(rot_vec)
coef_1 = gs.empty_like(angle)
coef_2 = gs.empty_like(coef_1)
mask_0 = gs.equal(angle, 0)
mask_0 = gs.squeeze(mask_0, axis=1)
mask_close_to_0 = gs.isclose(angle, 0)
mask_close_to_0 = gs.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. - gs.cos(angle[mask_else])))
coef_2[mask_else] = (angle[mask_else] ** (-2)
* (1. - (gs.sin(angle[mask_else])
/ angle[mask_else])))
term_1 = gs.zeros((n_rot_vecs, self.n, self.n))
term_2 = gs.zeros_like(term_1)
for i in range(n_rot_vecs):
term_1[i] = gs.eye(self.n) + skew_rot_vec[i] * coef_1[i]
term_2[i] = gs.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 inner_prod(tangent_vec_a, tangent_vec_b, base_point):
affine_part = self.bundle.ambient_metric.inner_product(
tangent_vec_a, tangent_vec_b, base_point)
n = tangent_vec_b.shape[-1]
inverse_base_point = GeneralLinear.inverse(base_point)
operator = gs.eye(n) + base_point * inverse_base_point
inverse_operator = GeneralLinear.inverse(operator)
diagonal_a = gs.einsum("...ij,...ji->...i", inverse_base_point,
tangent_vec_a)
diagonal_b = gs.einsum("...ij,...ji->...i", inverse_base_point,
tangent_vec_b)
aux = gs.einsum("...i,...j->...ij", diagonal_a, diagonal_b)
other_part = 2 * Matrices.frobenius_product(aux, inverse_operator)
return affine_part - other_part
def metric_matrix(self, base_point=None):
"""Compute the inner-product matrix, independent of the base point.
Parameters
----------
base_point : array-like, shape=[..., dim]
Base point.
Optional, default: None.
Returns
-------
inner_prod_mat : array-like, shape=[..., dim, dim]
Inner-product matrix.
"""
mat = gs.eye(self.dim)
return mat
def test_belongs(self):
"""Test of belongs method."""
mats = gs.array([[3., -1.], [-1., 3.]])
result = SPDMatrices(2).belongs(mats)
expected = True
self.assertAllClose(result, expected)
mats = gs.array([[-1., -1.], [-1., 3.]])
result = SPDMatrices(2).belongs(mats)
expected = False
self.assertAllClose(result, expected)
mats = gs.eye(3)
result = SPDMatrices(2).belongs(mats)
expected = False
self.assertAllClose(result, expected)
def get_identity(self, point_type='vector'):
"""Get the identity of the group.
Parameters
----------
point_type : str,
Point_type of the returned value. Unused here.
Returns
-------
identity : array-like, shape=[3,]
"""
identity = gs.zeros(self.dim)
if point_type == 'matrix':
identity = gs.eye(self.n)
return identity
def from_vector(vec, dtype=gs.float32):
"""Convert a vector into a symmetric matrix."""
vec_dim = vec.shape[-1]
mat_dim = (gs.sqrt(8. * vec_dim + 1) - 1) / 2
if mat_dim != int(mat_dim):
raise ValueError('Invalid input dimension, it must be of the form'
'(n_samples, n * (n + 1) / 2)')
mat_dim = int(mat_dim)
shape = (mat_dim, mat_dim)
mask = 2 * gs.ones(shape) - gs.eye(mat_dim)
indices = list(zip(*gs.triu_indices(mat_dim)))
vec = gs.cast(vec, dtype)
upper_triangular = gs.stack(
[gs.array_from_sparse(indices, data, shape) for data in vec])
mat = Matrices.to_symmetric(upper_triangular) * mask
return mat
def observation_jacobian(state, observation):
r"""Compute the matrix associated to the observation model.
The Jacobian is given by :math:`\begin{bmatrix} 1 & 0 \end{bmatrix}`.
Parameters
----------
state : unused
observation : unused
Returns
-------
jacobian : array-like, shape=[dim_obs, dim]
Jacobian of the observation.
"""
return gs.eye(LocalizationLinear.dim_obs, LocalizationLinear.dim)
def metric_matrix(self, base_point=None, n_jobs=1, **joblib_kwargs):
r"""Metric matrix at the tangent space at a base point.
Let :math:`f` be the immersion
:math:`f: M \rightarrow \mathbb{R}^n` of the manifold
:math:`M` into the Euclidean space :math:`\mathbb{R}^n`.
The elements of the metric matrix at a base point :math:`p`
are defined as:
:math:`(f*g)_{ij}(p) = <df_p e_i , df_p e_j>`,
for :math:`e_i, e_j` basis elements of :math:`M`.
Parameters
----------
base_point : array-like, shape=[..., dim]
Base point.
Optional, default: None.
Returns
-------
mat : array-like, shape=[..., dim, dim]
Inner-product matrix.
"""
immersed_base_point = self.immersion(base_point)
jacobian_immersion = self.jacobian_immersion(base_point)
basis_elements = gs.eye(self.dim)
@joblib.delayed
@joblib.wrap_non_picklable_objects
def pickable_inner_product(i, j):
immersed_basis_element_i = gs.matmul(jacobian_immersion,
basis_elements[i])
immersed_basis_element_j = gs.matmul(jacobian_immersion,
basis_elements[j])
return self.embedding_metric.inner_product(
immersed_basis_element_i,
immersed_basis_element_j,
base_point=immersed_base_point,
)
pool = joblib.Parallel(n_jobs=n_jobs, **joblib_kwargs)
out = pool(
pickable_inner_product(i, j)
for i, j in itertools.product(range(self.dim), range(self.dim)))
metric_mat = gs.reshape(gs.array(out), (-1, self.dim, self.dim))
return metric_mat[0] if base_point.ndim == 1 else metric_mat
def test_parallel_transport(self):
space = self.space
metric = self.shape_metric
shape = (self.n_samples, self.k_landmarks, self.m_ambient)
point = space.projection(gs.eye(4)[:, :3])
tan_b = gs.random.rand(*shape)
tan_b = space.to_tangent(tan_b, point)
tan_b = space.horizontal_projection(tan_b, point)
# use a vector orthonormal to tan_b
tan_a = gs.random.rand(*shape)
tan_a = space.to_tangent(tan_a, point)
tan_a = space.horizontal_projection(tan_a, point)
# orthonormalize and move to base_point
tan_a -= gs.einsum(
"...,...ij->...ij",
metric.inner_product(tan_a, tan_b, point) /
metric.squared_norm(tan_b, point),
tan_b,
)
tan_b = gs.einsum("...ij,...->...ij", tan_b,
1.0 / metric.norm(tan_b, point))
tan_a = gs.einsum("...ij,...->...ij", tan_a,
1.0 / metric.norm(tan_a, point))
transported = metric.parallel_transport(tan_a,
point,
tan_b,
n_steps=150,
step="rk4")
end_point = metric.exp(tan_b, point)
result = metric.norm(transported, end_point)
expected = metric.norm(tan_a, point)
self.assertAllClose(result, expected)
is_tangent = space.is_tangent(transported, end_point)
is_horizontal = space.is_horizontal(transported, end_point)
self.assertTrue(gs.all(is_tangent))
self.assertTrue(gs.all(is_horizontal))
transported = metric.parallel_transport(tan_a[0],
point,
end_point=end_point[0])
result = metric.norm(transported, end_point[0])
self.assertAllClose(result, expected[0])
def __init__(self, n):
super().__init__(
n=n + 1,
dim=int((n * (n + 1)) / 2),
embedding_space=GeneralLinear(n + 1, positive_det=True),
submersion=submersion,
value=gs.eye(n + 1),
tangent_submersion=tangent_submersion,
lie_algebra=SpecialEuclideanMatrixLieAlgebra(n=n),
)
self.rotations = SpecialOrthogonal(n=n)
self.translations = Euclidean(dim=n)
self.n = n
self.left_canonical_metric = SpecialEuclideanMatrixCannonicalLeftMetric(
group=self)
self.metric = self.left_canonical_metric
def jac(_, state):
"""Jacobian of bvp function.
Parameters
----------
state : array-like, shape=[2*dim, ...]
Vector of the state variables (position and speed)
_ : unused
Any (time).
Returns
-------
jac : array-like, shape=[dim, dim, ...]
"""
n_dim = state.ndim
n_times = state.shape[1] if n_dim > 1 else 1
position, velocity = state[: self.dim], state[self.dim :]
dgamma = self.jacobian_christoffels(gs.transpose(position))
df_dposition = -gs.einsum(
"j...,...ijkl,k...->il...", velocity, dgamma, velocity
)
gamma = self.christoffels(gs.transpose(position))
df_dvelocity = -2 * gs.einsum("...ijk,k...->ij...", gamma, velocity)
jac_nw = (
gs.zeros((self.dim, self.dim, state.shape[1]))
if n_dim > 1
else gs.zeros((self.dim, self.dim))
)
jac_ne = gs.squeeze(
gs.transpose(gs.tile(gs.eye(self.dim), (n_times, 1, 1)))
)
jac_sw = df_dposition
jac_se = df_dvelocity
jac = gs.concatenate(
(
gs.concatenate((jac_nw, jac_ne), axis=1),
gs.concatenate((jac_sw, jac_se), axis=1),
),
axis=0,
)
return jac
def matrix_from_rotation_vector(self, rot_vec):
"""
Convert rotation vector to rotation matrix.
:param rot_vec: rotation vector
:returns rot_mat: rotation matrix
"""
assert self.belongs(rot_vec)
rot_vec = self.regularize(rot_vec)
n_rot_vecs, _ = rot_vec.shape
if self.n == 3:
angle = gs.linalg.norm(rot_vec, axis=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
skew_rot_vec = skew_matrix_from_vector(rot_vec)
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
mask_0 = gs.isclose(angle, 0)
coef_1[mask_0] = 1 - (angle[mask_0]**2) / 6
coef_2[mask_0] = 1 / 2 - angle[mask_0]**2
coef_1[~mask_0] = gs.sin(angle[~mask_0]) / angle[~mask_0]
coef_2[~mask_0] = ((1 - gs.cos(angle[~mask_0])) /
(angle[~mask_0]**2))
term_1 = gs.zeros((n_rot_vecs, ) + (self.n, ) * 2)
term_2 = gs.zeros_like(term_1)
for i in range(n_rot_vecs):
term_1[i] = (gs.eye(self.dimension) +
coef_1[i] * skew_rot_vec[i])
term_2[i] = (coef_2[i] *
gs.matmul(skew_rot_vec[i], skew_rot_vec[i]))
rot_mat = term_1 + term_2
rot_mat = closest_rotation_matrix(rot_mat)
else:
skew_mat = skew_matrix_from_vector(rot_vec)
rot_mat = self.embedding_manifold.group_exp_from_identity(skew_mat)
return rot_mat
def log_at_antipodals_value_error_test_data(self):
smoke_data = [
dict(
n=3,
point=gs.eye(3),
base_point=gs.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
[0.0, 0.0, -1.0]]),
expected=pytest.raises(ValueError),
),
dict(
n=3,
mat1=SpecialOrthogonal(3).random_uniform(),
mat2=SpecialOrthogonal(3).random_uniform(),
expected=does_not_raise(),
),
]
return self.generate_tests(smoke_data)
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point):
r"""Compute the inner-product of two tangent vectors at a base point.
Canonical inner-product on the tangent space at `base_point`,
which is different from the inner-product induced by the embedding
(see [RLSMRZ2017]_).
.. math::
\langle\Delta, \tilde{\Delta}\rangle_{U}=\operatorname{tr}
\left(\Delta^{T}\left(I-\frac{1}{2} U U^{T}\right)
\tilde{\Delta}\right)
References
----------
.. [RLSMRZ2017] R Zimmermann. A matrix-algebraic algorithm for the
Riemannian logarithm on the Stiefel manifold under the canonical
metric. SIAM Journal on Matrix Analysis and Applications 38 (2),
322-342, 2017. https://epubs.siam.org/doi/pdf/10.1137/16M1074485
Parameters
----------
tangent_vec_a : array-like, shape=[n_samples, n, p]
First tangent vector at base point.
tangent_vec_b : array-like, shape=[n_samples, n, p]
Second tangent vector at base point.
base_point : array-like, shape=[n_samples, n, p]
Point in the Stiefel manifold.
Returns
-------
inner_prod : array-like, shape=[n_samples, 1]
Inner-product of the two tangent vectors.
"""
tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=3)
tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=3)
base_point = gs.to_ndarray(base_point, to_ndim=3)
base_point_transpose = gs.transpose(base_point, axes=(0, 2, 1))
aux = gs.matmul(
gs.transpose(tangent_vec_a, axes=(0, 2, 1)),
gs.eye(self.n) - 0.5 * gs.matmul(base_point, base_point_transpose))
inner_prod = gs.trace(gs.matmul(aux, tangent_vec_b), axis1=1, axis2=2)
inner_prod = gs.to_ndarray(inner_prod, to_ndim=2, axis=1)
return inner_prod
def jacobian_translation(self, point, left_or_right='left'):
"""
Compute the jacobian matrix of the differential
of the left/right translations
from the identity to point in the Lie group SE(3).
:param point: 6D vector element of SE(3)
:returns jacobian: 6x6 matrix
"""
assert self.belongs(point)
assert left_or_right in ('left', 'right')
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
jacobian = gs.zeros((n_points,) + (dim,) * 2)
if left_or_right == 'left':
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='left')
jacobian_trans = self.rotations.matrix_from_rotation_vector(
rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, dim_rotations:] = jacobian_trans
else:
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='right')
inv_skew_mat = - so_group.skew_matrix_from_vector(rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, :dim_rotations] = inv_skew_mat
jacobian[:, dim_rotations:, dim_rotations:] = gs.eye(self.n)
assert jacobian.ndim == 3
return jacobian
def from_vector_to_diagonal_matrix(vector):
"""Create diagonal matrices from rows of a matrix.
Parameters
----------
vector : array-like, shape=[m, n]
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)
return diagonals
def orthonormal_basis(self, metric_matrix):
"""Orthonormalize the basis with respect to the given metric.
This corresponds to a renormalization.
Parameters
----------
metric_matrix : array-like, shape=[dim, dim]
Matrix of a metric.
Returns
-------
basis : array-like, shape=[dim, n, n]
Orthonormal basis.
"""
metric_matrix = self.reshape_metric_matrix(metric_matrix) + gs.eye(
self.n)
return self.basis / gs.sqrt(2 * metric_matrix)
def __init__(
self, group, metric_mat_at_identity=None, left_or_right="left", **kwargs
):
super(_InvariantMetricMatrix, self).__init__(
dim=group.dim, default_point_type="matrix", **kwargs
)
self.group = group
self.lie_algebra = group.lie_algebra
if metric_mat_at_identity is None:
metric_mat_at_identity = gs.eye(self.group.dim)
geomstats.errors.check_parameter_accepted_values(
left_or_right, "left_or_right", ["left", "right"]
)
self.metric_mat_at_identity = metric_mat_at_identity
self.left_or_right = left_or_right
def inner_product_matrix_shape_test_data(self):
group = SpecialEuclidean(n=3, point_type="vector")
sym_mat_at_identity = gs.eye(group.dim)
smoke_data = [
dict(
group=group,
metric_mat_at_identity=sym_mat_at_identity,
left_or_right="left",
base_point=None,
),
dict(
group=group,
metric_mat_at_identity=sym_mat_at_identity,
left_or_right="left",
base_point=group.identity,
),
]
return self.generate_tests(smoke_data)
