def random_von_mises_fisher(self, kappa=10, n_samples=1):
"""Sample in the 2-sphere with the von Mises distribution.
Sample in the 2-sphere with the von Mises distribution centered in the
north pole.
Parameters
----------
kappa : int, optional
n_samples : int, optional
Returns
-------
point : array-like
"""
if self.dimension != 2:
raise NotImplementedError(
'Sampling from the von Mises Fisher distribution'
'is only implemented in dimension 2.')
angle = 2. * gs.pi * gs.random.rand(n_samples)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
unit_vector = gs.hstack((gs.cos(angle), gs.sin(angle)))
scalar = gs.random.rand(n_samples)
coord_z = 1. + 1. / kappa * gs.log(scalar + (1. - scalar) *
gs.exp(gs.array(-2. * kappa)))
coord_z = gs.to_ndarray(coord_z, to_ndim=2, axis=1)
coord_xy = gs.sqrt(1. - coord_z**2) * unit_vector
point = gs.hstack((coord_xy, coord_z))
return point
def grad(y_pred,
y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""
Closed-form for the gradient of pose_loss.
:return: 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':
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:]])
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)
grad = gs.einsum('ni,nij->ni', grad, differential)
grad = gs.squeeze(grad, axis=0)
return grad
def grad(y_pred,
y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""
Closed-form for the gradient of pose_loss.
:return: tangent vector at point y_pred.
"""
if y_pred.ndim == 1:
y_pred = gs.expand_dims(y_pred, axis=0)
if y_true.ndim == 1:
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'vector':
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:]])
grad = lie_group.grad(y_pred_pose, y_true_pose, SE3, metric)
differential = gs.zeros((1, 6, 7))
upper_left_block = gs.zeros((1, 3, 4))
lower_right_block = gs.zeros((1, 3, 3))
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
# TODO(nina): check that this sq norm is 1?
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.outer(quat_vec, quat_vec) / quat_vec_norm**2 +
2 * quat_arctan2 / quat_vec_norm * gs.eye(3))
upper_left_block[0, :, :1] = differential_scalar.transpose()
upper_left_block[0, :, 1:] = differential_vec
lower_right_block[0, :, :] = gs.eye(3)
differential[0, :3, :4] = upper_left_block
differential[0, 3:, 4:] = lower_right_block
grad = gs.matmul(grad, differential)
grad = gs.squeeze(grad, axis=0)
return grad
def _exp(tangent_vec, base_point):
circle_center = (base_point[0] +
base_point[1] * tangent_vec[1] / tangent_vec[0])
circle_radius = gs.sqrt((circle_center - base_point[0])**2 +
base_point[1]**2)
moebius_d = 1
moebius_c = 1 / (2 * circle_radius)
moebius_b = circle_center - circle_radius
moebius_a = (circle_center + circle_radius) * moebius_c
point_complex = base_point[0] + 1j * base_point[1]
tangent_vec_complex = tangent_vec[0] + 1j * tangent_vec[1]
point_moebius = (1j * (moebius_d * point_complex - moebius_b) /
(moebius_c * point_complex - moebius_a))
tangent_vec_moebius = (
-1j * tangent_vec_complex *
(1j * moebius_c * point_moebius + moebius_d)**2)
end_point_moebius = point_moebius * gs.exp(
tangent_vec_moebius / point_moebius)
end_point_complex = (
moebius_a * 1j * end_point_moebius +
moebius_b) / (moebius_c * 1j * end_point_moebius + moebius_d)
end_point_expected = gs.hstack(
[np.real(end_point_complex),
np.imag(end_point_complex)])
return end_point_expected
def regularize(self, point, point_type=None):
"""Regularize the point into the manifold's canonical representation.
Parameters
----------
point : array-like, shape=[n_samples, dim]
or shape=[n_samples, dim_2, dim_2]
Point to be regularized.
point_type : str, {'vector', 'matrix'}
Representation of point.
Returns
-------
regularized_point : array-like, shape=[n_samples, dim]
or shape=[n_samples, dim_2, dim_2]
Point in the manifold's canonical representation.
"""
if point_type is None:
point_type = self.default_point_type
assert point_type in ['vector', 'matrix']
if point_type == 'vector':
point = gs.to_ndarray(point, to_ndim=2)
intrinsic = self.metric.is_intrinsic(point)
regularized_point = self._iterate_over_manifolds(
'regularize', {'point': point}, intrinsic)
regularized_point = gs.hstack(regularized_point)
elif point_type == 'matrix':
regularized_point = [
manifold_i.regularize(point[:, i])
for i, manifold_i in enumerate(self.manifolds)]
regularized_point = gs.stack(regularized_point, axis=1)
return regularized_point
def adjoint_map(state):
r"""Construct the matrix associated to the adjoint representation.
The inner automorphism is given by :math:`Ad_X : g |-> XgX^-1`. For a
state :math:`X = (\theta, x, y)`, the matrix associated to its tangent
map, the adjoint representation, is
:math:`\begin{bmatrix} 1 & \\ -J [x, y] & R(\theta) \end{bmatrix}`,
where :math:`R(\theta)` is the rotation matrix of angle theta, and
:math:`J = \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix}`
Parameters
----------
state : array-like, shape=[dim]
Vector representing a state.
Returns
-------
adjoint : array-like, shape=[dim, dim]
Adjoint representation of the state.
"""
theta, _, _ = state
tangent_base = gs.array([[0.0, -1.0], [1.0, 0.0]])
orientation_part = gs.eye(Localization.dim_rot, Localization.dim)
pos_column = gs.reshape(state[1:], (Localization.group.n, 1))
position_wrt_orientation = Matrices.mul(-tangent_base, pos_column)
position_wrt_position = Localization.rotation_matrix(theta)
last_lines = gs.hstack(
(position_wrt_orientation, position_wrt_position))
ad = gs.vstack((orientation_part, last_lines))
return ad
def test_exp_vectorization(self):
point = gs.array([[1.0, 1.0], [1.0, 1.0]])
tangent_vec = gs.array([[2.0, 1.0], [2.0, 1.0]])
result = self.metric.exp(tangent_vec, point)
point = point[0]
tangent_vec = tangent_vec[0]
circle_center = point[0] + point[1] * tangent_vec[1] / tangent_vec[0]
circle_radius = gs.sqrt((circle_center - point[0])**2 + point[1]**2)
moebius_d = 1
moebius_c = 1 / (2 * circle_radius)
moebius_b = circle_center - circle_radius
moebius_a = (circle_center + circle_radius) * moebius_c
point_complex = point[0] + 1j * point[1]
tangent_vec_complex = tangent_vec[0] + 1j * tangent_vec[1]
point_moebius = (1j * (moebius_d * point_complex - moebius_b) /
(moebius_c * point_complex - moebius_a))
tangent_vec_moebius = (-1j * tangent_vec_complex *
(1j * moebius_c * point_moebius + moebius_d)**2)
end_point_moebius = point_moebius * gs.exp(
tangent_vec_moebius / point_moebius)
end_point_complex = (moebius_a * 1j * end_point_moebius +
moebius_b) / (moebius_c * 1j * end_point_moebius +
moebius_d)
end_point_expected = gs.hstack(
[np.real(end_point_complex),
np.imag(end_point_complex)])
expected = gs.stack([end_point_expected, end_point_expected])
self.assertAllClose(result, expected)
def test_exp(self):
point = gs.array([1., 1.])
tangent_vec = gs.array([2., 1.])
end_point = self.metric.exp(tangent_vec, point)
circle_center = point[0] + point[1] * tangent_vec[1] / tangent_vec[0]
circle_radius = gs.sqrt((circle_center - point[0])**2 + point[1]**2)
moebius_d = 1
moebius_c = 1 / (2 * circle_radius)
moebius_b = circle_center - circle_radius
moebius_a = (circle_center + circle_radius) * moebius_c
point_complex = point[0] + 1j * point[1]
tangent_vec_complex = tangent_vec[0] + 1j * tangent_vec[1]
point_moebius = 1j * (moebius_d * point_complex - moebius_b)\
/ (moebius_c * point_complex - moebius_a)
tangent_vec_moebius = -1j * tangent_vec_complex * (
1j * moebius_c * point_moebius + moebius_d)**2
end_point_moebius = point_moebius * gs.exp(
tangent_vec_moebius / point_moebius)
end_point_complex = (moebius_a * 1j * end_point_moebius + moebius_b)\
/ (moebius_c * 1j * end_point_moebius + moebius_d)
end_point_expected = gs.hstack(
[np.real(end_point_complex),
np.imag(end_point_complex)])
self.assertAllClose(end_point, end_point_expected)
def ivp(state, _):
"""Reformat the initial value problem geodesic ODE."""
position, velocity = state[: self.dim], state[self.dim :]
state = gs.stack([position, velocity])
vel, acc = self.geodesic_equation(state, _)
eq = (vel, acc)
return gs.hstack(eq)
def _half_plane_to_extrinsic_coordinates(point):
"""Convert half plane to extrinsic coordinates.
Convert the parameterization of a point in the hyperbolic plane
from its upper half plane model coordinates, to the extrinsic
coordinates.
Parameters
----------
point : array-like, shape=[n_samples, 2]
Point in hyperbolic space in half-plane coordinates.
Returns
-------
extrinsic : array-like, shape=[n_samples, 3]
Point in hyperbolic plane in extrinsic coordinates.
"""
assert point.shape[-1] == 2
x, y = point[:, 0], point[:, 1]
x2 = point[:, 0]**2
den = x2 + (1 + y)**2
x = gs.to_ndarray(x, to_ndim=2, axis=0)
y = gs.to_ndarray(y, to_ndim=2, axis=0)
x2 = gs.to_ndarray(x2, to_ndim=2, axis=0)
den = gs.to_ndarray(den, to_ndim=2, axis=0)
ball_point = gs.hstack((2 * x / den, (x2 + y**2 - 1) / den))
return Hyperbolic._ball_to_extrinsic_coordinates(ball_point)
def coefficients(ind_k):
param_k = base_point[..., ind_k]
param_sum = gs.sum(base_point, -1)
c1 = 1 / gs.polygamma(
1, param_k) / (1 / gs.polygamma(1, param_sum) -
gs.sum(1 / gs.polygamma(1, base_point), -1))
c2 = -c1 * gs.polygamma(2, param_sum) / gs.polygamma(1, param_sum)
mat_ones = gs.ones((n_points, self.dim, self.dim))
mat_diag = from_vector_to_diagonal_matrix(
-gs.polygamma(2, base_point) / gs.polygamma(1, base_point))
arrays = [
gs.zeros((1, ind_k)),
gs.ones((1, 1)),
gs.zeros((1, self.dim - ind_k - 1))
]
vec_k = gs.tile(gs.hstack(arrays), (n_points, 1))
val_k = gs.polygamma(2, param_k) / gs.polygamma(1, param_k)
vec_k = gs.einsum('i,ij->ij', val_k, vec_k)
mat_k = from_vector_to_diagonal_matrix(vec_k)
mat = gs.einsum('i,ijk->ijk', c2, mat_ones)\
- gs.einsum('i,ijk->ijk', c1, mat_diag) + mat_k
return 1 / 2 * mat
def _extrinsic_to_half_plane_coordinates(point):
"""Convert extrinsic to half plane coordinates.
Convert the parameterization of a point in the hyperbolic plane
from its intrinsic coordinates, to the poincare upper half plane
coordinates.
Parameters
----------
point : array-like, shape=[n_samples, 2]
Point in the hyperbolic plane in intrinsic coordinates.
Returns
-------
point_half_plane : array-like, shape=[n_samples, 2]
Point in the hyperbolic plane in Poincare upper half-plane
coordinates.
"""
point_ball = \
Hyperbolic._extrinsic_to_ball_coordinates(point)
assert point_ball.shape[-1] == 2
point_ball_x, point_ball_y = point_ball[:, 0], point_ball[:, 1]
point_ball_x2 = point_ball_x**2
denom = point_ball_x2 + (1 - point_ball_y)**2
point_ball_x = gs.to_ndarray(point_ball_x, to_ndim=2, axis=0)
point_ball_y = gs.to_ndarray(point_ball_y, to_ndim=2, axis=0)
point_ball_x2 = gs.to_ndarray(point_ball_x2, to_ndim=2, axis=0)
denom = gs.to_ndarray(denom, to_ndim=2, axis=0)
point_half_plane = gs.hstack(
((2 * point_ball_x) / denom,
(1 - point_ball_x2 - point_ball_y**2) / denom))
return point_half_plane
def regularize(self, point, point_type=None):
"""Regularize the point into the manifold's canonical representation.
Parameters
----------
point : array-like, shape=[n_samples, dim]
or shape=[n_samples, dim_2, dim_2]
Point to be regularized.
point_type : str, {'vector', 'matrix'}
Representation of point.
Returns
-------
regularized_point : array-like, shape=[n_samples, dim]
or shape=[n_samples, dim_2, dim_2]
Point in the manifold's canonical representation.
"""
# TODO(nina): Vectorize.
if point_type is None:
point_type = self.default_point_type
assert point_type in ['vector', 'matrix']
regularized_point = [
manifold_i.regularize(point_i)
for manifold_i, point_i in zip(self.manifolds, point)]
# TODO(nina): Put this in a decorator
if point_type == 'vector':
regularized_point = gs.hstack(regularized_point)
elif point_type == 'matrix':
regularized_point = gs.vstack(regularized_point)
return gs.all(regularized_point)
def regularize(self, point, point_type=None):
"""Regularize the point into the manifold's canonical representation.
Parameters
----------
point
point_type : str, {'vector', 'matrix'}
Returns
-------
regularize_points
"""
# TODO(nina): Vectorize.
if point_type is None:
point_type = self.default_point_type
assert point_type in ['vector', 'matrix']
regularize_points = [self.manifold[i].regularize(point[i])
for i in range(self.n_manifolds)]
# TODO(nina): Put this in a decorator
if point_type == 'vector':
regularize_points = gs.hstack(regularize_points)
elif point_type == 'matrix':
regularize_points = gs.vstack(regularize_points)
return gs.all(regularize_points)
return regularize_points
def regularize(self, point, point_type=None):
"""Regularize the point into the manifold's canonical representation.
Parameters
----------
point : array-like, shape=[..., {dim, [dim_2, dim_2]}]
Point to be regularized.
point_type : str, {'vector', 'matrix'}
Representation of point.
Returns
-------
regularized_point : array-like, shape=[..., {dim, [dim_2, dim_2]}]
Point in the manifold's canonical representation.
"""
if point_type is None:
point_type = self.default_point_type
geomstats.error.check_parameter_accepted_values(
point_type, 'point_type', ['vector', 'matrix'])
if point_type == 'vector':
intrinsic = self.metric.is_intrinsic(point)
regularized_point = self._iterate_over_manifolds(
'regularize', {'point': point}, intrinsic)
regularized_point = gs.hstack(regularized_point)
elif point_type == 'matrix':
regularized_point = [
manifold_i.regularize(point[:, i])
for i, manifold_i in enumerate(self.manifolds)
]
regularized_point = gs.stack(regularized_point, axis=1)
return regularized_point
def exp(self, tangent_vec, base_point, n_steps=N_STEPS):
"""Exponential map associated to the Fisher information metric.
Exponential map at base_point of tangent_vec computed by integration
of the geodesic equation (initial value problem), using the
christoffel symbols.
Parameters
----------
tangent_vec : array-like, shape=[n_samples, dim]
base_point : array-like, shape=[n_samples, dim]
n_steps : int
Returns
-------
exp : array-like, shape=[n_samples, dim]
"""
base_point = gs.to_ndarray(base_point, to_ndim=2)
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
def ivp(state, time):
"""Reformat the initial value problem geodesic ODE."""
position, velocity = state[:2], state[2:]
eq = self.geodesic_equation(velocity=velocity, position=position)
return gs.hstack([velocity, eq])
times = gs.linspace(0, 1, n_steps + 1)
exp = []
for point, vec in zip(base_point, tangent_vec):
initial_state = gs.hstack([point, vec])
geodesic = odeint(
ivp, initial_state, times, tuple(), rtol=1e-6)
exp.append(geodesic[-1, :2])
return exp[0] if len(base_point) == 1 else gs.stack(exp)
def coefficients(ind_k):
"""Christoffel symbols for contravariant index ind_k."""
param_k = base_point[..., ind_k]
param_sum = gs.sum(base_point, -1)
c1 = (
1
/ gs.polygamma(1, param_k)
/ (
1 / gs.polygamma(1, param_sum)
- gs.sum(1 / gs.polygamma(1, base_point), -1)
)
)
c2 = -c1 * gs.polygamma(2, param_sum) / gs.polygamma(1, param_sum)
mat_ones = gs.ones((n_points, self.dim, self.dim))
mat_diag = from_vector_to_diagonal_matrix(
-gs.polygamma(2, base_point) / gs.polygamma(1, base_point)
)
arrays = [
gs.zeros((1, ind_k)),
gs.ones((1, 1)),
gs.zeros((1, self.dim - ind_k - 1)),
]
vec_k = gs.tile(gs.hstack(arrays), (n_points, 1))
val_k = gs.polygamma(2, param_k) / gs.polygamma(1, param_k)
vec_k = gs.einsum("i,ij->ij", val_k, vec_k)
mat_k = from_vector_to_diagonal_matrix(vec_k)
mat = (
gs.einsum("i,ijk->ijk", c2, mat_ones)
- gs.einsum("i,ijk->ijk", c1, mat_diag)
+ mat_k
)
return 1 / 2 * mat
def random_von_mises_fisher(self, kappa=10, n_samples=1):
"""Sample in the 2-sphere with the von Mises distribution.
Sample in the 2-sphere with the von Mises distribution centered at the
north pole.
References
----------
https://en.wikipedia.org/wiki/Von_Mises_distribution
Parameters
----------
kappa : int
Kappa parameter of the von Mises distribution.
Optional, default: 10.
n_samples : int
Number of samples.
Optional, default: 1.
Returns
-------
point : array-like, shape=[..., 3]
Points sampled on the sphere in extrinsic coordinates
in Euclidean space of dimension 3.
"""
if self.dim != 2:
raise NotImplementedError(
'Sampling from the von Mises Fisher distribution'
'is only implemented in dimension 2.')
angle = 2. * gs.pi * gs.random.rand(n_samples)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
unit_vector = gs.hstack((gs.cos(angle), gs.sin(angle)))
scalar = gs.random.rand(n_samples)
coord_z = 1. + 1. / kappa * gs.log(
scalar + (1. - scalar) * gs.exp(gs.array(-2. * kappa)))
coord_z = gs.to_ndarray(coord_z, to_ndim=2, axis=1)
coord_xy = gs.sqrt(1. - coord_z**2) * unit_vector
point = gs.hstack((coord_xy, coord_z))
if n_samples == 1:
point = gs.squeeze(point, axis=0)
return point
def _to_lie_algebra(self, tangent_vec):
"""Project vector rotation part onto skew-symmetric matrices."""
translation_mask = gs.hstack(
[gs.ones((self.n, ) * 2), 2 * gs.ones((self.n, 1))])
translation_mask = gs.concatenate(
[translation_mask, gs.zeros((1, self.n + 1))], axis=0)
tangent_vec = tangent_vec * gs.where(translation_mask != 0.,
gs.array(1.), gs.array(0.))
tangent_vec = (tangent_vec - GeneralLinear.transpose(tangent_vec)) / 2.
return tangent_vec * translation_mask
def path(t):
"""Generate parameterized function for geodesic curve.
Parameters
----------
t : array-like, shape=[n_times,]
Times at which to compute points of the geodesics.
Returns
-------
geodesic : array-like, shape=[..., n_times, dim]
Values of the geodesic at times t.
"""
t = gs.to_ndarray(t, to_ndim=1)
n_times = len(t)
geod = []
if n_times < n_steps:
t_int = gs.linspace(0, 1, n_steps + 1)
tangent_vecs = gs.einsum('i,...k->...ik', t,
initial_tangent_vec)
for point, vec in zip(initial_point, tangent_vecs):
point = gs.tile(point, (n_times, 1))
exp = []
for pt, vc in zip(point, vec):
initial_state = gs.hstack([pt, vc])
solution = odeint(ivp,
initial_state,
t_int, (),
rtol=1e-6)
exp.append(solution[-1, :self.dim])
exp = exp[0] if n_times == 1 else gs.stack(exp)
geod.append(exp)
else:
t_int = t
for point, vec in zip(initial_point, initial_tangent_vec):
initial_state = gs.hstack([point, vec])
solution = odeint(ivp, initial_state, t_int, (), rtol=1e-6)
geod.append(solution[:, :self.dim])
return geod[0] if len(initial_point) == 1 else \
gs.stack(geod) # , axis=1)
def loss(y_pred, y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""
Loss function given by a riemannian metric on a Lie group,
by default the left-invariant canonical metric.
"""
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 == 'quaternion':
y_pred_rot_vec = SO3.rotation_vector_from_quaternion(y_pred[:, :4])
y_pred = gs.hstack([y_pred_rot_vec, y_pred[:, 4:]])
y_true_rot_vec = SO3.rotation_vector_from_quaternion(y_true[:, :4])
y_true = gs.hstack([y_true_rot_vec, y_true[:, 4:]])
loss = lie_group.loss(y_pred, y_true, SE3, metric)
return loss
def test_Localization_adjoint_map(self):
initial_state = gs.array([0.5, 1.0, 2.0])
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
first_line = gs.eye(1, 3)
last_lines = gs.hstack((gs.array([[2.0], [-1.0]]), rotation))
expected = gs.vstack((first_line, last_lines))
result = self.nonlinear_model.adjoint_map(initial_state)
self.assertAllClose(expected, result)
def fit(self, X, max_iter=100):
"""Predict for each data point the closest center in terms of
riemannian_metric distance
Parameters
----------
X : array-like, shape=[n_samples, n_features]
Training data, where n_samples is the number of samples
and n_features is the number of features.
max_iter : Maximum number of iterations
Returns
-------
self : object
Return centroids array
"""
n_samples = X.shape[0]
belongs = gs.zeros(n_samples)
self.centroids = [
gs.expand_dims(X[randint(0, n_samples - 1)], 0)
for i in range(self.n_clusters)
]
self.centroids = gs.concatenate(self.centroids)
index = 0
while index < max_iter:
index += 1
dists = [
gs.to_ndarray(
self.riemannian_metric.dist(self.centroids[i], X), 2, 1)
for i in range(self.n_clusters)
]
dists = gs.hstack(dists)
belongs = gs.argmin(dists, 1)
old_centroids = gs.copy(self.centroids)
for i in range(self.n_clusters):
fold = gs.squeeze(X[belongs == i])
if len(fold) > 0:
self.centroids[i] = self.riemannian_metric.mean(fold)
else:
self.centroids[i] = X[randint(0, n_samples - 1)]
centroids_distances = self.riemannian_metric.dist(
old_centroids, self.centroids)
if gs.mean(centroids_distances) < self.tol:
if self.verbose > 0:
print("Convergence Reached after ", index, " iterations")
return gs.copy(self.centroids)
return gs.copy(self.centroids)
def test_Localization_propagation_jacobian(self):
time_step = gs.array([0.5])
linear_vel = gs.array([1.0, 0.5])
angular_vel = gs.array([0.0])
increment = gs.concatenate((time_step, linear_vel, angular_vel),
axis=0)
first_line = gs.eye(1, 3)
last_lines = gs.hstack((gs.array([[-0.25], [0.5]]), gs.eye(2)))
expected = gs.vstack((first_line, last_lines))
result = self.nonlinear_model.propagation_jacobian(None, increment)
self.assertAllClose(expected, result)
def matrix_from_tait_bryan_angles_extrinsic_zyx(self, tait_bryan_angles):
"""Convert Tait-Bryan angles to rot mat in extrensic coords (zyx).
Convert a rotation given in terms of the tait bryan angles,
[angle_1, angle_2, angle_3] in extrinsic (fixed) coordinate system
in order zyx, into a rotation matrix.
rot_mat = X(angle_1).Y(angle_2).Z(angle_3)
where:
- X(angle_1) is a rotation of angle angle_1 around axis x.
- Y(angle_2) is a rotation of angle angle_2 around axis y.
- Z(angle_3) is a rotation of angle angle_3 around axis z.
Parameters
----------
tait_bryan_angles : array-like, shape=[..., 3]
Returns
-------
rot_mat : array-like, shape=[..., n, n]
"""
n_tait_bryan_angles, _ = tait_bryan_angles.shape
rot_mat = gs.zeros((n_tait_bryan_angles,) + (self.n,) * 2)
angle_1 = tait_bryan_angles[:, 0]
angle_2 = tait_bryan_angles[:, 1]
angle_3 = tait_bryan_angles[:, 2]
for i in range(n_tait_bryan_angles):
cos_angle_1 = gs.cos(angle_1[i])
sin_angle_1 = gs.sin(angle_1[i])
cos_angle_2 = gs.cos(angle_2[i])
sin_angle_2 = gs.sin(angle_2[i])
cos_angle_3 = gs.cos(angle_3[i])
sin_angle_3 = gs.sin(angle_3[i])
column_1 = [[cos_angle_2 * cos_angle_3],
[(cos_angle_1 * sin_angle_3
+ cos_angle_3 * sin_angle_1 * sin_angle_2)],
[(sin_angle_1 * sin_angle_3
- cos_angle_1 * cos_angle_3 * sin_angle_2)]]
column_2 = [[- cos_angle_2 * sin_angle_3],
[(cos_angle_1 * cos_angle_3
- sin_angle_1 * sin_angle_2 * sin_angle_3)],
[(cos_angle_3 * sin_angle_1
+ cos_angle_1 * sin_angle_2 * sin_angle_3)]]
column_3 = [[sin_angle_2],
[- cos_angle_2 * sin_angle_1],
[cos_angle_1 * cos_angle_2]]
rot_mat[i] = gs.hstack((column_1, column_2, column_3))
return rot_mat
def __init__(self, n):
super(_SpecialEuclideanMatrices,
self).__init__(default_point_type='matrix', n=n + 1)
self.rotations = SpecialOrthogonal(n=n)
self.translations = Euclidean(dim=n)
self.n = n
self.dim = int((n * (n + 1)) / 2)
translation_mask = gs.hstack(
[gs.ones((self.n, ) * 2), 2 * gs.ones((self.n, 1))])
translation_mask = gs.concatenate(
[translation_mask, gs.zeros((1, self.n + 1))], axis=0)
self.translation_mask = translation_mask
def loss(y_pred,
y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""Loss function given by a Riemannian metric on a Lie group.
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_loss : array-like
Loss using the Riemannian metric.
"""
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 == 'quaternion':
y_pred_rot_vec = SO3.rotation_vector_from_quaternion(y_pred[:, :4])
y_pred = gs.hstack([y_pred_rot_vec, y_pred[:, 4:]])
y_true_rot_vec = SO3.rotation_vector_from_quaternion(y_true[:, :4])
y_true = gs.hstack([y_true_rot_vec, y_true[:, 4:]])
lie_loss = lie_group.loss(y_pred, y_true, SE3, metric)
if gs.ndim(lie_loss) == 2:
lie_loss = gs.squeeze(lie_loss, axis=1)
if gs.ndim(lie_loss) == 1 and gs.shape(lie_loss)[0] == 1:
lie_loss = gs.squeeze(lie_loss, axis=0)
return lie_loss
def log(self, point, base_point):
"""Compute Riemannian logarithm of a curve wrt a base curve.
Parameters
----------
point : array-like, shape=[..., n_sampling_points, ambient_dim]
Discrete curve.
base_point : array-like, shape=[..., n_sampling_points, ambient_dim]
Discrete curve to use as base point.
Returns
-------
log : array-like, shape=[..., n_sampling_points, ambient_dim]
Tangent vector to a discrete curve.
"""
if not isinstance(self.ambient_metric, EuclideanMetric):
raise AssertionError('The logarithm map is only implemented '
'for discrete curves embedded in a '
'Euclidean space.')
point = gs.to_ndarray(point, to_ndim=3)
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_curves, n_sampling_points, n_coords = point.shape
curve_srv = self.square_root_velocity(point)
base_curve_srv = self.square_root_velocity(base_point)
base_curve_velocity = (n_sampling_points - 1) * (base_point[:, 1:, :] -
base_point[:, :-1, :])
base_curve_velocity_norm = self.pointwise_norm(base_curve_velocity,
base_point[:, :-1, :])
inner_prod = self.pointwise_inner_product(curve_srv - base_curve_srv,
base_curve_velocity,
base_point[:, :-1, :])
coef_1 = gs.sqrt(base_curve_velocity_norm)
coef_2 = 1 / base_curve_velocity_norm**(3 / 2) * inner_prod
term_1 = gs.einsum('ij,ijk->ijk', coef_1, curve_srv - base_curve_srv)
term_2 = gs.einsum('ij,ijk->ijk', coef_2, base_curve_velocity)
log_derivative = term_1 + term_2
log_starting_points = self.ambient_metric.log(
point=point[:, 0, :], base_point=base_point[:, 0, :])
log_starting_points = gs.to_ndarray(
log_starting_points, to_ndim=3, axis=1)
log_cumsum = gs.hstack(
[gs.zeros((n_curves, 1, n_coords)),
gs.cumsum(log_derivative, -2)])
log = log_starting_points + 1 / (n_sampling_points - 1) * log_cumsum
return log
def exp(self, tangent_vec, base_point, n_steps=N_STEPS):
"""Compute the exponential map.
Comute the exponential map associated to the Fisher information
metric at base_point of tangent_vec. This is achieved by integration
of the geodesic equation (initial value problem), using the Christoffel
symbols.
Parameters
----------
tangent_vec : array-like, shape=[..., dim]
Tangent vector at base point.
base_point : array-like, shape=[..., dim]
Base point.
n_steps : int
Number of steps for integration.
Optional, default: 100.
Returns
-------
exp : array-like, shape=[..., dim]
End point of the geodesic starting at base_point with
initial velocity tangent_vec and stopping at time 1.
"""
base_point = gs.to_ndarray(base_point, to_ndim=2)
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
n_base_points = base_point.shape[0]
n_tangent_vecs = tangent_vec.shape[0]
if n_base_points > n_tangent_vecs:
raise ValueError('There cannot be more base points than tangent '
'vectors.')
if n_tangent_vecs > n_base_points:
if n_base_points > 1:
raise ValueError('For several tangent vectors, specify '
'either one or the same number of base '
'points.')
base_point = gs.tile(base_point, (n_tangent_vecs, 1))
def ivp(state, _):
"""Reformat the initial value problem geodesic ODE."""
position, velocity = state[:self.dim], state[self.dim:]
eq = self.geodesic_equation(velocity=velocity, position=position)
return gs.hstack(eq)
times = gs.linspace(0, 1, n_steps + 1)
exp = []
for point, vec in zip(base_point, tangent_vec):
initial_state = gs.hstack([point, vec])
geodesic = odeint(ivp, initial_state, times, (), rtol=1e-6)
exp.append(geodesic[-1, :self.dim])
return exp[0] if len(base_point) == 1 else gs.stack(exp)
def inner_product(
self, tangent_vec_a, tangent_vec_b, base_point=None,
point_type=None):
"""Compute the inner-product of two tangent vectors at a base point.
Inner product defined by the Riemannian metric at point `base_point`
between tangent vectors `tangent_vec_a` and `tangent_vec_b`.
Parameters
----------
tangent_vec_a : array-like, shape=[n_samples, dimension + 1]
First tangent vector at base point.
tangent_vec_b : array-like, shape=[n_samples, dimension + 1]
Second tangent vector at base point.
base_point : array-like, shape=[n_samples, dimension + 1], optional
Point on the manifold.
point_type : str, {'vector', 'matrix'}
Type of representation used for points.
Returns
-------
inner_prod : array-like, shape=[n_samples, 1]
Inner-product of the two tangent vectors.
"""
if base_point is None:
base_point = [None, ] * self.n_metrics
if point_type is None:
point_type = self.default_point_type
if point_type == 'vector':
tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=2)
tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
intrinsic = self._is_intrinsic(tangent_vec_b)
args = {'tangent_vec_a': tangent_vec_a,
'tangent_vec_b': tangent_vec_b,
'base_point': base_point}
inner_prod = self._iterate_over_metrics(
'inner_product', args, intrinsic)
return gs.sum(gs.hstack(inner_prod), axis=1)
elif point_type == 'matrix':
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)
inner_products = [metric.inner_product(tangent_vec_a[:, i],
tangent_vec_b[:, i],
base_point[:, i])
for i, metric in enumerate(self.metrics)]
return sum(inner_products)
else:
raise ValueError('invalid point_type argument: {}, expected '
'either matrix of vector'.format(point_type))