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])