def compose(self, point_1, point_2, point_type=None):
"""
Compose two elements of SE(n).
Formula:
point_1 . point_2 = [R1 * R2, (R1 * t2) + t1]
where:
R1, R2 are rotation matrices,
t1, t2 are translation vectors.
"""
if point_type is None:
point_type = self.default_point_type
rotations = self.rotations
dim_rotations = rotations.dimension
point_1 = self.regularize(point_1, point_type=point_type)
point_2 = self.regularize(point_2, point_type=point_type)
if point_type == 'vector':
n_points_1, _ = point_1.shape
n_points_2, _ = point_2.shape
assert (point_1.shape == point_2.shape
or n_points_1 == 1
or n_points_2 == 1)
rot_vec_1 = point_1[:, :dim_rotations]
rot_mat_1 = rotations.matrix_from_rotation_vector(rot_vec_1)
rot_mat_1 = rotations.projection(rot_mat_1)
rot_vec_2 = point_2[:, :dim_rotations]
rot_mat_2 = rotations.matrix_from_rotation_vector(rot_vec_2)
rot_mat_2 = rotations.projection(rot_mat_2)
translation_1 = point_1[:, dim_rotations:]
translation_2 = point_2[:, dim_rotations:]
n_compositions = gs.maximum(n_points_1, n_points_2)
composition_rot_mat = gs.matmul(rot_mat_1, rot_mat_2)
composition_rot_vec = rotations.rotation_vector_from_matrix(
composition_rot_mat)
composition_translation = gs.zeros((n_compositions, self.n))
for i in range(n_compositions):
translation_1_i = (translation_1[0] if n_points_1 == 1
else translation_1[i])
rot_mat_1_i = (rot_mat_1[0] if n_points_1 == 1
else rot_mat_1[i])
translation_2_i = (translation_2[0] if n_points_2 == 1
else translation_2[i])
composition_translation[i] = (gs.dot(translation_2_i,
gs.transpose(rot_mat_1_i))
+ translation_1_i)
composition = gs.zeros((n_compositions, self.dimension))
composition[:, :dim_rotations] = composition_rot_vec
composition[:, dim_rotations:] = composition_translation
elif point_type == 'matrix':
raise NotImplementedError()
composition = self.regularize(composition, point_type=point_type)
return composition
def lifting(point, base_point):
"""Compute the lifting of a point.
This computation is based on the QR-decomposion.
e.g. :math:`P_x^{-1}(Q) = QR - X`.
Parameters
----------
point : array-like, shape=[..., n, p]
Point in the Stiefel manifold.
base_point : array-like, shape=[..., n, p]
Point in the Stiefel manifold.
Returns
-------
log : array-like, shape=[..., dim + 1]
Tangent vector at the base point equal to the lifting
of point at the base point.
"""
n_points, _, _ = point.shape
n_base_points, _, n = base_point.shape
if not (n_points == n_base_points
or n_points == 1
or n_base_points == 1):
raise NotImplementedError
n_liftings = gs.maximum(n_base_points, n_points)
def _make_minor(i, matrix):
return matrix[:i + 1, :i + 1]
def _make_column_r(i, matrix):
if i == 0:
if matrix[0, 0] <= 0:
raise ValueError('M[0,0] <= 0')
return gs.array([1. / matrix[0, 0]])
matrix_m_i = _make_minor(i, matrix_m_k)
inv_matrix_m_i = gs.linalg.inv(matrix_m_i)
b_i = _make_b(i, matrix_m_k, columns_list)
column_r_i = gs.matmul(inv_matrix_m_i, b_i)
if column_r_i[i] <= 0:
raise ValueError('(r_i)_i <= 0')
return column_r_i
def _make_b(i, matrix, list_matrices_r):
b = gs.ones(i + 1)
for j in range(i):
b[j] = - gs.matmul(
matrix[i, :j + 1], list_matrices_r[j])
return b
matrix_r = gs.zeros((n_liftings, n, n))
matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point)
for k in range(n_liftings):
columns_list = []
matrix_m_k = matrix_m[k]
for i in range(n):
column_r_i = _make_column_r(i, matrix_m_k)
columns_list.append(column_r_i)
matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i)
return gs.matmul(point, matrix_r) - base_point
def rotation_vector_from_matrix(self, rot_mat):
r"""Convert rotation matrix (in 3D) to rotation vector (axis-angle).
Get the angle through the trace of the rotation matrix:
The eigenvalues are:
:math:`\{1, \cos(angle) + i \sin(angle), \cos(angle) - i \sin(angle)\}`
so that:
:math:`trace = 1 + 2 \cos(angle), \{-1 \leq trace \leq 3\}`
Get the rotation vector through the formula:
:math:`S_r = \frac{angle}{(2 * \sin(angle) ) (R - R^T)}`
For the edge case where the angle is close to pi,
the formulation is derived by using the following equality (see the
Axis-angle representation on Wikipedia):
:math:`outer(r, r) = \frac{1}{2} (R + I_3)`
In nD, the rotation vector stores the :math:`n(n-1)/2` values
of the skew-symmetric matrix representing the rotation.
Parameters
----------
rot_mat : array-like, shape=[..., n, n]
Returns
-------
regularized_rot_vec : array-like, shape=[..., 3]
"""
n_rot_mats, _, _ = rot_mat.shape
trace = gs.trace(rot_mat, axis1=1, axis2=2)
trace = gs.to_ndarray(trace, to_ndim=2, axis=1)
trace_num = gs.clip(trace, -1, 3)
angle = gs.arccos(0.5 * (trace_num - 1))
rot_mat_transpose = gs.transpose(rot_mat, axes=(0, 2, 1))
rot_vec_not_pi = self.vector_from_skew_matrix(rot_mat -
rot_mat_transpose)
mask_0 = gs.cast(gs.isclose(angle, 0.), gs.float32)
mask_pi = gs.cast(gs.isclose(angle, gs.pi, atol=1e-2), gs.float32)
mask_else = (1 - mask_0) * (1 - mask_pi)
numerator = 0.5 * mask_0 + angle * mask_else
denominator = (1 - angle**2 /
6) * mask_0 + 2 * gs.sin(angle) * mask_else + mask_pi
rot_vec_not_pi = rot_vec_not_pi * numerator / denominator
vector_outer = 0.5 * (gs.eye(3) + rot_mat)
gs.set_diag(
vector_outer,
gs.maximum(0., gs.diagonal(vector_outer, axis1=1, axis2=2)))
squared_diag_comp = gs.diagonal(vector_outer, axis1=1, axis2=2)
diag_comp = gs.sqrt(squared_diag_comp)
norm_line = gs.linalg.norm(vector_outer, axis=2)
max_line_index = gs.argmax(norm_line, axis=1)
selected_line = gs.get_slice(vector_outer,
(range(n_rot_mats), max_line_index))
signs = gs.sign(selected_line)
rot_vec_pi = angle * signs * diag_comp
rot_vec = rot_vec_not_pi + mask_pi * rot_vec_pi
return self.regularize(rot_vec)
def exp(self, tangent_vec, base_point):
"""Compute the Riemannian exponential of a tangent vector.
Parameters
----------
tangent_vec : array-like, shape=[..., n, p]
Tangent vector at a base point.
base_point : array-like, shape=[..., n, p]
Point in the Stiefel manifold.
Returns
-------
exp : array-like, shape=[..., n, p]
Point in the Stiefel manifold equal to the Riemannian exponential
of tangent_vec at the base point.
"""
n_tangent_vecs, _, _ = tangent_vec.shape
n_base_points, _, p = base_point.shape
if not (n_tangent_vecs == n_base_points
or n_tangent_vecs == 1
or n_base_points == 1):
raise NotImplementedError
if n_tangent_vecs == 1:
tangent_vec = gs.tile(tangent_vec, (n_base_points, 1, 1))
if n_base_points == 1:
base_point = gs.tile(base_point, (n_tangent_vecs, 1, 1))
matrix_a = gs.einsum(
'nij, njk->nik',
gs.transpose(base_point, axes=(0, 2, 1)), tangent_vec)
matrix_k = (tangent_vec
- gs.einsum('nij,njk->nik', base_point, matrix_a))
matrix_q, matrix_r = gs.linalg.qr(matrix_k)
matrix_ar = gs.concatenate(
[matrix_a,
-gs.transpose(matrix_r, axes=(0, 2, 1))],
axis=2)
zeros = gs.zeros(
(gs.maximum(n_base_points, n_tangent_vecs), p, p))
matrix_rz = gs.concatenate(
[matrix_r,
zeros],
axis=2)
block = gs.concatenate([matrix_ar, matrix_rz], axis=1)
matrix_mn_e = gs.linalg.expm(block)
exp = gs.einsum(
'nij,njk->nik',
gs.concatenate(
[base_point,
matrix_q],
axis=2),
matrix_mn_e[:, :, 0:p])
return exp
def lifting(self, point, base_point):
"""
Lifting map, based on QR-decomposion:
P_x^{-1}(Q) = QR - X
"""
point = gs.to_ndarray(point, to_ndim=3)
n_points, _, _ = point.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, p, n = base_point.shape
assert (n_points == n_base_points or n_points == 1
or n_base_points == 1)
if n_base_points == 1:
base_point = gs.tile(base_point, (n_points, 1, 1))
if n_points == 1:
point = gs.tile(point, (n_base_points, 1, 1))
n_liftings = gs.maximum(n_base_points, n_points)
def make_minor(i, matrix):
return matrix[:i + 1, :i + 1]
def make_column_r(i, matrix):
if i == 0:
assert matrix[0, 0] > 0, 'M[0,0] <= 0'
return gs.array([1. / matrix[0, 0]])
else:
# get principal minor
matrix_m_i = make_minor(i, matrix_m_k)
assert gs.linalg.det(matrix_m_i) != 0
inv_matrix_m_i = gs.linalg.inv(matrix_m_i)
b_i = make_b(i, matrix_m_k, columns_list)
column_r_i = gs.matmul(inv_matrix_m_i, b_i)
assert column_r_i[i] > 0, '(r_i)_i <= 0'
return column_r_i
def make_b(i, matrix, list_matrices_r):
b = gs.ones(i + 1)
for j in range(i):
b[j] = -gs.matmul(matrix[i, :j + 1], list_matrices_r[j])
return b
matrix_r = gs.zeros((n_liftings, n, n))
matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point)
for k in range(n_liftings):
columns_list = []
matrix_m_k = matrix_m[k]
for i in range(n):
column_r_i = make_column_r(i, matrix_m_k)
columns_list.append(column_r_i)
matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i)
return gs.matmul(point, matrix_r) - base_point
def lifting(self, point, base_point):
"""Compute the lifting of a point.
This computation is based on the QR-decomposion.
e.g. :math:`P_x^{-1}(Q) = QR - X`.
Parameters
----------
point : array-like, shape=[n_samples, n, p]
Point in the Stiefel manifold.
base_point : array-like, shape=[n_samples, n, p]
Point in the Stiefel manifold.
Returns
-------
log : array-like, shape=[n_samples, dimension + 1]
Tangent vector at the base point equal to the lifting
of point at the base point.
"""
point = gs.to_ndarray(point, to_ndim=3)
n_points, _, _ = point.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, p, n = base_point.shape
assert (n_points == n_base_points or n_points == 1
or n_base_points == 1)
if n_base_points == 1:
base_point = gs.tile(base_point, (n_points, 1, 1))
if n_points == 1:
point = gs.tile(point, (n_base_points, 1, 1))
n_liftings = gs.maximum(n_base_points, n_points)
def _make_minor(i, matrix):
return matrix[:i + 1, :i + 1]
def _make_column_r(i, matrix):
if i == 0:
assert matrix[0, 0] > 0, 'M[0,0] <= 0'
return gs.array([1. / matrix[0, 0]])
else:
matrix_m_i = _make_minor(i, matrix_m_k)
inv_matrix_m_i = gs.linalg.inv(matrix_m_i)
b_i = _make_b(i, matrix_m_k, columns_list)
column_r_i = gs.matmul(inv_matrix_m_i, b_i)
assert column_r_i[i] > 0, '(r_i)_i <= 0'
return column_r_i
def _make_b(i, matrix, list_matrices_r):
b = gs.ones(i + 1)
for j in range(i):
b[j] = -gs.matmul(matrix[i, :j + 1], list_matrices_r[j])
return b
matrix_r = gs.zeros((n_liftings, n, n))
matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point)
for k in range(n_liftings):
columns_list = []
matrix_m_k = matrix_m[k]
for i in range(n):
column_r_i = _make_column_r(i, matrix_m_k)
columns_list.append(column_r_i)
matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i)
return gs.matmul(point, matrix_r) - base_point
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 power-Euclidean metric.
Parameters
----------
tangent_vec_a : array-like, shape=[n_samples, n, n]
tangent_vec_b : array-like, shape=[n_samples, n, n]
base_point : array-like, shape={n_samples, n, n]
Returns
-------
inner_product : float
"""
power_euclidean = self.power_euclidean
tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=3)
n_tangent_vecs_a, _, _ = tangent_vec_a.shape
tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=3)
n_tangent_vecs_b, _, _ = tangent_vec_b.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, _, _ = base_point.shape
spd_space = self.space
assert (n_tangent_vecs_a == n_tangent_vecs_b == n_base_points
or n_tangent_vecs_a == n_tangent_vecs_b and n_base_points == 1
or n_base_points == n_tangent_vecs_a and n_tangent_vecs_b == 1
or n_base_points == n_tangent_vecs_b and n_tangent_vecs_a == 1
or n_tangent_vecs_a == 1 and n_tangent_vecs_b == 1
or n_base_points == 1 and n_tangent_vecs_a == 1
or n_base_points == 1 and n_tangent_vecs_b == 1)
if n_tangent_vecs_a == 1:
tangent_vec_a = gs.tile(
tangent_vec_a,
(gs.maximum(n_base_points, n_tangent_vecs_b), 1, 1))
if n_tangent_vecs_b == 1:
tangent_vec_b = gs.tile(
tangent_vec_b,
(gs.maximum(n_base_points, n_tangent_vecs_a), 1, 1))
if n_base_points == 1:
base_point = gs.tile(
base_point,
(gs.maximum(n_tangent_vecs_a, n_tangent_vecs_b), 1, 1))
if power_euclidean == 1:
product = gs.matmul(tangent_vec_a, tangent_vec_b)
inner_product = gs.trace(product, axis1=1, axis2=2)
else:
modified_tangent_vec_a = \
spd_space.differential_power(power_euclidean, tangent_vec_a,
base_point)
modified_tangent_vec_b = \
spd_space.differential_power(power_euclidean, tangent_vec_b,
base_point)
product = gs.matmul(modified_tangent_vec_a, modified_tangent_vec_b)
inner_product = gs.trace(product, axis1=1, axis2=2) \
/ (power_euclidean ** 2)
inner_product = gs.to_ndarray(inner_product, to_ndim=2, axis=1)
return inner_product
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point):
"""Compute the affine-invariant inner product.
Compute the inner product of tangent_vec_a and tangent_vec_b
at point base_point using the affine invariant Riemannian metric.
Parameters
----------
tangent_vec_a : array-like, shape=[n_samples, n, n]
tangent_vec_b : array-like, shape=[n_samples, n, n]
base_point : array-like, shape=[n_samples, n, n]
Returns
-------
inner_product : array-like, shape=[n_samples, n, n]
"""
power_affine = self.power_affine
tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=3)
n_tangent_vecs_a, _, _ = tangent_vec_a.shape
tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=3)
n_tangent_vecs_b, _, _ = tangent_vec_b.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, _, _ = base_point.shape
spd_space = self.space
assert (n_tangent_vecs_a == n_tangent_vecs_b == n_base_points
or n_tangent_vecs_a == n_tangent_vecs_b and n_base_points == 1
or n_base_points == n_tangent_vecs_a and n_tangent_vecs_b == 1
or n_base_points == n_tangent_vecs_b and n_tangent_vecs_a == 1
or n_tangent_vecs_a == 1 and n_tangent_vecs_b == 1
or n_base_points == 1 and n_tangent_vecs_a == 1
or n_base_points == 1 and n_tangent_vecs_b == 1)
if n_tangent_vecs_a == 1:
tangent_vec_a = gs.tile(
tangent_vec_a,
(gs.maximum(n_base_points, n_tangent_vecs_b), 1, 1))
if n_tangent_vecs_b == 1:
tangent_vec_b = gs.tile(
tangent_vec_b,
(gs.maximum(n_base_points, n_tangent_vecs_a), 1, 1))
if n_base_points == 1:
base_point = gs.tile(
base_point,
(gs.maximum(n_tangent_vecs_a, n_tangent_vecs_b), 1, 1))
if power_affine == 1:
inv_base_point = gs.linalg.inv(base_point)
inner_product = self._aux_inner_product(tangent_vec_a,
tangent_vec_b,
inv_base_point)
else:
modified_tangent_vec_a =\
spd_space.differential_power(power_affine, tangent_vec_a,
base_point)
modified_tangent_vec_b =\
spd_space.differential_power(power_affine, tangent_vec_b,
base_point)
power_inv_base_point = gs.linalg.powerm(base_point, -power_affine)
inner_product = self._aux_inner_product(modified_tangent_vec_a,
modified_tangent_vec_b,
power_inv_base_point)
inner_product = inner_product / (power_affine**2)
inner_product = gs.to_ndarray(inner_product, to_ndim=2, axis=1)
return inner_product
