def compose(self, point_a, point_b):
"""Compute the group product of elements `point_a` and `point_b`.
Parameters
----------
point_a : array-like, shape=[..., 3]
Left factor in the product.
point_b : array-like, shape=[..., 3]
Right factor in the product.
Returns
-------
point_ab : array-like, shape=[..., 3]
Product of point_a and point_b along the first dimension.
"""
point_ab = point_a + point_b
point_ab_additional_term = gs.array(
1 / 2 * (point_a[..., 0] * point_b[..., 1] -
point_a[..., 1] * point_b[..., 0]))
point_ab = point_ab + gs.concatenate(
[
gs.zeros_like(point_ab[..., :2]),
point_ab_additional_term[..., None]
],
axis=-1,
)
return point_ab
def load_cities():
"""Load data from data/cities/cities.json.
Returns
-------
data : array-like, shape=[50, 2]
Array with each row representing one sample,
i. e. latitude and longitude of a city.
Angles are in radians.
name : list
List of city names.
"""
with open(CITIES_PATH, encoding="utf-8") as json_file:
data_file = json.load(json_file)
names = [row["city"] for row in data_file]
data = list(
map(
lambda row:
[row[col_name] / 180 * gs.pi for col_name in ["lat", "lng"]],
data_file,
))
data = gs.array(data)
colat = gs.pi / 2 - data[:, 0]
colat = gs.expand_dims(colat, axis=1)
lng = gs.expand_dims(data[:, 1] + gs.pi, axis=1)
data = gs.concatenate([colat, lng], axis=1)
sphere = Hypersphere(dim=2)
data = sphere.spherical_to_extrinsic(data)
return data, names
def main():
"""Plot the geodesics."""
initial_point = gs.array([np.sqrt(2), 1., 0.])
stack_initial_point = gs.stack([initial_point] * N_DISKS, axis=0)
initial_point = gs.to_ndarray(stack_initial_point, to_ndim=3)
end_point_intrinsic = gs.array([1.5, 1.5])
end_point_intrinsic = end_point_intrinsic.reshape(1, 1, 2)
end_point = POINCARE_POLYDISK.intrinsic_to_extrinsic_coords(
end_point_intrinsic)
end_point = gs.concatenate([end_point] * N_DISKS, axis=1)
vector = gs.array([3.5, 0.6, 0.8])
stack_vector = gs.stack([vector] * N_DISKS, axis=0)
vector = gs.to_ndarray(stack_vector, to_ndim=3)
initial_tangent_vec = POINCARE_POLYDISK.projection_to_tangent_space(
vector=vector, base_point=initial_point)
fig = plt.figure()
plot_geodesic_between_two_points(initial_point=initial_point,
end_point=end_point,
ax=fig)
plot_geodesic_with_initial_tangent_vector(
initial_point=initial_point,
initial_tangent_vec=initial_tangent_vec,
ax=fig)
plt.show()
def random_point(self, n_samples=1, bound=1.):
"""Sample in the product space from the uniform distribution.
Parameters
----------
n_samples : int, optional
Number of samples.
bound : float
Bound of the interval in which to sample for non compact manifolds.
Optional, default: 1.
Returns
-------
samples : array-like, shape=[..., dim + 1]
Points sampled on the hypersphere.
"""
point_type = self.default_point_type
geomstats.errors.check_parameter_accepted_values(
point_type, 'point_type', ['vector', 'matrix'])
if point_type == 'vector':
data = self.manifolds[0].random_point(n_samples, bound)
if len(self.manifolds) > 1:
for space in self.manifolds[1:]:
samples = space.random_point(n_samples, bound)
data = gs.concatenate([data, samples], axis=-1)
return data
point = [
space.random_point(n_samples, bound) for space in self.manifolds
]
samples = gs.stack(point, axis=-2)
return samples
def load_poses(only_rotations=True):
"""Load data from data/poses/poses.csv.
Returns
-------
data : array-like, shape=[5, 3] or shape=[5, 6]
Array with each row representing one sample,
i. e. one 3D rotation or one 3D rotation + 3D translation.
img_paths : list
List of img paths.
"""
data = []
img_paths = []
so3 = SpecialOrthogonal(n=3, point_type="vector")
with open(POSES_PATH) as json_file:
data_file = json.load(json_file)
for row in data_file:
pose_mat = gs.array(row["rot_mat"])
pose_vec = so3.rotation_vector_from_matrix(pose_mat)
if not only_rotations:
trans_vec = gs.array(row["trans_mat"])
pose_vec = gs.concatenate([pose_vec, trans_vec], axis=-1)
data.append(pose_vec)
img_paths.append(row["img"])
data = gs.array(data)
return data, img_paths
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""Convert point from intrinsic to extrinsic 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]
Point on the hypersphere, in intrinsic coordinates.
Returns
-------
point_extrinsic : array-like, shape=[n_samples, dimension + 1]
Point on the hypersphere, in extrinsic coordinates in
Euclidean space.
"""
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 test_dist_broadcast(self):
point_a = gs.array([[0.2, 0.5], [0.3, 0.1]])
point_b = gs.array([[0.3, -0.5], [0.2, 0.2]])
point_c = gs.array([[0.2, 0.3], [0.5, 0.5], [-0.4, 0.1]])
point_d = gs.array([0.1, 0.2, 0.3])
dist_a_b = self.manifold.metric.dist_broadcast(point_a, point_b)
dist_b_c = gs.flatten(
self.manifold.metric.dist_broadcast(point_b, point_c))
result_vect = gs.concatenate((dist_a_b, dist_b_c), axis=0)
result_a_b = [
self.manifold.metric.dist_broadcast(point_a[i], point_b[i])
for i in range(len(point_b))
]
result_b_c = [
self.manifold.metric.dist_broadcast(point_b[i], point_c[j])
for i in range(len(point_b)) for j in range(len(point_c))
]
result = result_a_b + result_b_c
result = gs.stack(result, axis=0)
self.assertAllClose(result_vect, result)
with pytest.raises(ValueError):
self.manifold.metric.dist_broadcast(point_a, point_d)
def _ball_to_extrinsic_coordinates(point):
"""Convert ball to extrinsic coordinates.
Convert the parameterization of a point in hyperbolic space
from its poincare ball model coordinates, to the extrinsic
coordinates.
Parameters
----------
point : array-like, shape=[..., dim]
Point in hyperbolic space in Poincare ball coordinates.
Returns
-------
extrinsic : array-like, shape=[..., dim + 1]
Point in hyperbolic space in extrinsic coordinates.
"""
squared_norm = gs.sum(point**2, -1)
denominator = 1 - squared_norm
t = gs.to_ndarray((1 + squared_norm) / denominator, to_ndim=2, axis=1)
expanded_denominator = gs.expand_dims(denominator, -1)
expanded_denominator = gs.tile(expanded_denominator,
(1, point.shape[-1]))
intrinsic = (2 * point) / expanded_denominator
return gs.concatenate([t, intrinsic], -1)
def exp_domain(self, tangent_vec, base_point):
"""Compute the domain of the Euclidean exponential map.
Compute the real interval of time where the Euclidean geodesic starting
at point `base_point` in direction `tangent_vec` is defined.
Parameters
----------
tangent_vec : array-like, shape=[n_samples, n, n]
base_point : array-like, shape=[n_samples, n, n]
Returns
-------
exp_domain : array-like, shape=[n_samples, 2]
"""
base_point = gs.to_ndarray(base_point, to_ndim=3)
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=3)
invsqrt_base_point = gs.linalg.powerm(base_point, -.5)
reduced_vec = gs.matmul(invsqrt_base_point, tangent_vec)
reduced_vec = gs.matmul(reduced_vec, invsqrt_base_point)
eigvals = gs.linalg.eigvalsh(reduced_vec)
min_eig = gs.amin(eigvals, axis=1)
max_eig = gs.amax(eigvals, axis=1)
inf_value = gs.where(max_eig <= 0, -math.inf, - 1 / max_eig)
inf_value = gs.to_ndarray(inf_value, to_ndim=2)
sup_value = gs.where(min_eig >= 0, math.inf, - 1 / min_eig)
sup_value = gs.to_ndarray(sup_value, to_ndim=2)
domain = gs.concatenate((inf_value, sup_value), axis=1)
return domain
def propagate(state, sensor_input):
r"""Propagate state with constant velocity motion model on SE(2).
From a given state (orientation, position) pair :math:`(\theta, x)`,
a new one is obtained as :math:`(\theta + dt * \omega,
x + dt * R(\theta) u)`, where the time step, the linear and angular
velocities u and :math:\omega are given some sensor (e.g., odometers).
Parameters
----------
state : array-like, shape=[dim]
Vector representing a state (orientation, position).
sensor_input : array-like, shape=[4]
Vector representing the information from the sensor.
Returns
-------
new_state : array-like, shape=[dim]
Vector representing the propagated state.
"""
dt, linear_vel, angular_vel = Localization.preprocess_input(sensor_input)
theta, _, _ = state
local_vel = gs.matvec(Localization.rotation_matrix(theta), linear_vel)
new_pos = state[1:] + dt * local_vel
theta = theta + dt * angular_vel
theta = Localization.regularize_angle(theta)
return gs.concatenate((theta, new_pos), axis=0)
def test_fit(self):
"""Test the fit method."""
X_train_a = gs.array([[[EULER ** 2, 0], [0, 1]], [[1, 0], [0, 1]]])
X_train_b = gs.array([[[EULER ** 8, 0], [0, 1]], [[1, 0], [0, 1]]])
X_train = gs.concatenate([X_train_a, X_train_b])
y_train = gs.array([0, 0, 1, 1])
for metric in METRICS:
MDMEstimator = RiemannianMinimumDistanceToMeanClassifier(
metric(n=2), n_classes=2, point_type="matrix"
)
MDMEstimator.fit(X_train, y_train)
bary_a_fit = MDMEstimator.mean_estimates_[0]
bary_b_fit = MDMEstimator.mean_estimates_[1]
if metric in [SPDMetricAffine, SPDMetricLogEuclidean]:
bary_a_expected = gs.array([[EULER, 0], [0, 1]])
bary_b_expected = gs.array([[EULER ** 4, 0], [0, 1]])
elif metric in [SPDMetricEuclidean]:
bary_a_expected = gs.array([[.5 * EULER ** 2 + .5, 0], [0, 1]])
bary_b_expected = gs.array([[.5 * EULER ** 8 + .5, 0], [0, 1]])
else:
raise ValueError("Invalid metric: {}".format(metric))
self.assertAllClose(bary_a_fit, bary_a_expected)
self.assertAllClose(bary_b_fit, bary_b_expected)
def test_Localization_propagate(self):
initial_state = gs.array([0.5, 1., 2.])
time_step = gs.array([0.5])
linear_vel = gs.array([1., 0.5])
angular_vel = gs.array([0.])
increment = gs.concatenate((time_step, linear_vel, angular_vel),
axis=0)
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
next_position = initial_state[1:] + time_step * gs.matmul(
rotation, linear_vel)
expected = gs.concatenate((gs.array([angle]), next_position), axis=0)
result = self.nonlinear_model.propagate(initial_state, increment)
self.assertAllClose(expected, result)
def random_point(self, n_samples=1, bound=1.0, n_iter=100):
r"""Sample in `math:`R_*^{m\times n}` from a normal distribution.
Parameters
----------
n_samples : int
Number of samples.
Optional, default: 1.
bound: float
Bound of the interval in which to sample each matrix entry.
Optional, default: 1.
n_iter : int
Maximum number of trials to sample a matrix with full rank
Optional, default: 100.
Returns
-------
samples : array-like, shape=[..., m, n]
Point sampled on `math:`R_*^{m\times n}`
"""
m = self.ambient_space.shape[0]
n = self.ambient_space.shape[1]
sample = []
n_accepted, iteration = 0, 0
while n_accepted < n_samples and iteration < n_iter:
raw_samples = gs.random.normal(size=(n_samples - n_accepted, m, n))
ranks = gs.linalg.matrix_rank(raw_samples)
selected = ranks == self.rank
sample.append(raw_samples[selected])
n_accepted += gs.sum(selected)
iteration += 1
if n_samples == 1:
return sample[0][0]
return gs.concatenate(sample)
def main():
"""Plot an Agglomerative Hierarchical Clustering on the sphere."""
sphere = Hypersphere(dim=2)
sphere_distance = sphere.metric.dist
n_clusters = 2
n_samples_per_dataset = 50
dataset_1 = sphere.random_von_mises_fisher(kappa=10,
n_samples=n_samples_per_dataset)
dataset_2 = -sphere.random_von_mises_fisher(
kappa=10, n_samples=n_samples_per_dataset)
dataset = gs.concatenate((dataset_1, dataset_2), axis=0)
clustering = AgglomerativeHierarchicalClustering(n_clusters=n_clusters,
distance=sphere_distance)
clustering.fit(dataset)
clustering_labels = clustering.labels_
plt.figure(0)
ax = plt.subplot(111, projection="3d")
plt.title("Agglomerative Hierarchical Clustering")
sphere_plot = visualization.Sphere()
sphere_plot.draw(ax=ax)
for i_label in range(n_clusters):
points_label_i = dataset[clustering_labels == i_label, ...]
sphere_plot.draw_points(ax=ax, points=points_label_i)
plt.show()
def flip_determinant(matrix, det):
"""Change sign of the determinant if it is negative.
For a batch of matrices, multiply the matrices which have negative
determinant by a diagonal matrix :math: `diag(1,...,1,-1) from the right.
This changes the sign of the last column of the matrix.
Parameters
----------
matrix : array-like, shape=[...,n ,m]
Matrix to transform.
det : array-like, shape=[...]
Determinant of matrix, or any other scalar to use as threshold to
determine whether to change the sign of the last column of matrix.
Returns
-------
matrix_flipped : array-like, shape=[..., n, m]
Matrix with the sign of last column changed if det < 0.
"""
if gs.any(det < 0):
ones = gs.ones(matrix.shape[-1])
reflection_vec = gs.concatenate([ones[:-1], gs.array([-1.0])], axis=0)
mask = gs.cast(det < 0, matrix.dtype)
sign = mask[..., None] * reflection_vec + (1.0 - mask)[...,
None] * ones
return gs.einsum("...ij,...j->...ij", matrix, sign)
return matrix
def intrinsic_to_extrinsic_coords(self, point_intrinsic):
"""Convert point from intrinsic to extrinsic coordinates.
Convert from the intrinsic coordinates in the hypersphere,
to the extrinsic coordinates in Euclidean space.
Parameters
----------
point_intrinsic : array-like, shape=[..., dim]
Point on the hypersphere, in intrinsic coordinates.
Returns
-------
point_extrinsic : array-like, shape=[..., dim + 1]
Point on the hypersphere, in extrinsic coordinates in
Euclidean space.
"""
sq_coord_0 = 1. - gs.linalg.norm(point_intrinsic, axis=-1)**2
if gs.any(gs.less(sq_coord_0, 0.)):
raise ValueError('Square-root of a negative number.')
coord_0 = gs.sqrt(sq_coord_0)
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 rotate_points(points, end_point):
"""Apply to points the rotation from north_pole to end_point.
A QR decomposition is used to find the rotation that maps the north pole
(1, 0,...,0) to the end_point, then this rotation is applied to the
input points.
Parameters
----------
points : array-like, shape=[..., n]
Points to rotate.
end_point : array-like, shape=[n, ]
Point to parametrise the rotation.
Returns
-------
rotated_points : array-like, shape=[..., n]
Points after the rotation.
"""
n = end_point.shape[0]
base_point = gs.array([1.0] + [0] * (n - 1))
embedded = gs.concatenate([end_point[None, :], gs.zeros((n - 1, n))])
norm = gs.linalg.norm(end_point)
q, _ = gs.linalg.qr(gs.transpose(embedded) / norm)
new_points = gs.matmul(points[None, :], gs.transpose(q)) * norm
if not gs.allclose(gs.matmul(q, base_point[:, None])[:, 0], end_point):
new_points = -new_points
return new_points[0]
def expectation_maximisation_poincare_ball():
"""Apply EM algorithm on three random data clusters."""
dim = 2
n_samples = 5
cluster_1 = gs.random.uniform(low=0.2, high=0.6, size=(n_samples, dim))
cluster_2 = gs.random.uniform(low=-0.6, high=-0.2, size=(n_samples, dim))
cluster_3 = gs.random.uniform(low=-0.3, high=0, size=(n_samples, dim))
cluster_3[:, 0] = -cluster_3[:, 0]
data = gs.concatenate((cluster_1, cluster_2, cluster_3), axis=0)
n_clusters = 3
manifold = PoincareBall(dim=2)
metric = manifold.metric
EM = RiemannianEM(n_gaussians=n_clusters,
metric=metric,
initialisation_method='random',
mean_method='frechet-poincare-ball')
means, variances, mixture_coefficients = EM.fit(data=data, max_iter=100)
# Plot result
plot = plot_gaussian_mixture_distribution(data,
mixture_coefficients,
means,
variances,
plot_precision=100,
save_path='result.png',
metric=metric)
return plot
def inverse(self, point):
r"""Compute the group inverse in SE(n).
Parameters
----------
point: array-like, shape=[..., 3]
Returns
-------
inverse_point : array-like, shape=[..., 3]
The inverted point.
Notes
-----
:math:`(R, t)^{-1} = (R^{-1}, R^{-1}.(-t))`
"""
rotations = self.rotations
dim_rotations = rotations.dim
point = self.regularize(point)
rot_vec = point[:, :dim_rotations]
translation = point[:, dim_rotations:]
inverse_rotation = -rot_vec
inv_rot_mat = rotations.matrix_from_rotation_vector(inverse_rotation)
inverse_translation = gs.einsum(
'ni,nij->nj', -translation,
gs.transpose(inv_rot_mat, axes=(0, 2, 1)))
inverse_point = gs.concatenate([inverse_rotation, inverse_translation],
axis=-1)
return self.regularize(inverse_point)
def exp_domain(tangent_vec, base_point):
"""Compute the domain of the Euclidean exponential map.
Compute the real interval of time where the Euclidean geodesic starting
at point `base_point` in direction `tangent_vec` is defined.
Parameters
----------
tangent_vec : array-like, shape=[..., n, n]
Tangent vector at base point.
base_point : array-like, shape=[..., n, n]
Base point.
Returns
-------
exp_domain : array-like, shape=[..., 2]
Interval of time where the geodesic is defined.
"""
invsqrt_base_point = SymmetricMatrices.powerm(base_point, -0.5)
reduced_vec = gs.matmul(invsqrt_base_point, tangent_vec)
reduced_vec = gs.matmul(reduced_vec, invsqrt_base_point)
eigvals = gs.linalg.eigvalsh(reduced_vec)
min_eig = gs.amin(eigvals, axis=1)
max_eig = gs.amax(eigvals, axis=1)
inf_value = gs.where(max_eig <= 0.0, gs.array(-math.inf),
-1.0 / max_eig)
inf_value = gs.to_ndarray(inf_value, to_ndim=2)
sup_value = gs.where(min_eig >= 0.0, gs.array(-math.inf),
-1.0 / min_eig)
sup_value = gs.to_ndarray(sup_value, to_ndim=2)
domain = gs.concatenate((inf_value, sup_value), axis=1)
return domain
def ball_to_half_space_tangent(tangent_vec, base_point):
"""Convert ball to half-space tangent coordinates.
Convert the parameterization of a tangent vector to the
hyperbolic space from its Poincare ball coordinates, to
the Poinare half-space coordinates.
Parameters
----------
tangent_vec : array-like, shape=[..., dim]
Tangent vector at the base point in the Poincare ball.
base_point : array-like, shape=[..., dim]
Point in the Poincare ball.
Returns
-------
tangent_vec_half_spacel : array-like, shape=[..., dim]
Tangent vector in the Poincare half-space.
"""
sq_norm = gs.sum(base_point ** 2, axis=-1)
den = 1 + sq_norm - 2 * base_point[..., -1]
scalar_prod = gs.sum(base_point * tangent_vec, -1)
component_1 = (
gs.einsum('...i,...->...i', tangent_vec[..., :-1], 2. / den)
- 4. * gs.einsum(
'...i,...->...i', base_point[..., :-1],
(scalar_prod - tangent_vec[..., -1]) / den**2))
component_2 = -2. * scalar_prod / den \
- 2 * (1. - sq_norm) * (scalar_prod - tangent_vec[..., -1]) \
/ den**2
tangent_vec_half_space = gs.concatenate(
[component_1, component_2[..., None]], axis=-1)
return tangent_vec_half_space
def ball_to_half_space_coordinates(point):
"""Convert ball to half space coordinates.
Convert the parameterization of a point in the hyperbolic space
from its Poincare ball coordinates, to the Poincare half-space
coordinates.
Parameters
----------
point : array-like, shape=[..., dim]
Point in the hyperbolic space in Poincare ball coordinates.
Returns
-------
point_ball : array-like, shape=[..., dim]
Point in the hyperbolic space in half-space coordinates.
"""
sq_norm = gs.sum(point ** 2, axis=-1)
den = 1 + sq_norm - 2 * point[..., -1]
component_1 = gs.einsum(
'...i,...->...i', point[..., :-1], 2. / den)
component_2 = (1 - sq_norm) / den
point_half_space = gs.concatenate(
[component_1, component_2[..., None]], axis=-1)
return point_half_space
def main():
y_pred = gs.array([1., 1.5, -0.3, 5., 6., 7.])
y_true = gs.array([0.1, 1.8, -0.1, 4., 5., 6.])
loss_rot_vec = loss(y_pred, y_true)
grad_rot_vec = grad(y_pred, y_true)
if os.environ['GEOMSTATS_BACKEND'] == 'tensorflow':
with tf.Session() as sess:
loss_rot_vec = sess.run(loss_rot_vec)
grad_rot_vec = sess.run(grad_rot_vec)
print('The loss between the poses using rotation vectors is: {}'.format(
loss_rot_vec[0, 0]))
print('The riemannian gradient is: {}'.format(grad_rot_vec))
angle = gs.pi / 6
cos = gs.cos(angle / 2)
sin = gs.sin(angle / 2)
u = gs.array([1., 2., 3.])
u = u / gs.linalg.norm(u)
scalar = gs.array(cos)
vec = sin * u
translation = gs.array([5., 6., 7.])
y_pred_quaternion = gs.concatenate([[scalar], vec, translation], axis=0)
angle = gs.pi / 7
cos = gs.cos(angle / 2)
sin = gs.sin(angle / 2)
u = gs.array([1., 2., 3.])
u = u / gs.linalg.norm(u)
scalar = gs.array(cos)
vec = sin * u
translation = gs.array([4., 5., 6.])
y_true_quaternion = gs.concatenate([[scalar], vec, translation], axis=0)
loss_quaternion = loss(y_pred_quaternion,
y_true_quaternion,
representation='quaternion')
grad_quaternion = grad(y_pred_quaternion,
y_true_quaternion,
representation='quaternion')
if os.environ['GEOMSTATS_BACKEND'] == 'tensorflow':
with tf.Session() as sess:
loss_quaternion = sess.run(loss_quaternion)
grad_quaternion = sess.run(grad_quaternion)
print('The loss between the poses using quaternions is: {}'.format(
loss_quaternion[0, 0]))
print('The riemannian gradient is: {}'.format(grad_quaternion))
def grad(y_pred, y_true,
metric=SO3.bi_invariant_metric,
representation='vector'):
"""Closed-form for the gradient of pose_loss.
Parameters
----------
y_pred : array-like
Prediction on SO(3).
y_true : array-like
Ground-truth on SO(3).
metric : RiemannianMetric
Metric used to compute the loss and gradient.
representation : str, {'vector', 'matrix'}
Representation chosen for points in SE(3).
Returns
-------
lie_grad : array-like
Tangent vector at point y_pred.
"""
y_pred = gs.expand_dims(y_pred, axis=0)
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'vector':
lie_grad = lie_group.grad(y_pred, y_true, SO3, metric)
if representation == 'quaternion':
quat_scalar = y_pred[:, :1]
quat_vec = y_pred[:, 1:]
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_scalar = gs.to_ndarray(differential_scalar, to_ndim=2)
differential_scalar = gs.transpose(differential_scalar)
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 ** 2)
+ 2 * quat_arctan2 / quat_vec_norm * gs.eye(3))
differential_vec = gs.squeeze(differential_vec)
differential = gs.concatenate(
[differential_scalar, differential_vec],
axis=1)
y_pred = SO3.rotation_vector_from_quaternion(y_pred)
y_true = SO3.rotation_vector_from_quaternion(y_true)
lie_grad = lie_group.grad(y_pred, y_true, SO3, metric)
lie_grad = gs.matmul(lie_grad, differential)
lie_grad = gs.squeeze(lie_grad, axis=0)
return lie_grad
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 log(self, point, base_point, max_iter=30):
"""Compute the Riemannian logarithm of a point.
Based on [ZR2017]_.
References
----------
.. [ZR2017] Zimmermann, Ralf. "A Matrix-Algebraic Algorithm for the
Riemannian Logarithm on the Stiefel Manifold under the Canonical
Metric" SIAM J. Matrix Anal. & Appl., 38(2), 322–342, 2017.
https://arxiv.org/pdf/1604.05054.pdf
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 Riemannian logarithm
of point at the base point.
"""
p = base_point.shape[-1]
transpose_base_point = Matrices.transpose(base_point)
matrix_m = gs.matmul(transpose_base_point, point)
matrix_q, matrix_n = StiefelCanonicalMetric._normal_component_qr(
point, base_point, matrix_m)
matrix_v = StiefelCanonicalMetric._orthogonal_completion(
matrix_m, matrix_n)
matrix_v = StiefelCanonicalMetric._procrustes_preprocessing(
p, matrix_v, matrix_m, matrix_n)
for _ in range(max_iter):
matrix_lv = gs.linalg.logm(matrix_v)
matrix_c = matrix_lv[:, p:2 * p, p:2 * p]
# TODO(nina): Add break condition
# of the form: if gs.all(gs.less_equal(norm_matrix_c, tol)):
matrix_phi = gs.linalg.expm(-matrix_c)
aux_matrix = gs.matmul(matrix_v[:, :, p:2 * p], matrix_phi)
matrix_v = gs.concatenate([matrix_v[:, :, 0:p], aux_matrix],
axis=2)
matrix_xv = gs.matmul(base_point, matrix_lv[:, 0:p, 0:p])
matrix_qv = gs.matmul(matrix_q, matrix_lv[:, p:2 * p, 0:p])
return matrix_xv + matrix_qv
def create_data(kalman, true_init, true_inputs, obs_freq):
"""Create data for a specific example.
Parameters
----------
kalman : KalmanFilter
Filter which will be used to estimate the state.
true_init : array-like, shape=[dim]
True initial state.
true_inputs : list(array-like, shape=[dim_input])
Noise-free inputs giving the evolution of the true state.
obs_freq : int
Number of time steps between observations.
Returns
-------
true_traj : array-like, shape=[len(true_inputs), dim]
Trajectory of the true state.
inputs : list(array-like, shape=[dim_input])
Simulated noisy inputs received by the sensor.
observations : array-like, shape=[len(true_inputs)/obs_freq, dim_obs]
Simulated noisy observations of the system.
"""
true_traj = [1 * true_init]
for incr in true_inputs:
true_traj.append(kalman.model.propagate(true_traj[-1], incr))
true_obs = [
kalman.model.observation_model(pose) for pose in true_traj[obs_freq::obs_freq]
]
obs_dtype = true_obs[0].dtype
observations = gs.stack(
[
gs.array(
np.random.multivariate_normal(obs, kalman.measurement_noise),
dtype=obs_dtype,
)
for obs in true_obs
]
)
input_dtype = true_inputs[0].dtype
inputs = [
gs.concatenate(
[
incr[:1],
gs.array(
np.random.multivariate_normal(incr[1:], kalman.process_noise),
dtype=input_dtype,
),
],
axis=0,
)
for incr in true_inputs
]
inputs = [gs.cast(incr, input_dtype) for incr in inputs]
return gs.array(true_traj), inputs, observations
def main():
"""Compute pole ladder and plot the construction."""
base_point = SPACE.random_uniform(1)
tangent_vec_b = SPACE.random_uniform(1)
tangent_vec_b = SPACE.projection_to_tangent_space(tangent_vec_b,
base_point)
tangent_vec_b *= N_STEPS / 2
tangent_vec_a = SPACE.random_uniform(1)
tangent_vec_a = SPACE.projection_to_tangent_space(tangent_vec_a,
base_point) * N_STEPS / 4
ladder = METRIC.ladder_parallel_transport(tangent_vec_a,
tangent_vec_b,
base_point,
n_steps=N_STEPS,
return_geodesics=True)
pole_ladder = ladder['transported_tangent_vec']
trajectory = ladder['trajectory']
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111, projection='3d')
sphere_visu = visualization.Sphere(n_meridians=30)
ax = sphere_visu.set_ax(ax=ax)
t = gs.linspace(0, 1, N_POINTS)
t_main = gs.linspace(0, 1, N_POINTS * 4)
for points in trajectory:
main_geodesic, diagonal, final_geodesic = points
sphere_visu.draw_points(ax,
main_geodesic(t_main),
marker='o',
c='b',
s=2)
sphere_visu.draw_points(ax, diagonal(-t), marker='o', c='r', s=2)
sphere_visu.draw_points(ax, diagonal(t), marker='o', c='r', s=2)
sphere_visu.draw_points(ax, final_geodesic(-t), marker='o', c='g', s=2)
sphere_visu.draw_points(ax, final_geodesic(t), marker='o', c='g', s=2)
tangent_vectors = gs.stack([tangent_vec_b, tangent_vec_a, pole_ladder],
axis=0) / N_STEPS
base_point = gs.to_ndarray(base_point, to_ndim=2)
origin = gs.concatenate(
[base_point, base_point,
final_geodesic(gs.array([0]))])
ax.quiver(origin[:, 0],
origin[:, 1],
origin[:, 2],
tangent_vectors[:, 0],
tangent_vectors[:, 1],
tangent_vectors[:, 2],
color=['black', 'black', 'black'],
linewidth=2)
sphere_visu.draw(ax, linewidth=1)
plt.show()
def normalization_factor(self, variances):
"""Return normalization factor.
Parameters
----------
variances : array-like, shape=[n,]
Array of equally distant values of the
variance precision time.
Returns
-------
norm_func : array-like, shape=[n,]
Normalisation factor for all given variances.
"""
binomial_coefficient = None
n_samples = variances.shape[0]
expand_variances = gs.expand_dims(variances, axis=0)
expand_variances = gs.repeat(expand_variances, self.dim, axis=0)
if binomial_coefficient is None:
dim_range = gs.arange(self.dim)
dim_range[0] = 1
n_fact = dim_range.prod()
k_fact = gs.concatenate(
[gs.expand_dims(dim_range[:i].prod(), 0)
for i in range(1, dim_range.shape[0] + 1)], 0)
nmk_fact = gs.flip(k_fact, 0)
binomial_coefficient = n_fact / (k_fact * nmk_fact)
binomial_coefficient = gs.expand_dims(binomial_coefficient, -1)
binomial_coefficient = gs.repeat(binomial_coefficient,
n_samples, axis=1)
range_ = gs.expand_dims(gs.arange(self.dim), -1)
range_ = gs.repeat(range_, n_samples, axis=1)
ones_ = gs.expand_dims(gs.ones(self.dim), -1)
ones_ = gs.repeat(ones_, n_samples, axis=1)
alternate_neg = (-ones_) ** (range_)
erf_arg = (((self.dim - 1) - 2 * range_) *
expand_variances) / gs.sqrt(2)
exp_arg = ((((self.dim - 1) - 2 * range_) *
expand_variances) / gs.sqrt(2)) ** 2
norm_func_1 = (1 + gs.erf(erf_arg)) * gs.exp(exp_arg)
norm_func_2 = binomial_coefficient * norm_func_1
norm_func_3 = alternate_neg * norm_func_2
norm_func = NORMALIZATION_FACTOR_CST * variances * \
norm_func_3.sum(0) * (1 / (2 ** (self.dim - 1)))
return norm_func
def compose(self, point_a, point_b, point_type=None):
r"""Compose two elements of SE(n).
Parameters
----------
point_1 : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}]
point_2 : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}]
point_type: str, {'vector', 'matrix'}, optional
default: self.default_point_type
Equation
---------
(:math: `(R_1, t_1) \\cdot (R_2, t_2) = (R_1 R_2, R_1 t_2 + t_1)`)
Returns
-------
composition : the composition of point_1 and point_2
"""
rotations = self.rotations
dim_rotations = rotations.dim
point_a = self.regularize(point_a, point_type=point_type)
point_b = self.regularize(point_b, point_type=point_type)
if point_type == 'vector':
n_points_a, _ = point_a.shape
n_points_b, _ = point_b.shape
if not (point_a.shape == point_b.shape or n_points_a == 1
or n_points_b == 1):
raise ValueError()
rot_vec_a = point_a[:, :dim_rotations]
rot_mat_a = rotations.matrix_from_rotation_vector(rot_vec_a)
rot_vec_b = point_b[:, :dim_rotations]
rot_mat_b = rotations.matrix_from_rotation_vector(rot_vec_b)
translation_a = point_a[:, dim_rotations:]
translation_b = point_b[:, dim_rotations:]
composition_rot_mat = gs.matmul(rot_mat_a, rot_mat_b)
composition_rot_vec = rotations.rotation_vector_from_matrix(
composition_rot_mat)
composition_translation = gs.einsum(
'...j,...kj->...k', translation_b, rot_mat_a) + translation_a
composition = gs.concatenate(
(composition_rot_vec, composition_translation), axis=-1)
return self.regularize(composition, point_type=point_type)
if point_type == 'matrix':
return GeneralLinear.compose(point_a, point_b)
raise ValueError('Invalid point_type, expected \'vector\' or '
'\'matrix\'.')
