def compose(self, point_1, point_2, point_type=None): """ Compose two elements of SE(n). Formula: point_1 . point_2 = [R1 * R2, (R1 * t2) + t1] where: R1, R2 are rotation matrices, t1, t2 are translation vectors. """ if point_type is None: point_type = self.default_point_type rotations = self.rotations dim_rotations = rotations.dimension point_1 = self.regularize(point_1, point_type=point_type) point_2 = self.regularize(point_2, point_type=point_type) if point_type == 'vector': n_points_1, _ = point_1.shape n_points_2, _ = point_2.shape assert (point_1.shape == point_2.shape or n_points_1 == 1 or n_points_2 == 1) rot_vec_1 = point_1[:, :dim_rotations] rot_mat_1 = rotations.matrix_from_rotation_vector(rot_vec_1) rot_mat_1 = rotations.projection(rot_mat_1) rot_vec_2 = point_2[:, :dim_rotations] rot_mat_2 = rotations.matrix_from_rotation_vector(rot_vec_2) rot_mat_2 = rotations.projection(rot_mat_2) translation_1 = point_1[:, dim_rotations:] translation_2 = point_2[:, dim_rotations:] n_compositions = gs.maximum(n_points_1, n_points_2) composition_rot_mat = gs.matmul(rot_mat_1, rot_mat_2) composition_rot_vec = rotations.rotation_vector_from_matrix( composition_rot_mat) composition_translation = gs.zeros((n_compositions, self.n)) for i in range(n_compositions): translation_1_i = (translation_1[0] if n_points_1 == 1 else translation_1[i]) rot_mat_1_i = (rot_mat_1[0] if n_points_1 == 1 else rot_mat_1[i]) translation_2_i = (translation_2[0] if n_points_2 == 1 else translation_2[i]) composition_translation[i] = (gs.dot(translation_2_i, gs.transpose(rot_mat_1_i)) + translation_1_i) composition = gs.zeros((n_compositions, self.dimension)) composition[:, :dim_rotations] = composition_rot_vec composition[:, dim_rotations:] = composition_translation elif point_type == 'matrix': raise NotImplementedError() composition = self.regularize(composition, point_type=point_type) return composition
def lifting(point, base_point): """Compute the lifting of a point. This computation is based on the QR-decomposion. e.g. :math:`P_x^{-1}(Q) = QR - X`. 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 lifting of point at the base point. """ n_points, _, _ = point.shape n_base_points, _, n = base_point.shape if not (n_points == n_base_points or n_points == 1 or n_base_points == 1): raise NotImplementedError n_liftings = gs.maximum(n_base_points, n_points) def _make_minor(i, matrix): return matrix[:i + 1, :i + 1] def _make_column_r(i, matrix): if i == 0: if matrix[0, 0] <= 0: raise ValueError('M[0,0] <= 0') return gs.array([1. / matrix[0, 0]]) matrix_m_i = _make_minor(i, matrix_m_k) inv_matrix_m_i = gs.linalg.inv(matrix_m_i) b_i = _make_b(i, matrix_m_k, columns_list) column_r_i = gs.matmul(inv_matrix_m_i, b_i) if column_r_i[i] <= 0: raise ValueError('(r_i)_i <= 0') return column_r_i def _make_b(i, matrix, list_matrices_r): b = gs.ones(i + 1) for j in range(i): b[j] = - gs.matmul( matrix[i, :j + 1], list_matrices_r[j]) return b matrix_r = gs.zeros((n_liftings, n, n)) matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point) for k in range(n_liftings): columns_list = [] matrix_m_k = matrix_m[k] for i in range(n): column_r_i = _make_column_r(i, matrix_m_k) columns_list.append(column_r_i) matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i) return gs.matmul(point, matrix_r) - base_point
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 exp(self, tangent_vec, base_point): """Compute the Riemannian exponential of a tangent vector. 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 Riemannian exponential of tangent_vec at the base point. """ n_tangent_vecs, _, _ = tangent_vec.shape n_base_points, _, p = base_point.shape if not (n_tangent_vecs == n_base_points or n_tangent_vecs == 1 or n_base_points == 1): raise NotImplementedError if n_tangent_vecs == 1: tangent_vec = gs.tile(tangent_vec, (n_base_points, 1, 1)) if n_base_points == 1: base_point = gs.tile(base_point, (n_tangent_vecs, 1, 1)) matrix_a = gs.einsum( 'nij, njk->nik', gs.transpose(base_point, axes=(0, 2, 1)), tangent_vec) matrix_k = (tangent_vec - gs.einsum('nij,njk->nik', base_point, matrix_a)) matrix_q, matrix_r = gs.linalg.qr(matrix_k) matrix_ar = gs.concatenate( [matrix_a, -gs.transpose(matrix_r, axes=(0, 2, 1))], axis=2) zeros = gs.zeros( (gs.maximum(n_base_points, n_tangent_vecs), p, p)) matrix_rz = gs.concatenate( [matrix_r, zeros], axis=2) block = gs.concatenate([matrix_ar, matrix_rz], axis=1) matrix_mn_e = gs.linalg.expm(block) exp = gs.einsum( 'nij,njk->nik', gs.concatenate( [base_point, matrix_q], axis=2), matrix_mn_e[:, :, 0:p]) return exp
def lifting(self, point, base_point): """ Lifting map, based on QR-decomposion: P_x^{-1}(Q) = QR - X """ point = gs.to_ndarray(point, to_ndim=3) n_points, _, _ = point.shape base_point = gs.to_ndarray(base_point, to_ndim=3) n_base_points, p, n = base_point.shape assert (n_points == n_base_points or n_points == 1 or n_base_points == 1) if n_base_points == 1: base_point = gs.tile(base_point, (n_points, 1, 1)) if n_points == 1: point = gs.tile(point, (n_base_points, 1, 1)) n_liftings = gs.maximum(n_base_points, n_points) def make_minor(i, matrix): return matrix[:i + 1, :i + 1] def make_column_r(i, matrix): if i == 0: assert matrix[0, 0] > 0, 'M[0,0] <= 0' return gs.array([1. / matrix[0, 0]]) else: # get principal minor matrix_m_i = make_minor(i, matrix_m_k) assert gs.linalg.det(matrix_m_i) != 0 inv_matrix_m_i = gs.linalg.inv(matrix_m_i) b_i = make_b(i, matrix_m_k, columns_list) column_r_i = gs.matmul(inv_matrix_m_i, b_i) assert column_r_i[i] > 0, '(r_i)_i <= 0' return column_r_i def make_b(i, matrix, list_matrices_r): b = gs.ones(i + 1) for j in range(i): b[j] = -gs.matmul(matrix[i, :j + 1], list_matrices_r[j]) return b matrix_r = gs.zeros((n_liftings, n, n)) matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point) for k in range(n_liftings): columns_list = [] matrix_m_k = matrix_m[k] for i in range(n): column_r_i = make_column_r(i, matrix_m_k) columns_list.append(column_r_i) matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i) return gs.matmul(point, matrix_r) - base_point
def lifting(self, point, base_point): """Compute the lifting of a point. This computation is based on the QR-decomposion. e.g. :math:`P_x^{-1}(Q) = QR - X`. Parameters ---------- point : array-like, shape=[n_samples, n, p] Point in the Stiefel manifold. base_point : array-like, shape=[n_samples, n, p] Point in the Stiefel manifold. Returns ------- log : array-like, shape=[n_samples, dimension + 1] Tangent vector at the base point equal to the lifting of point at the base point. """ point = gs.to_ndarray(point, to_ndim=3) n_points, _, _ = point.shape base_point = gs.to_ndarray(base_point, to_ndim=3) n_base_points, p, n = base_point.shape assert (n_points == n_base_points or n_points == 1 or n_base_points == 1) if n_base_points == 1: base_point = gs.tile(base_point, (n_points, 1, 1)) if n_points == 1: point = gs.tile(point, (n_base_points, 1, 1)) n_liftings = gs.maximum(n_base_points, n_points) def _make_minor(i, matrix): return matrix[:i + 1, :i + 1] def _make_column_r(i, matrix): if i == 0: assert matrix[0, 0] > 0, 'M[0,0] <= 0' return gs.array([1. / matrix[0, 0]]) else: matrix_m_i = _make_minor(i, matrix_m_k) inv_matrix_m_i = gs.linalg.inv(matrix_m_i) b_i = _make_b(i, matrix_m_k, columns_list) column_r_i = gs.matmul(inv_matrix_m_i, b_i) assert column_r_i[i] > 0, '(r_i)_i <= 0' return column_r_i def _make_b(i, matrix, list_matrices_r): b = gs.ones(i + 1) for j in range(i): b[j] = -gs.matmul(matrix[i, :j + 1], list_matrices_r[j]) return b matrix_r = gs.zeros((n_liftings, n, n)) matrix_m = gs.matmul(gs.transpose(base_point, axes=(0, 2, 1)), point) for k in range(n_liftings): columns_list = [] matrix_m_k = matrix_m[k] for i in range(n): column_r_i = _make_column_r(i, matrix_m_k) columns_list.append(column_r_i) matrix_r[k, :len(column_r_i), i] = gs.array(column_r_i) return gs.matmul(point, matrix_r) - base_point
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point): """ Compute the inner product of tangent_vec_a and tangent_vec_b at point base_point using the power-Euclidean metric. Parameters ---------- tangent_vec_a : array-like, shape=[n_samples, n, n] tangent_vec_b : array-like, shape=[n_samples, n, n] base_point : array-like, shape={n_samples, n, n] Returns ------- inner_product : float """ power_euclidean = self.power_euclidean tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=3) n_tangent_vecs_a, _, _ = tangent_vec_a.shape tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=3) n_tangent_vecs_b, _, _ = tangent_vec_b.shape base_point = gs.to_ndarray(base_point, to_ndim=3) n_base_points, _, _ = base_point.shape spd_space = self.space assert (n_tangent_vecs_a == n_tangent_vecs_b == n_base_points or n_tangent_vecs_a == n_tangent_vecs_b and n_base_points == 1 or n_base_points == n_tangent_vecs_a and n_tangent_vecs_b == 1 or n_base_points == n_tangent_vecs_b and n_tangent_vecs_a == 1 or n_tangent_vecs_a == 1 and n_tangent_vecs_b == 1 or n_base_points == 1 and n_tangent_vecs_a == 1 or n_base_points == 1 and n_tangent_vecs_b == 1) if n_tangent_vecs_a == 1: tangent_vec_a = gs.tile( tangent_vec_a, (gs.maximum(n_base_points, n_tangent_vecs_b), 1, 1)) if n_tangent_vecs_b == 1: tangent_vec_b = gs.tile( tangent_vec_b, (gs.maximum(n_base_points, n_tangent_vecs_a), 1, 1)) if n_base_points == 1: base_point = gs.tile( base_point, (gs.maximum(n_tangent_vecs_a, n_tangent_vecs_b), 1, 1)) if power_euclidean == 1: product = gs.matmul(tangent_vec_a, tangent_vec_b) inner_product = gs.trace(product, axis1=1, axis2=2) else: modified_tangent_vec_a = \ spd_space.differential_power(power_euclidean, tangent_vec_a, base_point) modified_tangent_vec_b = \ spd_space.differential_power(power_euclidean, tangent_vec_b, base_point) product = gs.matmul(modified_tangent_vec_a, modified_tangent_vec_b) inner_product = gs.trace(product, axis1=1, axis2=2) \ / (power_euclidean ** 2) inner_product = gs.to_ndarray(inner_product, to_ndim=2, axis=1) return inner_product
def inner_product(self, tangent_vec_a, tangent_vec_b, base_point): """Compute the affine-invariant inner product. Compute the inner product of tangent_vec_a and tangent_vec_b at point base_point using the affine invariant Riemannian metric. Parameters ---------- tangent_vec_a : array-like, shape=[n_samples, n, n] tangent_vec_b : array-like, shape=[n_samples, n, n] base_point : array-like, shape=[n_samples, n, n] Returns ------- inner_product : array-like, shape=[n_samples, n, n] """ power_affine = self.power_affine tangent_vec_a = gs.to_ndarray(tangent_vec_a, to_ndim=3) n_tangent_vecs_a, _, _ = tangent_vec_a.shape tangent_vec_b = gs.to_ndarray(tangent_vec_b, to_ndim=3) n_tangent_vecs_b, _, _ = tangent_vec_b.shape base_point = gs.to_ndarray(base_point, to_ndim=3) n_base_points, _, _ = base_point.shape spd_space = self.space assert (n_tangent_vecs_a == n_tangent_vecs_b == n_base_points or n_tangent_vecs_a == n_tangent_vecs_b and n_base_points == 1 or n_base_points == n_tangent_vecs_a and n_tangent_vecs_b == 1 or n_base_points == n_tangent_vecs_b and n_tangent_vecs_a == 1 or n_tangent_vecs_a == 1 and n_tangent_vecs_b == 1 or n_base_points == 1 and n_tangent_vecs_a == 1 or n_base_points == 1 and n_tangent_vecs_b == 1) if n_tangent_vecs_a == 1: tangent_vec_a = gs.tile( tangent_vec_a, (gs.maximum(n_base_points, n_tangent_vecs_b), 1, 1)) if n_tangent_vecs_b == 1: tangent_vec_b = gs.tile( tangent_vec_b, (gs.maximum(n_base_points, n_tangent_vecs_a), 1, 1)) if n_base_points == 1: base_point = gs.tile( base_point, (gs.maximum(n_tangent_vecs_a, n_tangent_vecs_b), 1, 1)) if power_affine == 1: inv_base_point = gs.linalg.inv(base_point) inner_product = self._aux_inner_product(tangent_vec_a, tangent_vec_b, inv_base_point) else: modified_tangent_vec_a =\ spd_space.differential_power(power_affine, tangent_vec_a, base_point) modified_tangent_vec_b =\ spd_space.differential_power(power_affine, tangent_vec_b, base_point) power_inv_base_point = gs.linalg.powerm(base_point, -power_affine) inner_product = self._aux_inner_product(modified_tangent_vec_a, modified_tangent_vec_b, power_inv_base_point) inner_product = inner_product / (power_affine**2) inner_product = gs.to_ndarray(inner_product, to_ndim=2, axis=1) return inner_product