def retraction(tangent_vec, base_point):
"""Compute the retraction of a tangent vector.
This computation is based on the QR-decomposition.
e.g. :math:`P_x(V) = qf(X + V)`.
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 retraction
of tangent_vec at the base point.
"""
n_tangent_vecs, _, _ = tangent_vec.shape
n_base_points, _, _ = base_point.shape
if not (n_tangent_vecs == n_base_points or n_tangent_vecs == 1
or n_base_points == 1):
raise NotImplementedError
matrix_q, matrix_r = gs.linalg.qr(base_point + tangent_vec)
diagonal = gs.diagonal(matrix_r, axis1=1, axis2=2)
sign = gs.sign(gs.sign(diagonal) + 0.5)
diag = algebra_utils.from_vector_to_diagonal_matrix(sign)
result = gs.einsum('nij,njk->nik', matrix_q, diag)
return result
def retraction(tangent_vec, base_point):
"""Compute the retraction of a tangent vector.
This computation is based on the QR-decomposition.
e.g. :math:`P_x(V) = qf(X + V)`.
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 retraction
of tangent_vec at the base point.
"""
matrix_q, matrix_r = gs.linalg.qr(base_point + tangent_vec)
diagonal = gs.diagonal(matrix_r, axis1=-2, axis2=-1)
sign = gs.sign(gs.sign(diagonal) + 0.5)
diag = algebra_utils.from_vector_to_diagonal_matrix(sign)
result = Matrices.mul(matrix_q, diag)
return result
def retraction(self, tangent_vec, base_point):
"""
Retraction map, based on QR-decomposion:
P_x(V) = qf(X + V)
"""
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=3)
n_tangent_vecs, _, _ = tangent_vec.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, n, p = base_point.shape
assert (n_tangent_vecs == n_base_points
or n_tangent_vecs == 1
or n_base_points == 1)
if n_base_points == 1:
base_point = gs.tile(base_point, (n_tangent_vecs, 1, 1))
if n_tangent_vecs == 1:
tangent_vec = gs.tile(tangent_vec, (n_base_points, 1, 1))
matrix_q, matrix_r = gs.linalg.qr(base_point+tangent_vec)
diagonal = gs.diagonal(matrix_r, axis1=1, axis2=2)
sign = gs.sign(gs.sign(diagonal) + 0.5)
diag = gs.diag(sign)
result = gs.einsum('nij,njk->nik', matrix_q, diag)
return result
def reshape_metric_matrix(self):
"""Reshape diagonal metric matrix to a symmetric matrix of size n.
Reshape a diagonal metric matrix of size `dim x dim` into a symmetric
matrix of size `n x n` where :math: `dim= n (n -1) / 2` is the
dimension of the space of skew symmetric matrices. The
non-diagonal coefficients in the output matrix correspond to the
basis matrices of this space. The diagonal is filled with ones.
This useful to compute a matrix inner product.
Parameters
----------
metric_matrix : array-like, shape=[dim, dim]
Diagonal metric matrix.
Returns
-------
symmetric_matrix : array-like, shape=[n, n]
Symmetric matrix.
"""
if Matrices.is_diagonal(self.metric_mat_at_identity):
metric_coeffs = gs.diagonal(self.metric_mat_at_identity)
metric_mat = gs.abs(
self.lie_algebra.matrix_representation(metric_coeffs))
return metric_mat
raise ValueError('This is only possible for a diagonal matrix')
def jacobian_diffeomorphism(self, base_point):
r"""Jacobian of the diffeomorphism at base point.
Let :math:`f` be the diffeomorphism
:math:`f: M \rightarrow N` of the manifold
:math:`M` into the manifold `N`.
Parameters
----------
base_point : array-like, shape=[..., *shape]
Base point.
Returns
-------
mat : array-like, shape=[..., *i_shape, *shape]
Inner-product matrix.
"""
rad = base_point.shape[:-len(self.shape)]
base_point = gs.reshape(base_point, (-1, ) + self.shape)
J = self.raw_jacobian_diffeomorphism(base_point)
J = gs.moveaxis(
gs.diagonal(J, axis1=0,
axis2=len(self.embedding_metric.shape) + 1),
-1,
0,
)
J = gs.reshape(J, rad + self.embedding_metric.shape + self.shape)
return J
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 diagonal(mat):
"""Return the diagonal of a matrix as a vector.
Parameters
----------
mat : array-like, shape=[..., m, n]
Matrix.
Returns
-------
diagonal : array-like, shape=[..., min(m, n)]
Vector of diagonal coefficients.
"""
return gs.diagonal(mat, axis1=-2, axis2=-1)
def __sample_spd(self, samples):
n = self.mean.shape[-1]
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_euclidean = gs.random.multivariate_normal(
mean_euclidean, self.cov, (samples, ))
diag = samples_euclidean[:, :n]
off_diag = samples_euclidean[:, n:] / gs.sqrt(2.0)
samples_sym = gs.mat_from_diag_triu_tril(diag=diag,
tri_upp=off_diag,
tri_low=off_diag)
samples_spd = self.manifold.expm(samples_sym)
return samples_spd
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 retraction(self, tangent_vec, base_point):
"""Compute the retraction of a tangent vector.
This computation is based on the QR-decomposition.
e.g. :math:`P_x(V) = qf(X + V)`.
Parameters
----------
tangent_vec : array-like, shape=[n_samples, n, p]
Tangent vector at a base point.
base_point : array-like, shape=[n_samples, n, p]
Point in the Stiefel manifold.
Returns
-------
exp : array-like, shape=[n_samples, n, p]
Point in the Stiefel manifold equal to the retraction
of tangent_vec at the base point.
"""
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=3)
n_tangent_vecs, _, _ = tangent_vec.shape
base_point = gs.to_ndarray(base_point, to_ndim=3)
n_base_points, n, p = base_point.shape
assert (n_tangent_vecs == n_base_points or n_tangent_vecs == 1
or n_base_points == 1)
if n_base_points == 1:
base_point = gs.tile(base_point, (n_tangent_vecs, 1, 1))
if n_tangent_vecs == 1:
tangent_vec = gs.tile(tangent_vec, (n_base_points, 1, 1))
matrix_q, matrix_r = gs.linalg.qr(base_point + tangent_vec)
diagonal = gs.diagonal(matrix_r, axis1=1, axis2=2)
sign = gs.sign(gs.sign(diagonal) + 0.5)
diag = gs.diag(sign)
result = gs.einsum('nij,njk->nik', matrix_q, diag)
return result
def inverse_jacobian_diffeomorphism(self, image_point):
r"""Inverse Jacobian of the diffeomorphism at image point.
Parameters
----------
image_point : array-like, shape=[..., *i_shape]
Base point.
Returns
-------
mat : array-like, shape=[..., *shape, *i_shape]
Inner-product matrix.
"""
rad = image_point.shape[:-len(self.embedding_metric.shape)]
image_point = gs.reshape(image_point,
(-1, ) + self.embedding_metric.shape)
J = self.raw_inverse_jacobian_diffeomorphism(image_point)
J = gs.moveaxis(gs.diagonal(J, axis1=0, axis2=len(self.shape) + 1), -1,
0)
J = gs.reshape(J, rad + self.shape + self.embedding_metric.shape)
return J
def is_diagonal(cls, mat, atol=gs.atol):
"""Check if a matrix is square and diagonal.
Parameters
----------
mat : array-like, shape=[..., n, n]
Matrix.
atol : float
Absolute tolerance.
Optional, default: backend atol.
Returns
-------
is_diagonal : array-like, shape=[...,]
Boolean evaluating if the matrix is square and diagonal.
"""
is_square = cls.is_square(mat)
if not gs.all(is_square):
return False
diagonal_mat = from_vector_to_diagonal_matrix(
gs.diagonal(mat, axis1=-2, axis2=-1))
is_diagonal = gs.all(gs.isclose(mat, diagonal_mat, atol=atol),
axis=(-2, -1))
return is_diagonal
def rotation_vector_from_matrix(self, rot_mat):
"""
In 3D, convert rotation matrix to rotation vector
(axis-angle representation).
Get the angle through the trace of the rotation matrix:
The eigenvalues are:
1, cos(angle) + i sin(angle), cos(angle) - i sin(angle)
so that: trace = 1 + 2 cos(angle), -1 <= trace <= 3
Get the rotation vector through the formula:
S_r = angle / ( 2 * sin(angle) ) (R - R^T)
For the edge case where the angle is close to pi,
the formulation is derived by going from rotation matrix to unit
quaternion to axis-angle:
r = angle * v / |v|, where (w, v) is a unit quaternion.
In nD, the rotation vector stores the n(n-1)/2 values of the
skew-symmetric matrix representing the rotation.
"""
rot_mat = gs.to_ndarray(rot_mat, to_ndim=3)
n_rot_mats, mat_dim_1, mat_dim_2 = rot_mat.shape
assert mat_dim_1 == mat_dim_2 == self.n
rot_mat = closest_rotation_matrix(rot_mat)
if self.n == 3:
trace = gs.trace(rot_mat, axis1=1, axis2=2)
trace = gs.to_ndarray(trace, to_ndim=2, axis=1)
assert trace.shape == (n_rot_mats, 1), trace.shape
cos_angle = .5 * (trace - 1)
cos_angle = gs.clip(cos_angle, -1, 1)
angle = gs.arccos(cos_angle)
rot_mat_transpose = gs.transpose(rot_mat, axes=(0, 2, 1))
rot_vec = vector_from_skew_matrix(rot_mat - rot_mat_transpose)
mask_0 = gs.isclose(angle, 0)
mask_0 = gs.squeeze(mask_0, axis=1)
rot_vec[mask_0] = (rot_vec[mask_0] * (.5 -
(trace[mask_0] - 3.) / 12.))
mask_pi = gs.isclose(angle, gs.pi)
mask_pi = gs.squeeze(mask_pi, axis=1)
# choose the largest diagonal element
# to avoid a square root of a negative number
a = 0
if gs.any(mask_pi):
a = gs.argmax(gs.diagonal(rot_mat[mask_pi], axis1=1, axis2=2))
b = gs.mod(a + 1, 3)
c = gs.mod(a + 2, 3)
# compute the axis vector
sq_root = gs.sqrt(
(rot_mat[mask_pi, a, a] - rot_mat[mask_pi, b, b] -
rot_mat[mask_pi, c, c] + 1.))
rot_vec_pi = gs.zeros((sum(mask_pi), self.dimension))
rot_vec_pi[:, a] = sq_root / 2.
rot_vec_pi[:, b] = (
(rot_mat[mask_pi, b, a] + rot_mat[mask_pi, a, b]) /
(2. * sq_root))
rot_vec_pi[:, c] = (
(rot_mat[mask_pi, c, a] + rot_mat[mask_pi, a, c]) /
(2. * sq_root))
rot_vec[mask_pi] = (angle[mask_pi] * rot_vec_pi /
gs.linalg.norm(rot_vec_pi))
mask_else = ~mask_0 & ~mask_pi
rot_vec[mask_else] = (angle[mask_else] /
(2. * gs.sin(angle[mask_else])) *
rot_vec[mask_else])
else:
skew_mat = self.embedding_manifold.group_log_from_identity(rot_mat)
rot_vec = vector_from_skew_matrix(skew_mat)
return self.regularize(rot_vec)
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 closest(rot):
d_coefs = gs.diagonal(rot)
d_sign = gs.where(d_coefs >= 0, 1., -1.)
return mul(rot, gs.diag(d_sign)[0])
