def norm(self, vector, base_point=None):
"""
Norm of a vector associated to the inner product
at the tangent space at a base point.
Note: This only works for positive-definite
Riemannian metrics and inner products.
Parameters
----------
vector: array-like, shape=[n_samples, dimension]
or shape=[1, dimension]
base_point: array-like, shape=[n_samples, dimension]
or shape=[1, dimension]
"""
sq_norm = self.squared_norm(vector, base_point)
norm = gs.sqrt(sq_norm)
return norm
def regularize(self, point):
"""
Regularize a point to the canonical representation
chosen for the Hyperbolic space, to avoid numerical issues.
"""
assert gs.all(self.belongs(point))
point = gs.to_ndarray(point, to_ndim=2)
sq_norm = self.embedding_metric.squared_norm(point)
real_norm = gs.sqrt(gs.abs(sq_norm))
mask_0 = gs.isclose(real_norm, 0)
mask_0 = gs.squeeze(mask_0, axis=1)
mask_not_0 = ~mask_0
projected_point = point
projected_point[mask_not_0] = (point[mask_not_0] /
real_norm[mask_not_0])
return projected_point
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, optional
Kappa parameter of the von Mises distribution.
n_samples : int, optional
Number of samples.
Returns
-------
point : array-like, shape=[n_samples, 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 pointwise_norm(self, tangent_vec, base_curve):
"""Compute the point-wise norm of a tangent vector at a base curve.
Parameters
----------
tangent_vec : array-like, shape=[..., n_sampling_points, ambient_dim]
Tangent vector to discrete curve.
base_curve : array-like, shape=[..., n_sampling_points, ambient_dim]
Point representing a discrete curve.
Returns
-------
norm : array-like, shape=[..., n_sampling_points]
Point-wise norms.
"""
sq_norm = self.pointwise_inner_product(
tangent_vec_a=tangent_vec, tangent_vec_b=tangent_vec,
base_curve=base_curve)
return gs.sqrt(sq_norm)
def noise_jacobian(state, sensor_input):
r"""Compute the matrix associated to the propagation noise.
The noise being considered multiplicative, it is simply the identity
scaled by the time step.
Parameters
----------
state : unused
sensor_input : array-like, shape=[4]
Vector representing the information from the sensor.
Returns
-------
jacobian : array-like, shape=[dim_noise, dim]
Jacobian of the propagation w.r.t. the noise.
"""
dt, _, _ = Localization.preprocess_input(sensor_input)
return gs.sqrt(dt) * gs.eye(Localization.dim_noise)
def orthonormal_basis(self, basis, base_point=None):
"""Orthonormalize the basis with respect to the metric.
This corresponds to a renormalization.
Parameters
----------
basis : array-like, shape=[dim, dim]
Matrix of a metric.
base_point
Returns
-------
basis : array-like, shape=[dim, n, n]
Orthonormal basis.
"""
norms = self.squared_norm(basis, base_point)
return gs.einsum('i, ikl->ikl', 1. / gs.sqrt(norms), basis)
def exp(self, tangent_vec, base_point):
"""
Riemannian exponential of a tangent vector wrt to a base point.
"""
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
sq_norm_tangent_vec = self.embedding_metric.squared_norm(tangent_vec)
norm_tangent_vec = gs.sqrt(sq_norm_tangent_vec)
mask_0 = gs.isclose(sq_norm_tangent_vec, 0.)
mask_0 = gs.to_ndarray(mask_0, to_ndim=1)
mask_else = ~mask_0
mask_else = gs.to_ndarray(mask_else, to_ndim=1)
mask_0_float = gs.cast(mask_0, gs.float32)
mask_else_float = gs.cast(mask_else, gs.float32)
coef_1 = gs.zeros_like(norm_tangent_vec)
coef_2 = gs.zeros_like(norm_tangent_vec)
coef_1 += mask_0_float * (1. +
COSH_TAYLOR_COEFFS[2] * norm_tangent_vec**2 +
COSH_TAYLOR_COEFFS[4] * norm_tangent_vec**4 +
COSH_TAYLOR_COEFFS[6] * norm_tangent_vec**6 +
COSH_TAYLOR_COEFFS[8] * norm_tangent_vec**8)
coef_2 += mask_0_float * (1. +
SINH_TAYLOR_COEFFS[3] * norm_tangent_vec**2 +
SINH_TAYLOR_COEFFS[5] * norm_tangent_vec**4 +
SINH_TAYLOR_COEFFS[7] * norm_tangent_vec**6 +
SINH_TAYLOR_COEFFS[9] * norm_tangent_vec**8)
# This avoids dividing by 0.
norm_tangent_vec += mask_0_float * 1.0
coef_1 += mask_else_float * (gs.cosh(norm_tangent_vec))
coef_2 += mask_else_float * ((gs.sinh(norm_tangent_vec) /
(norm_tangent_vec)))
exp = (gs.einsum('ni,nj->nj', coef_1, base_point) +
gs.einsum('ni,nj->nj', coef_2, tangent_vec))
hyperbolic_space = HyperbolicSpace(dimension=self.dimension)
exp = hyperbolic_space.regularize(exp)
return exp
def msd_hbar_s2(location, data):
"""compute the mean-square deviation from the location to the points of
the dataset and the mean Hessian of square distance at the location"""
sphere = Hypersphere(2)
num_sample = len(data)
assert num_sample > 0, "Dataset needs to have at least one data"
var = 0.0
hbar = 0.0
for item in data:
sq_dist = sphere.metric.squared_dist(location, item)[0, 0]
var = var + sq_dist
# hbar = E(h(dist ^ 2)) with h(t) = sqrt(t) cot( sqrt(t) ) for kappa=1
if sq_dist > 1e-4:
d = gs.sqrt(sq_dist)
h = d / gs.tan(d)
else:
h = 1.0 - sq_dist / 3.0 - sq_dist ** 2 / 45.0 - 2 / 945 * \
sq_dist ** 3 - sq_dist ** 4 / 4725
hbar = hbar + h
return var / num_sample, hbar / num_sample
def test_intrinsic_and_extrinsic_coords(self):
"""
Test that the composition of
intrinsic_to_extrinsic_coords and
extrinsic_to_intrinsic_coords
gives the identity.
"""
point_int = gs.array([.1, 0., 0., .1])
point_ext = self.space.intrinsic_to_extrinsic_coords(point_int)
result = self.space.extrinsic_to_intrinsic_coords(point_ext)
expected = point_int
self.assertAllClose(result, expected)
point_ext = (1. / (gs.sqrt(6.)) * gs.array([1., 0., 0., 1., 2.]))
point_int = self.space.extrinsic_to_intrinsic_coords(point_ext)
result = self.space.intrinsic_to_extrinsic_coords(point_int)
expected = point_ext
self.assertAllClose(result, expected)
def sample(self, n_samples):
"""Generate samples for SPD manifold."""
if isinstance(self.manifold.metric, SPDMetricLogEuclidean):
sym_matrix = self.manifold.logm(self.mean)
mean_euclidean = gs.hstack(
(
gs.diagonal(sym_matrix)[None, :],
gs.sqrt(2.0) * gs.triu_to_vec(sym_matrix, k=1)[None, :],
)
)[0]
_samples = self.samples_sym(mean_euclidean, self.cov, n_samples)
else:
samples_sym = self.samples_sym(
gs.zeros(self.manifold.dim), self.cov, n_samples
)
mean_half = self.manifold.powerm(self.mean, 0.5)
_samples = Matrices.mul(mean_half, samples_sym, mean_half)
return self.manifold.expm(_samples)
def pointwise_norm(self, tangent_vec, base_curve):
"""Compute the norm of tangent vector components at base curve.
TODO: (revise this to refer to action on single elements)
Compute the norms of the components of a (series of) tangent
vector(s) at (a) base curve(s).
Parameters
----------
tangent_vec :
base_curve :
Returns
-------
norm :
"""
sq_norm = self.pointwise_inner_product(tangent_vec_a=tangent_vec,
tangent_vec_b=tangent_vec,
base_curve=base_curve)
return gs.sqrt(sq_norm)
def log(self, curve, base_curve):
"""
Riemannian logarithm of a curve wrt a base curve.
"""
if not isinstance(self.embedding_metric, EuclideanMetric):
raise AssertionError('The logarithm map is only implemented '
'for dicretized curves embedded in a '
'Euclidean space.')
curve = gs.to_ndarray(curve, to_ndim=3)
base_curve = gs.to_ndarray(base_curve, to_ndim=3)
n_curves, n_sampling_points, n_coords = curve.shape
curve_srv = self.square_root_velocity(curve)
base_curve_srv = self.square_root_velocity(base_curve)
base_curve_velocity = (n_sampling_points - 1) * (base_curve[:, 1:, :] -
base_curve[:, :-1, :])
base_curve_velocity_norm = self.pointwise_norm(base_curve_velocity,
base_curve[:, :-1, :])
inner_prod = self.pointwise_inner_product(curve_srv - base_curve_srv,
base_curve_velocity,
base_curve[:, :-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.embedding_metric.log(
point=curve[:, 0, :], base_point=base_curve[:, 0, :])
log_starting_points = gs.transpose(
np.tile(log_starting_points, (1, 1, 1)), (1, 0, 2))
log_cumsum = gs.hstack(
[gs.zeros((n_curves, 1, n_coords)),
np.cumsum(log_derivative, -2)])
log = log_starting_points + 1 / (n_sampling_points - 1) * log_cumsum
return log
def test_intrinsic_and_extrinsic_coords(self):
"""
Test that the composition of
intrinsic_to_extrinsic_coords and
extrinsic_to_intrinsic_coords
gives the identity.
"""
space = Hypersphere(dim=2)
point_int = gs.array([0.1, 0.0])
point_ext = space.intrinsic_to_extrinsic_coords(point_int)
result = space.extrinsic_to_intrinsic_coords(point_ext)
expected = point_int
self.assertAllClose(result, expected)
point_ext = 1. / (gs.sqrt(2.)) * gs.array([1.0, 0.0, 1.0])
point_int = space.extrinsic_to_intrinsic_coords(point_ext)
result = space.intrinsic_to_extrinsic_coords(point_int)
expected = point_ext
self.assertAllClose(result, expected)
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""
Convert from the intrinsic coordinates in the Hypersphere,
to the extrinsic coordinates in Euclidean space.
Parameters
----------
point_intrinsic : array-like, shape=[n_samples, dimension]
Returns
-------
point_extrinsic : array-like, shape=[n_samples, dimension + 1]
"""
point_intrinsic = gs.to_ndarray(point_intrinsic, to_ndim=2)
coord_0 = gs.sqrt(1. - gs.linalg.norm(point_intrinsic, axis=-1)**2)
coord_0 = gs.to_ndarray(coord_0, to_ndim=2, axis=-1)
point_extrinsic = gs.concatenate([coord_0, point_intrinsic], axis=-1)
return point_extrinsic
def dist(self, point_a, point_b):
"""Geodesic distance between two points.
Note: It only works for positive definite
Riemannian metrics.
Parameters
----------
point_a : array-like, shape=[..., dim]
Point.
point_b : array-like, shape=[..., dim]
Point.
Returns
-------
dist : array-like, shape=[...,]
Distance.
"""
sq_dist = self.squared_dist(point_a, point_b)
dist = gs.sqrt(sq_dist)
return dist
def _intrinsic_to_extrinsic_coordinates(point):
"""Convert intrinsic to extrinsic coordinates.
Convert the parameterization of a point in hyperbolic space
from its intrinsic coordinates, to its extrinsic coordinates
in Minkowski space.
Parameters
----------
point : array-like, shape=[..., dim]
Point in hyperbolic space in intrinsic coordinates.
Returns
-------
point_extrinsic : array-like, shape=[..., dim + 1]
Point in hyperbolic space in extrinsic coordinates.
"""
coord_0 = gs.sqrt(1.0 + gs.sum(point ** 2, axis=-1))
point_extrinsic = gs.concatenate([coord_0[..., None], point], axis=-1)
return point_extrinsic
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""
Convert the parameterization of a point on the Hyperbolic space
from its intrinsic coordinates, to its extrinsic coordinates
in Minkowski space.
Parameters
----------
point_intrinsic : array-like, shape=[n_samples, dimension]
Returns
-------
point_extrinsic : array-like, shape=[n_samples, dimension + 1]
"""
point_intrinsic = gs.to_ndarray(point_intrinsic, to_ndim=2)
coord_0 = gs.sqrt(1. + gs.linalg.norm(point_intrinsic, axis=-1) ** 2)
coord_0 = gs.to_ndarray(coord_0, to_ndim=2, axis=1)
point_extrinsic = gs.concatenate([coord_0, point_intrinsic], axis=-1)
return point_extrinsic
def exp(self, tangent_vec, base_point):
"""
Riemannian exponential of a tangent vector wrt to a base point.
"""
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
sq_norm_tangent_vec = self.embedding_metric.squared_norm(tangent_vec)
norm_tangent_vec = gs.sqrt(sq_norm_tangent_vec)
mask_0 = gs.isclose(sq_norm_tangent_vec, 0)
mask_0 = gs.to_ndarray(mask_0, to_ndim=1)
mask_else = ~mask_0
mask_else = gs.to_ndarray(mask_else, to_ndim=1)
coef_1 = gs.zeros_like(norm_tangent_vec)
coef_2 = gs.zeros_like(norm_tangent_vec)
coef_1[mask_0] = (1. +
COSH_TAYLOR_COEFFS[2] * norm_tangent_vec[mask_0]**2 +
COSH_TAYLOR_COEFFS[4] * norm_tangent_vec[mask_0]**4 +
COSH_TAYLOR_COEFFS[6] * norm_tangent_vec[mask_0]**6 +
COSH_TAYLOR_COEFFS[8] * norm_tangent_vec[mask_0]**8)
coef_2[mask_0] = (1. +
SINH_TAYLOR_COEFFS[3] * norm_tangent_vec[mask_0]**2 +
SINH_TAYLOR_COEFFS[5] * norm_tangent_vec[mask_0]**4 +
SINH_TAYLOR_COEFFS[7] * norm_tangent_vec[mask_0]**6 +
SINH_TAYLOR_COEFFS[9] * norm_tangent_vec[mask_0]**8)
coef_1[mask_else] = gs.cosh(norm_tangent_vec[mask_else])
coef_2[mask_else] = (gs.sinh(norm_tangent_vec[mask_else]) /
norm_tangent_vec[mask_else])
exp = (gs.einsum('ni,nj->nj', coef_1, base_point) +
gs.einsum('ni,nj->nj', coef_2, tangent_vec))
hyperbolic_space = HyperbolicSpace(dimension=self.dimension)
exp = hyperbolic_space.regularize(exp)
return exp
def exp(self, tangent_vec, base_curve):
"""
Riemannian exponential of a tangent vector wrt to a base curve.
"""
if not isinstance(self.embedding_metric, EuclideanMetric):
raise AssertionError('The exponential map is only implemented '
'for dicretized curves embedded in a '
'Euclidean space.')
base_curve = gs.to_ndarray(base_curve, to_ndim=3)
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=3)
n_sampling_points = base_curve.shape[1]
base_curve_srv = self.square_root_velocity(base_curve)
tangent_vec_derivative = (n_sampling_points - 1) * (
tangent_vec[:, 1:, :] - tangent_vec[:, :-1, :])
base_curve_velocity = (n_sampling_points - 1) * (base_curve[:, 1:, :] -
base_curve[:, :-1, :])
base_curve_velocity_norm = self.pointwise_norm(base_curve_velocity,
base_curve[:, :-1, :])
inner_prod = self.pointwise_inner_product(tangent_vec_derivative,
base_curve_velocity,
base_curve[:, :-1, :])
coef_1 = 1 / gs.sqrt(base_curve_velocity_norm)
coef_2 = -1 / (2 * base_curve_velocity_norm**(5 / 2)) * inner_prod
term_1 = gs.einsum('ij,ijk->ijk', coef_1, tangent_vec_derivative)
term_2 = gs.einsum('ij,ijk->ijk', coef_2, base_curve_velocity)
srv_initial_derivative = term_1 + term_2
end_curve_srv = self.l2_metric.exp(tangent_vec=srv_initial_derivative,
base_curve=base_curve_srv)
end_curve_starting_point = self.embedding_metric.exp(
tangent_vec=tangent_vec[:, 0, :], base_point=base_curve[:, 0, :])
end_curve = self.square_root_velocity_inverse(
end_curve_srv, end_curve_starting_point)
return end_curve
def dist(self, curve_a, curve_b):
"""
Geodesic distance between two discretized curves.
"""
assert curve_a.shape == curve_b.shape
curve_a = gs.to_ndarray(curve_a, to_ndim=3)
curve_b = gs.to_ndarray(curve_b, to_ndim=3)
n_curves, n_sampling_points, n_coords = curve_a.shape
curve_a = gs.reshape(curve_a, (n_curves * n_sampling_points, n_coords))
curve_b = gs.reshape(curve_b, (n_curves * n_sampling_points, n_coords))
dist = self.embedding_metric.dist(curve_a, curve_b)
dist = gs.reshape(dist, (n_curves, n_sampling_points))
n_sampling_points_float = gs.array(n_sampling_points)
n_sampling_points_float = gs.cast(n_sampling_points_float, gs.float32)
dist = gs.sqrt(gs.sum(dist**2, -1) / n_sampling_points_float)
dist = gs.to_ndarray(dist, to_ndim=1)
dist = gs.to_ndarray(dist, to_ndim=2, axis=1)
return dist
def test_intrinsic_and_extrinsic_coords(self):
"""
Test that the composition of
intrinsic_to_extrinsic_coords and
extrinsic_to_intrinsic_coords
gives the identity.
"""
point_int = gs.array([.1, 0., 0., .1])
point_ext = self.space.intrinsic_to_extrinsic_coords(point_int)
result = self.space.extrinsic_to_intrinsic_coords(point_ext)
expected = point_int
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
point_ext = 1. / (gs.sqrt(6.)) * gs.array([1., 0., 0., 1., 2.])
point_int = self.space.extrinsic_to_intrinsic_coords(point_ext)
result = self.space.intrinsic_to_extrinsic_coords(point_int)
expected = point_ext
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
def estimation(kalman, initial_state, inputs, observations, obs_freq):
"""Carry out the state estimation for a specific system.
Parameters
----------
kalman : KalmanFilter
Filter used to estimate the state.
initial_state : array-like, shape=[dim]
Guess of the true initial state.
inputs : list(array-like, shape=[dim_input])
Inputs received by the propagation sensor.
observations : array-like, shape=[len(inputs) + 1/obs_freq, dim_obs]
Measurements of the system.
obs_freq : int
Number of time steps between observations.
Returns
-------
traj : array-like, shape=[len(inputs) + 1, dim]
Estimated trajectory.
three_sigmas : array-like, shape=[len(inputs) + 1, dim]
3-sigma envelope of the estimated state covariance.
"""
kalman.state = 1 * initial_state
traj = [1 * kalman.state]
uncertainty = [1 * gs.diagonal(kalman.covariance)]
for i, _ in enumerate(inputs):
kalman.propagate(inputs[i])
if i > 0 and i % obs_freq == obs_freq - 1:
kalman.update(observations[(i // obs_freq)])
traj.append(1 * kalman.state)
uncertainty.append(1 * gs.diagonal(kalman.covariance))
traj = gs.array(traj)
uncertainty = gs.array(uncertainty)
three_sigmas = 3 * gs.sqrt(uncertainty)
return traj, three_sigmas
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 random_von_mises_fisher(self, kappa=10, n_samples=1):
"""
Sample in the 2-sphere with the von Mises distribution
centered in the north pole.
"""
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(-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 _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 noise_jacobian(state, sensor_input):
r"""Compute the matrix associated to the propagation noise.
The noise is supposed additive and only applies to the speed part.
The Jacobian is given by :math:`\begin{bmatrix} 0 & \sqrt{dt}
\end{bmatrix}`.
Parameters
----------
state : unused
sensor_input : array-like, shape=[2]
Vector representing the information from the accelerometer.
Returns
-------
jacobian : array-like, shape=[dim_noise, dim]
Jacobian of the propagation w.r.t. the noise.
"""
dt, _ = sensor_input
dim = LocalizationLinear.dim
position_wrt_noise = gs.zeros((dim // 2, dim // 2))
speed_wrt_noise = gs.sqrt(dt) * gs.eye(dim // 2)
jac = gs.vstack((position_wrt_noise, speed_wrt_noise))
return jac
def dist(self, point_a, point_b):
"""Compute the geodesic distance between two points.
Parameters
----------
point_a : array-like, shape=[..., dim]
First point in the Poincare ball.
point_b : array-like, shape=[..., dim]
Second point in the Poincare ball.
Returns
-------
dist : array-like, shape=[...,]
Geodesic distance between the two points.
"""
point_a_norm = gs.clip(gs.sum(point_a**2, -1), 0.0, 1 - EPSILON)
point_b_norm = gs.clip(gs.sum(point_b**2, -1), 0.0, 1 - EPSILON)
diff_norm = gs.sum((point_a - point_b) ** 2, -1)
norm_function = 1 + 2 * diff_norm / ((1 - point_a_norm) * (1 - point_b_norm))
dist = gs.log(norm_function + gs.sqrt(norm_function**2 - 1))
dist *= self.scale
return dist
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""Convert point from intrinsic to extrensic coordinates.
Convert from the intrinsic coordinates in the Hypersphere,
to the extrinsic coordinates in Euclidean space.
Parameters
----------
point_intrinsic : array-like, shape=[n_samples, dimension]
Returns
-------
point_extrinsic : array-like, shape=[n_samples, dimension + 1]
"""
point_intrinsic = gs.to_ndarray(point_intrinsic, to_ndim=2)
# FIXME: The next line needs to be guarded against taking the sqrt of
# negative numbers.
coord_0 = gs.sqrt(1. - gs.linalg.norm(point_intrinsic, axis=-1)**2)
coord_0 = gs.to_ndarray(coord_0, to_ndim=2, axis=-1)
point_extrinsic = gs.concatenate([coord_0, point_intrinsic], axis=-1)
return point_extrinsic
def _intrinsic_to_extrinsic_coordinates(point_intrinsic):
"""Convert intrinsic to extrinsic coordinates.
Convert the parameterization of a point in hyperbolic space
from its intrinsic coordinates, to its extrinsic coordinates
in Minkowski space.
Parameters
----------
point_intrinsic : array-like, shape=[..., dim]
Point in hyperbolic space in intrinsic coordinates.
Returns
-------
point_extrinsic : array-like, shape=[..., dim + 1]
Point in hyperbolic space in extrinsic coordinates.
"""
coord_0 = gs.sqrt(1. + gs.linalg.norm(point_intrinsic, axis=-1)**2)
coord_0 = gs.to_ndarray(coord_0, to_ndim=1)
coord_0 = gs.to_ndarray(coord_0, to_ndim=2, axis=1)
point_extrinsic = gs.hstack([coord_0, point_intrinsic])
return point_extrinsic
def random_von_mises_fisher(
self, mu=None, kappa=10, n_samples=1, max_iter=100):
"""Sample with the von Mises-Fisher distribution.
This distribution corresponds to the maximum entropy distribution
given a mean. In dimension 2, a closed form expression is available.
In larger dimension, rejection sampling is used according to [Wood94]_
References
----------
https://en.wikipedia.org/wiki/Von_Mises-Fisher_distribution
.. [Wood94] Wood, Andrew T. A. “Simulation of the von Mises Fisher
Distribution.” Communications in Statistics - Simulation
and Computation, June 27, 2007.
https://doi.org/10.1080/03610919408813161.
Parameters
----------
mu : array-like, shape=[dim]
Mean parameter of the distribution.
kappa : float
Kappa parameter of the von Mises distribution.
Optional, default: 10.
n_samples : int
Number of samples.
Optional, default: 1.
max_iter : int
Maximum number of trials in the rejection algorithm. In case it
is reached, the current number of samples < n_samples is returned.
Optional, default: 100.
Returns
-------
point : array-like, shape=[n_samples, dim + 1]
Points sampled on the sphere in extrinsic coordinates
in Euclidean space of dimension dim + 1.
"""
dim = self.dim
if dim == 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_x = 1. + 1. / kappa * gs.log(
scalar + (1. - scalar) * gs.exp(gs.array(-2. * kappa)))
coord_x = gs.to_ndarray(coord_x, to_ndim=2, axis=1)
coord_yz = gs.sqrt(1. - coord_x ** 2) * unit_vector
sample = gs.hstack((coord_x, coord_yz))
else:
# rejection sampling in the general case
sqrt = gs.sqrt(4 * kappa ** 2. + dim ** 2)
envelop_param = (-2 * kappa + sqrt) / dim
node = (1. - envelop_param) / (1. + envelop_param)
correction = kappa * node + dim * gs.log(1. - node ** 2)
n_accepted, n_iter = 0, 0
result = []
while (n_accepted < n_samples) and (n_iter < max_iter):
sym_beta = beta.rvs(
dim / 2, dim / 2, size=n_samples - n_accepted)
sym_beta = gs.cast(sym_beta, node.dtype)
coord_x = (1 - (1 + envelop_param) * sym_beta) / (
1 - (1 - envelop_param) * sym_beta)
accept_tol = gs.random.rand(n_samples - n_accepted)
criterion = (
kappa * coord_x
+ dim * gs.log(1 - node * coord_x)
- correction) > gs.log(accept_tol)
result.append(coord_x[criterion])
n_accepted += gs.sum(criterion)
n_iter += 1
if n_accepted < n_samples:
logging.warning(
'Maximum number of iteration reached in rejection '
'sampling before n_samples were accepted.')
coord_x = gs.concatenate(result)
coord_rest = _Hypersphere(dim - 1).random_uniform(n_accepted)
coord_rest = gs.einsum(
'...,...i->...i', gs.sqrt(1 - coord_x ** 2), coord_rest)
sample = gs.concatenate([coord_x[..., None], coord_rest], axis=1)
if mu is not None:
sample = utils.rotate_points(sample, mu)
return sample if (n_samples > 1) else sample[0]
