def test_projection_and_belongs(self): gs.random.seed(3) group = SpecialOrthogonal(n=4) mat = gs.random.rand(4, 4) point = group.projection(mat) result = group.belongs(point) self.assertTrue(result) mat = gs.random.rand(2, 4, 4) point = group.projection(mat) result = group.belongs(point, atol=1e-4) self.assertTrue(gs.all(result))
def test_load_poses_only_rotations(self): """Test that the poses belong to SO(3).""" so3 = SpecialOrthogonal(n=3, point_type="vector") data, _ = data_utils.load_poses() result = so3.belongs(data) self.assertTrue(gs.all(result))
class _SpecialEuclideanVectors(LieGroup): """Base Class for the special Euclidean groups in 2d and 3d in vector form. i.e. the Lie group of rigid transformations. Elements of SE(2), SE(3) can either be represented as vectors (in 2d or 3d) or as matrices in general. The matrix representation corresponds to homogeneous coordinates. This class is specific to the vector representation of rotations. For the matrix representation use the SpecialEuclidean class and set `n=2` or `n=3`. Parameter --------- epsilon : float Precision to use for calculations involving potential division by 0 in rotations. Optional, default: 0. """ def __init__(self, n, epsilon=0.0): dim = n * (n + 1) // 2 LieGroup.__init__( self, dim=dim, shape=(dim, ), default_point_type="vector", lie_algebra=Euclidean(dim), ) self.n = n self.epsilon = epsilon self.rotations = SpecialOrthogonal(n=n, point_type="vector", epsilon=epsilon) self.translations = Euclidean(dim=n) def get_identity(self, point_type=None): """Get the identity of the group. Parameters ---------- point_type : str, {'vector', 'matrix'} The point_type of the returned value. Optional, default: self.default_point_type Returns ------- identity : array-like, shape={[dim], [n + 1, n + 1]} """ if point_type is None: point_type = self.default_point_type identity = gs.zeros(self.dim) return identity identity = property(get_identity) def get_point_type_shape(self, point_type=None): """Get the shape of the instance given the default_point_style.""" return self.get_identity(point_type).shape def belongs(self, point, atol=gs.atol): """Evaluate if a point belongs to SE(2) or SE(3). Parameters ---------- point : array-like, shape=[..., dim] Point to check. Returns ------- belongs : array-like, shape=[...,] Boolean indicating whether point belongs to SE(2) or SE(3). """ point_dim = point.shape[-1] point_ndim = point.ndim belongs = gs.logical_and(point_dim == self.dim, point_ndim < 3) belongs = gs.logical_and( belongs, self.rotations.belongs(point[..., :self.rotations.dim], atol=atol)) return belongs def projection(self, point): """Project a point to the group. The point is regularized, so that the norm of the rotation part lie in [0, pi). Parameters ---------- point: array-like, shape[..., dim] Point. Returns ------- projected: array-like, shape[..., dim] Regularized point. """ return self.regularize(point) def regularize(self, point): """Regularize a point to the default representation for SE(n). Parameters ---------- point : array-like, shape=[..., dim] Point to regularize. Returns ------- point : array-like, shape=[..., dim] Regularized point. """ rotations = self.rotations dim_rotations = rotations.dim regularized_point = gs.copy(point) rot_vec = regularized_point[..., :dim_rotations] regularized_rot_vec = rotations.regularize(rot_vec) translation = regularized_point[..., dim_rotations:] return gs.concatenate([regularized_rot_vec, translation], axis=-1) @geomstats.vectorization.decorator(["else", "vector", "else"]) def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None): """Regularize a tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[..., dim] Tangent vector at base point. metric : RiemannianMetric Metric. Optional, default: None. Returns ------- regularized_vec : array-like, shape=[..., dim] Regularized vector. """ return self.regularize_tangent_vec(tangent_vec, self.identity, metric) @geomstats.vectorization.decorator(["else", "vector"]) def matrix_from_vector(self, vec): """Convert point in vector point-type to matrix. Parameters ---------- vec : array-like, shape=[..., dim] Vector. Returns ------- mat : array-like, shape=[..., n+1, n+1] Matrix. """ vec = self.regularize(vec) output_shape = ((vec.shape[0], self.n + 1, self.n + 1) if vec.ndim == 2 else (self.n + 1, ) * 2) rot_vec = vec[..., :self.rotations.dim] trans_vec = vec[..., self.rotations.dim:] rot_mat = self.rotations.matrix_from_rotation_vector(rot_vec) return homogeneous_representation(rot_mat, trans_vec, output_shape) @geomstats.vectorization.decorator(["else", "vector", "vector"]) def compose(self, point_a, point_b): r"""Compose two elements of SE(2) or SE(3). Parameters ---------- point_a : array-like, shape=[..., dim] Point of the group. point_b : array-like, shape=[..., dim] Point of the group. Equation -------- (:math: `(R_1, t_1) \\cdot (R_2, t_2) = (R_1 R_2, R_1 t_2 + t_1)`) Returns ------- composition : array-like, shape=[..., dim] Composition of point_a and point_b. """ rotations = self.rotations dim_rotations = rotations.dim point_a = self.regularize(point_a) point_b = self.regularize(point_b) 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) @geomstats.vectorization.decorator(["else", "vector"]) def inverse(self, point): r"""Compute the group inverse in SE(n). Parameters ---------- point: array-like, shape=[..., dim] Point. Returns ------- inverse_point : array-like, shape=[..., dim] 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) @geomstats.vectorization.decorator(["else", "vector"]) def exp_from_identity(self, tangent_vec): """Compute group exponential of the tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[..., 3] Tangent vector at base point. Returns ------- group_exp: array-like, shape=[..., 3] Group exponential of the tangent vectors computed at the identity. """ rotations = self.rotations dim_rotations = rotations.dim rot_vec = tangent_vec[..., :dim_rotations] rot_vec_regul = self.rotations.regularize(rot_vec) rot_vec_regul = gs.to_ndarray(rot_vec_regul, to_ndim=2, axis=1) transform = self._exp_translation_transform(rot_vec_regul) translation = tangent_vec[..., dim_rotations:] exp_translation = gs.einsum("ijk, ik -> ij", transform, translation) group_exp = gs.concatenate([rot_vec, exp_translation], axis=1) group_exp = self.regularize(group_exp) return group_exp @geomstats.vectorization.decorator(["else", "vector"]) def log_from_identity(self, point): """Compute the group logarithm of the point at the identity. Parameters ---------- point: array-like, shape=[..., 3] Point. Returns ------- group_log: array-like, shape=[..., 3] Group logarithm in the Lie algebra. """ point = self.regularize(point) rotations = self.rotations dim_rotations = rotations.dim rot_vec = point[:, :dim_rotations] translation = point[:, dim_rotations:] transform = self._log_translation_transform(rot_vec) log_translation = gs.einsum("ijk, ik -> ij", transform, translation) return gs.concatenate([rot_vec, log_translation], axis=1) def random_point(self, n_samples=1, bound=1.0, **kwargs): r"""Sample in SE(n) with the uniform distribution. Parameters ---------- n_samples : int Number of samples. Optional, default: 1. bound : float Upper bound for the translation part of the sample. Optional, default: 1. Returns ------- random_point : array-like, shape=[..., dim] Sample. """ random_translation = self.translations.random_point(n_samples, bound) random_rot_vec = self.rotations.random_uniform(n_samples) return gs.concatenate([random_rot_vec, random_translation], axis=-1)
class TestFrechetMean(geomstats.tests.TestCase): _multiprocess_can_split_ = True def setUp(self): gs.random.seed(123) self.sphere = Hypersphere(dim=4) self.hyperbolic = Hyperboloid(dim=3) self.euclidean = Euclidean(dim=2) self.minkowski = Minkowski(dim=2) self.so3 = SpecialOrthogonal(n=3, point_type='vector') self.so_matrix = SpecialOrthogonal(n=3) def test_logs_at_mean_default_gradient_descent_sphere(self): n_tests = 10 estimator = FrechetMean( metric=self.sphere.metric, method='default', lr=1.) result = [] for _ in range(n_tests): # take 2 random points, compute their mean, and verify that # log of each at the mean is opposite points = self.sphere.random_uniform(n_samples=2) estimator.fit(points) mean = estimator.estimate_ logs = self.sphere.metric.log(point=points, base_point=mean) result.append(gs.linalg.norm(logs[1, :] + logs[0, :])) result = gs.stack(result) expected = gs.zeros(n_tests) self.assertAllClose(expected, result) def test_logs_at_mean_adaptive_gradient_descent_sphere(self): n_tests = 10 estimator = FrechetMean(metric=self.sphere.metric, method='adaptive') result = [] for _ in range(n_tests): # take 2 random points, compute their mean, and verify that # log of each at the mean is opposite points = self.sphere.random_uniform(n_samples=2) estimator.fit(points) mean = estimator.estimate_ logs = self.sphere.metric.log(point=points, base_point=mean) result.append(gs.linalg.norm(logs[1, :] + logs[0, :])) result = gs.stack(result) expected = gs.zeros(n_tests) self.assertAllClose(expected, result) def test_estimate_shape_default_gradient_descent_sphere(self): dim = 5 point_a = gs.array([1., 0., 0., 0., 0.]) point_b = gs.array([0., 1., 0., 0., 0.]) points = gs.array([point_a, point_b]) mean = FrechetMean( metric=self.sphere.metric, method='default', verbose=True) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim,)) def test_estimate_shape_adaptive_gradient_descent_sphere(self): dim = 5 point_a = gs.array([1., 0., 0., 0., 0.]) point_b = gs.array([0., 1., 0., 0., 0.]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method='adaptive') mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim,)) def test_estimate_and_belongs_default_gradient_descent_sphere(self): point_a = gs.array([1., 0., 0., 0., 0.]) point_b = gs.array([0., 1., 0., 0., 0.]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method='default') mean.fit(points) result = self.sphere.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_estimate_default_gradient_descent_so3(self): points = self.so3.random_uniform(2) mean_vec = FrechetMean( metric=self.so3.bi_invariant_metric, method='default', lr=1.) mean_vec.fit(points) logs = self.so3.bi_invariant_metric.log(points, mean_vec.estimate_) result = gs.sum(logs, axis=0) expected = gs.zeros_like(points[0]) self.assertAllClose(result, expected) def test_estimate_and_belongs_default_gradient_descent_so3(self): point = self.so3.random_uniform(10) mean_vec = FrechetMean( metric=self.so3.bi_invariant_metric, method='default') mean_vec.fit(point) result = self.so3.belongs(mean_vec.estimate_) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_and_tf_only def test_estimate_default_gradient_descent_so_matrix(self): points = self.so_matrix.random_uniform(2) mean_vec = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method='default', lr=1.) mean_vec.fit(points) logs = self.so_matrix.bi_invariant_metric.log( points, mean_vec.estimate_) result = gs.sum(logs, axis=0) expected = gs.zeros_like(points[0]) self.assertAllClose(result, expected, atol=1e-5) @geomstats.tests.np_and_tf_only def test_estimate_and_belongs_default_gradient_descent_so_matrix(self): point = self.so_matrix.random_uniform(10) mean = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method='default') mean.fit(point) result = self.so_matrix.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_and_tf_only def test_estimate_and_belongs_adaptive_gradient_descent_so_matrix(self): point = self.so_matrix.random_uniform(10) mean = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method='adaptive', verbose=True, lr=.5) mean.fit(point) result = self.so_matrix.belongs(mean.estimate_) self.assertTrue(result) @geomstats.tests.np_and_tf_only def test_estimate_and_coincide_default_so_vec_and_mat(self): point = self.so_matrix.random_uniform(3) mean = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method='default') mean.fit(point) expected = mean.estimate_ mean_vec = FrechetMean( metric=self.so3.bi_invariant_metric, method='default') point_vec = self.so3.rotation_vector_from_matrix(point) mean_vec.fit(point_vec) result_vec = mean_vec.estimate_ result = self.so3.matrix_from_rotation_vector(result_vec) self.assertAllClose(result, expected) def test_estimate_and_belongs_adaptive_gradient_descent_sphere(self): point_a = gs.array([1., 0., 0., 0., 0.]) point_b = gs.array([0., 1., 0., 0., 0.]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method='adaptive') mean.fit(points) result = self.sphere.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_variance_sphere(self): point = gs.array([0., 0., 0., 0., 1.]) points = gs.array([point, point]) result = variance( points, base_point=point, metric=self.sphere.metric) expected = gs.array(0.) self.assertAllClose(expected, result) def test_estimate_default_gradient_descent_sphere(self): point = gs.array([0., 0., 0., 0., 1.]) points = gs.array([point, point]) mean = FrechetMean(metric=self.sphere.metric, method='default') mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_adaptive_gradient_descent_sphere(self): point = gs.array([0., 0., 0., 0., 1.]) points = gs.array([point, point]) mean = FrechetMean(metric=self.sphere.metric, method='adaptive') mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_spd(self): point = SPDMatrices(3).random_point() points = gs.array([point, point]) mean = FrechetMean(metric=SPDMetricAffine(3), point_type='matrix') mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_variance_hyperbolic(self): point = gs.array([2., 1., 1., 1.]) points = gs.array([point, point]) result = variance( points, base_point=point, metric=self.hyperbolic.metric) expected = gs.array(0.) self.assertAllClose(result, expected) def test_estimate_hyperbolic(self): point = gs.array([2., 1., 1., 1.]) points = gs.array([point, point]) mean = FrechetMean(metric=self.hyperbolic.metric) mean.fit(X=points) expected = point result = mean.estimate_ self.assertAllClose(result, expected) def test_estimate_and_belongs_hyperbolic(self): point_a = self.hyperbolic.random_point() point_b = self.hyperbolic.random_point() point_c = self.hyperbolic.random_point() points = gs.stack([point_a, point_b, point_c], axis=0) mean = FrechetMean(metric=self.hyperbolic.metric) mean.fit(X=points) result = self.hyperbolic.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_mean_euclidean_shape(self): dim = 2 point = gs.array([1., 4.]) mean = FrechetMean(metric=self.euclidean.metric) points = [point, point, point] mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim,)) def test_mean_euclidean(self): point = gs.array([1., 4.]) mean = FrechetMean(metric=self.euclidean.metric) points = [point, point, point] mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) points = gs.array([ [1., 2.], [2., 3.], [3., 4.], [4., 5.]]) weights = [1., 2., 1., 2.] mean = FrechetMean(metric=self.euclidean.metric) mean.fit(points, weights=weights) result = mean.estimate_ expected = gs.array([16. / 6., 22. / 6.]) self.assertAllClose(result, expected) def test_variance_euclidean(self): points = gs.array([ [1., 2.], [2., 3.], [3., 4.], [4., 5.]]) weights = gs.array([1., 2., 1., 2.]) base_point = gs.zeros(2) result = variance( points, weights=weights, base_point=base_point, metric=self.euclidean.metric) # we expect the average of the points' sq norms. expected = gs.array((1 * 5. + 2 * 13. + 1 * 25. + 2 * 41.) / 6.) self.assertAllClose(result, expected) def test_mean_matrices_shape(self): m, n = (2, 2) point = gs.array([ [1., 4.], [2., 3.]]) metric = MatricesMetric(m, n) mean = FrechetMean(metric=metric, point_type='matrix') points = [point, point, point] mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (m, n)) def test_mean_matrices(self): m, n = (2, 2) point = gs.array([ [1., 4.], [2., 3.]]) metric = MatricesMetric(m, n) mean = FrechetMean(metric=metric, point_type='matrix') points = [point, point, point] mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) def test_mean_minkowski_shape(self): dim = 2 point = gs.array([2., -math.sqrt(3)]) points = [point, point, point] mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim,)) def test_mean_minkowski(self): point = gs.array([2., -math.sqrt(3)]) points = [point, point, point] mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) points = gs.array([ [1., 0.], [2., math.sqrt(3)], [3., math.sqrt(8)], [4., math.sqrt(24)]]) weights = gs.array([1., 2., 1., 2.]) mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points, weights=weights) result = mean.estimate_ result = self.minkowski.belongs(result) expected = gs.array(True) self.assertAllClose(result, expected) def test_variance_minkowski(self): points = gs.array([ [1., 0.], [2., math.sqrt(3)], [3., math.sqrt(8)], [4., math.sqrt(24)]]) weights = gs.array([1., 2., 1., 2.]) base_point = gs.array([-1., 0.]) var = variance( points, weights=weights, base_point=base_point, metric=self.minkowski.metric) result = var != 0 # we expect the average of the points' Minkowski sq norms. expected = True self.assertAllClose(result, expected) def test_one_point(self): point = gs.array([0., 0., 0., 0., 1.]) mean = FrechetMean(metric=self.sphere.metric, method='default') mean.fit(X=point) result = mean.estimate_ expected = point self.assertAllClose(expected, result) mean = FrechetMean( metric=self.sphere.metric, method='frechet-poincare-ball') mean.fit(X=point) result = mean.estimate_ expected = point self.assertAllClose(expected, result)
class _SpecialEuclideanMatrices(GeneralLinear, LieGroup): """Class for special Euclidean group. Parameters ---------- n : int Integer dimension of the underlying Euclidean space. Matrices will be of size: (n+1) x (n+1). Attributes ---------- rotations : SpecialOrthogonal Subgroup of rotations of size n. translations : Euclidean Subgroup of translations of size n. left_canonical_metric : InvariantMetric The left invariant metric that corresponds to the Frobenius inner product at the identity. right_canonical_metric : InvariantMetric The right invariant metric that corresponds to the Frobenius inner product at the identity. metric : MatricesMetric The Euclidean (Frobenius) inner product. """ def __init__(self, n): super().__init__(n=n + 1, dim=int((n * (n + 1)) / 2), default_point_type='matrix', lie_algebra=SpecialEuclideanMatrixLieAlgebra(n=n)) self.rotations = SpecialOrthogonal(n=n) self.translations = Euclidean(dim=n) self.n = n self.left_canonical_metric = \ SpecialEuclideanMatrixCannonicalLeftMetric(group=self) def get_identity(self): """Return the identity matrix.""" return gs.eye(self.n + 1, self.n + 1) identity = property(get_identity) def belongs(self, point, atol=gs.atol): """Check whether point is of the form rotation, translation. Parameters ---------- point : array-like, shape=[..., n, n]. Point to be checked. atol : float Tolerance threshold. Returns ------- belongs : array-like, shape=[...,] Boolean denoting if point belongs to the group. """ n = self.n belongs = Matrices(n + 1, n + 1).belongs(point) if gs.all(belongs): rotation = point[..., :n, :n] belongs = self.rotations.belongs(rotation, atol=atol) last_line_except_last_term = point[..., n:, :-1] all_but_last_zeros = ~gs.any(last_line_except_last_term, axis=(-2, -1)) belongs = gs.logical_and(belongs, all_but_last_zeros) last_term = point[..., n, n] belongs = gs.logical_and(belongs, gs.isclose(last_term, 1., atol=atol)) return belongs def random_point(self, n_samples=1, bound=1.): """Sample in SE(n) from the uniform distribution. Parameters ---------- n_samples : int Number of samples. Optional, default: 1. bound: float Bound of the interval in which to sample each entry of the translation part. Optional, default: 1. Returns ------- samples : array-like, shape=[..., n + 1, n + 1] Sample in SE(n). """ random_translation = self.translations.random_point(n_samples) random_rotation = self.rotations.random_uniform(n_samples) output_shape = ((n_samples, self.n + 1, self.n + 1) if n_samples != 1 else (self.n + 1, ) * 2) random_point = homogeneous_representation(random_rotation, random_translation, output_shape) return random_point @classmethod def inverse(cls, point): """Return the inverse of a point. Parameters ---------- point : array-like, shape=[..., n, n] Point to be inverted. """ n = point.shape[-1] - 1 transposed_rot = cls.transpose(point[..., :n, :n]) translation = point[..., :n, -1] translation = gs.einsum('...ij,...j->...i', transposed_rot, translation) return homogeneous_representation(transposed_rot, -translation, point.shape)
class _SpecialEuclideanMatrices(GeneralLinear, LieGroup): """Class for special orthogonal groups. Parameters ---------- n : int Integer representing the shape of the matrices: n x n. """ def __init__(self, n): super(_SpecialEuclideanMatrices, self).__init__(default_point_type='matrix', n=n + 1) self.rotations = SpecialOrthogonal(n=n) self.translations = Euclidean(dim=n) self.n = n self.dim = int((n * (n + 1)) / 2) def get_identity(self): """Return the identity matrix.""" return gs.eye(self.n + 1, self.n + 1) identity = property(get_identity) def belongs(self, point): """Check whether point is of the form rotation, translation. Parameters ---------- point : array-like, shape=[..., n, n]. Point to be checked. Returns ------- belongs : array-like, shape=[...,] Boolean denoting if point belongs to the group. """ point_dim1, point_dim2 = point.shape[-2:] belongs = (point_dim1 == point_dim2 == self.n + 1) rotation = point[..., :self.n, :self.n] rot_belongs = self.rotations.belongs(rotation) belongs = gs.logical_and(belongs, rot_belongs) last_line_except_last_term = point[..., self.n:, :-1] all_but_last_zeros = ~gs.any(last_line_except_last_term, axis=(-2, -1)) belongs = gs.logical_and(belongs, all_but_last_zeros) last_term = point[..., self.n:, self.n:] belongs = gs.logical_and(belongs, gs.all(last_term == 1, axis=(-2, -1))) if point.ndim == 2: return gs.squeeze(belongs) return gs.flatten(belongs) def _is_in_lie_algebra(self, tangent_vec, atol=TOLERANCE): """Project vector rotation part onto skew-symmetric matrices.""" point_dim1, point_dim2 = tangent_vec.shape[-2:] belongs = (point_dim1 == point_dim2 == self.n + 1) rotation = tangent_vec[..., :self.n, :self.n] rot_belongs = self.is_skew_symmetric(rotation, atol=atol) belongs = gs.logical_and(belongs, rot_belongs) last_line = tangent_vec[..., -1, :] all_zeros = ~gs.any(last_line, axis=-1) belongs = gs.logical_and(belongs, all_zeros) return belongs def _to_lie_algebra(self, tangent_vec): """Project vector rotation part onto skew-symmetric matrices.""" translation_mask = gs.hstack( [gs.ones((self.n, ) * 2), 2 * gs.ones((self.n, 1))]) translation_mask = gs.concatenate( [translation_mask, gs.zeros((1, self.n + 1))], axis=0) tangent_vec = tangent_vec * gs.where(translation_mask != 0., gs.array(1.), gs.array(0.)) tangent_vec = (tangent_vec - GeneralLinear.transpose(tangent_vec)) / 2. return tangent_vec * translation_mask def random_uniform(self, n_samples=1, tol=1e-6): """Sample in SE(n) from the uniform distribution. Parameters ---------- n_samples : int Number of samples. Optional, default: 1. tol : unused Returns ------- samples : array-like, shape=[..., n + 1, n + 1] Sample in SE(n). """ random_translation = self.translations.random_uniform(n_samples) random_rotation = self.rotations.random_uniform(n_samples) random_rotation = gs.to_ndarray(random_rotation, to_ndim=3) random_translation = gs.to_ndarray(random_translation, to_ndim=2) random_translation = gs.transpose( gs.to_ndarray(random_translation, to_ndim=3, axis=1), (0, 2, 1)) random_point = gs.concatenate((random_rotation, random_translation), axis=2) last_line = gs.zeros((n_samples, 1, self.n + 1)) random_point = gs.concatenate((random_point, last_line), axis=1) random_point = gs.assignment(random_point, 1, (-1, -1), axis=0) if gs.shape(random_point)[0] == 1: random_point = gs.squeeze(random_point, axis=0) return random_point
class TestFrechetMean(geomstats.tests.TestCase): _multiprocess_can_split_ = True def setup_method(self): gs.random.seed(123) self.sphere = Hypersphere(dim=4) self.hyperbolic = Hyperboloid(dim=3) self.euclidean = Euclidean(dim=2) self.minkowski = Minkowski(dim=2) self.so3 = SpecialOrthogonal(n=3, point_type="vector") self.so_matrix = SpecialOrthogonal(n=3) self.curves_2d = DiscreteCurves(R2) self.elastic_metric = ElasticMetric(a=1, b=1, ambient_manifold=R2) def test_logs_at_mean_curves_2d(self): n_tests = 10 metric = self.curves_2d.srv_metric estimator = FrechetMean(metric=metric, init_step_size=1.0) result = [] for _ in range(n_tests): # take 2 random points, compute their mean, and verify that # log of each at the mean is opposite points = self.curves_2d.random_point(n_samples=2) estimator.fit(points) mean = estimator.estimate_ # Expand and tile are added because of GitHub issue 1575 mean = gs.expand_dims(mean, axis=0) mean = gs.tile(mean, (2, 1, 1)) logs = metric.log(point=points, base_point=mean) logs_srv = metric.aux_differential_srv_transform(logs, curve=mean) # Note that the logs are NOT inverse, only the logs_srv are. result.append(gs.linalg.norm(logs_srv[1, :] + logs_srv[0, :])) result = gs.stack(result) expected = gs.zeros(n_tests) self.assertAllClose(expected, result, atol=1e-5) def test_logs_at_mean_default_gradient_descent_sphere(self): n_tests = 10 estimator = FrechetMean(metric=self.sphere.metric, method="default", init_step_size=1.0) result = [] for _ in range(n_tests): # take 2 random points, compute their mean, and verify that # log of each at the mean is opposite points = self.sphere.random_uniform(n_samples=2) estimator.fit(points) mean = estimator.estimate_ logs = self.sphere.metric.log(point=points, base_point=mean) result.append(gs.linalg.norm(logs[1, :] + logs[0, :])) result = gs.stack(result) expected = gs.zeros(n_tests) self.assertAllClose(expected, result) def test_logs_at_mean_adaptive_gradient_descent_sphere(self): n_tests = 10 estimator = FrechetMean(metric=self.sphere.metric, method="adaptive") result = [] for _ in range(n_tests): # take 2 random points, compute their mean, and verify that # log of each at the mean is opposite points = self.sphere.random_uniform(n_samples=2) estimator.fit(points) mean = estimator.estimate_ logs = self.sphere.metric.log(point=points, base_point=mean) result.append(gs.linalg.norm(logs[1, :] + logs[0, :])) result = gs.stack(result) expected = gs.zeros(n_tests) self.assertAllClose(expected, result) def test_estimate_shape_default_gradient_descent_sphere(self): dim = 5 point_a = gs.array([1.0, 0.0, 0.0, 0.0, 0.0]) point_b = gs.array([0.0, 1.0, 0.0, 0.0, 0.0]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method="default", verbose=True) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim, )) def test_estimate_shape_adaptive_gradient_descent_sphere(self): dim = 5 point_a = gs.array([1.0, 0.0, 0.0, 0.0, 0.0]) point_b = gs.array([0.0, 1.0, 0.0, 0.0, 0.0]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method="adaptive") mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim, )) def test_estimate_shape_elastic_metric(self): points = self.curves_2d.random_point(n_samples=2) mean = FrechetMean(metric=self.elastic_metric) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (points.shape[1:])) def test_estimate_shape_metric(self): points = self.curves_2d.random_point(n_samples=2) mean = FrechetMean(metric=self.curves_2d.srv_metric) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (points.shape[1:])) def test_estimate_and_belongs_default_gradient_descent_sphere(self): point_a = gs.array([1.0, 0.0, 0.0, 0.0, 0.0]) point_b = gs.array([0.0, 1.0, 0.0, 0.0, 0.0]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method="default") mean.fit(points) result = self.sphere.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_estimate_and_belongs_curves_2d(self): points = self.curves_2d.random_point(n_samples=2) mean = FrechetMean(metric=self.curves_2d.srv_metric) mean.fit(points) result = self.curves_2d.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_estimate_default_gradient_descent_so3(self): points = self.so3.random_uniform(2) mean_vec = FrechetMean(metric=self.so3.bi_invariant_metric, method="default", init_step_size=1.0) mean_vec.fit(points) logs = self.so3.bi_invariant_metric.log(points, mean_vec.estimate_) result = gs.sum(logs, axis=0) expected = gs.zeros_like(points[0]) self.assertAllClose(result, expected) def test_estimate_and_belongs_default_gradient_descent_so3(self): point = self.so3.random_uniform(10) mean_vec = FrechetMean(metric=self.so3.bi_invariant_metric, method="default") mean_vec.fit(point) result = self.so3.belongs(mean_vec.estimate_) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_autograd_and_tf_only def test_estimate_default_gradient_descent_so_matrix(self): points = self.so_matrix.random_uniform(2) mean_vec = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method="default", init_step_size=1.0, ) mean_vec.fit(points) logs = self.so_matrix.bi_invariant_metric.log(points, mean_vec.estimate_) result = gs.sum(logs, axis=0) expected = gs.zeros_like(points[0]) self.assertAllClose(result, expected, atol=1e-5) @geomstats.tests.np_autograd_and_tf_only def test_estimate_and_belongs_default_gradient_descent_so_matrix(self): point = self.so_matrix.random_uniform(10) mean = FrechetMean(metric=self.so_matrix.bi_invariant_metric, method="default") mean.fit(point) result = self.so_matrix.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_autograd_and_tf_only def test_estimate_and_belongs_adaptive_gradient_descent_so_matrix(self): point = self.so_matrix.random_uniform(10) mean = FrechetMean( metric=self.so_matrix.bi_invariant_metric, method="adaptive", init_step_size=0.5, verbose=True, ) mean.fit(point) result = self.so_matrix.belongs(mean.estimate_) self.assertTrue(result) @geomstats.tests.np_autograd_and_tf_only def test_estimate_and_coincide_default_so_vec_and_mat(self): point = self.so_matrix.random_uniform(3) mean = FrechetMean(metric=self.so_matrix.bi_invariant_metric, method="default") mean.fit(point) expected = mean.estimate_ mean_vec = FrechetMean(metric=self.so3.bi_invariant_metric, method="default") point_vec = self.so3.rotation_vector_from_matrix(point) mean_vec.fit(point_vec) result_vec = mean_vec.estimate_ result = self.so3.matrix_from_rotation_vector(result_vec) self.assertAllClose(result, expected) def test_estimate_and_belongs_adaptive_gradient_descent_sphere(self): point_a = gs.array([1.0, 0.0, 0.0, 0.0, 0.0]) point_b = gs.array([0.0, 1.0, 0.0, 0.0, 0.0]) points = gs.array([point_a, point_b]) mean = FrechetMean(metric=self.sphere.metric, method="adaptive") mean.fit(points) result = self.sphere.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_variance_sphere(self): point = gs.array([0.0, 0.0, 0.0, 0.0, 1.0]) points = gs.array([point, point]) result = variance(points, base_point=point, metric=self.sphere.metric) expected = gs.array(0.0) self.assertAllClose(expected, result) def test_estimate_default_gradient_descent_sphere(self): point = gs.array([0.0, 0.0, 0.0, 0.0, 1.0]) points = gs.array([point, point]) mean = FrechetMean(metric=self.sphere.metric, method="default") mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_elastic_metric(self): point = self.curves_2d.random_point(n_samples=1) points = gs.array([point, point]) mean = FrechetMean(metric=self.elastic_metric) mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_curves_2d(self): point = self.curves_2d.random_point(n_samples=1) points = gs.array([point, point]) mean = FrechetMean(metric=self.curves_2d.srv_metric) mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_adaptive_gradient_descent_sphere(self): point = gs.array([0.0, 0.0, 0.0, 0.0, 1.0]) points = gs.array([point, point]) mean = FrechetMean(metric=self.sphere.metric, method="adaptive") mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_spd(self): point = SPDMatrices(3).random_point() points = gs.array([point, point]) mean = FrechetMean(metric=SPDMetricAffine(3), point_type="matrix") mean.fit(X=points) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_estimate_spd_two_samples(self): space = SPDMatrices(3) metric = SPDMetricAffine(3) point = space.random_point(2) mean = FrechetMean(metric) mean.fit(point) result = mean.estimate_ expected = metric.exp(metric.log(point[0], point[1]) / 2, point[1]) self.assertAllClose(expected, result) def test_variance_hyperbolic(self): point = gs.array([2.0, 1.0, 1.0, 1.0]) points = gs.array([point, point]) result = variance(points, base_point=point, metric=self.hyperbolic.metric) expected = gs.array(0.0) self.assertAllClose(result, expected) def test_estimate_hyperbolic(self): point = gs.array([2.0, 1.0, 1.0, 1.0]) points = gs.array([point, point]) mean = FrechetMean(metric=self.hyperbolic.metric) mean.fit(X=points) expected = point result = mean.estimate_ self.assertAllClose(result, expected) def test_estimate_and_belongs_hyperbolic(self): point_a = self.hyperbolic.random_point() point_b = self.hyperbolic.random_point() point_c = self.hyperbolic.random_point() points = gs.stack([point_a, point_b, point_c], axis=0) mean = FrechetMean(metric=self.hyperbolic.metric) mean.fit(X=points) result = self.hyperbolic.belongs(mean.estimate_) expected = True self.assertAllClose(result, expected) def test_mean_euclidean_shape(self): dim = 2 point = gs.array([1.0, 4.0]) mean = FrechetMean(metric=self.euclidean.metric) points = [point, point, point] mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim, )) def test_mean_euclidean(self): point = gs.array([1.0, 4.0]) mean = FrechetMean(metric=self.euclidean.metric) points = [point, point, point] mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) points = gs.array([[1.0, 2.0], [2.0, 3.0], [3.0, 4.0], [4.0, 5.0]]) weights = [1.0, 2.0, 1.0, 2.0] mean = FrechetMean(metric=self.euclidean.metric) mean.fit(points, weights=weights) result = mean.estimate_ expected = gs.array([16.0 / 6.0, 22.0 / 6.0]) self.assertAllClose(result, expected) def test_variance_euclidean(self): points = gs.array([[1.0, 2.0], [2.0, 3.0], [3.0, 4.0], [4.0, 5.0]]) weights = gs.array([1.0, 2.0, 1.0, 2.0]) base_point = gs.zeros(2) result = variance(points, weights=weights, base_point=base_point, metric=self.euclidean.metric) # we expect the average of the points' sq norms. expected = gs.array((1 * 5.0 + 2 * 13.0 + 1 * 25.0 + 2 * 41.0) / 6.0) self.assertAllClose(result, expected) def test_mean_matrices_shape(self): m, n = (2, 2) point = gs.array([[1.0, 4.0], [2.0, 3.0]]) metric = MatricesMetric(m, n) mean = FrechetMean(metric=metric, point_type="matrix") points = [point, point, point] mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (m, n)) def test_mean_matrices(self): m, n = (2, 2) point = gs.array([[1.0, 4.0], [2.0, 3.0]]) metric = MatricesMetric(m, n) mean = FrechetMean(metric=metric, point_type="matrix") points = [point, point, point] mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) def test_mean_minkowski_shape(self): dim = 2 point = gs.array([2.0, -math.sqrt(3)]) points = [point, point, point] mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points) result = mean.estimate_ self.assertAllClose(gs.shape(result), (dim, )) def test_mean_minkowski(self): point = gs.array([2.0, -math.sqrt(3)]) points = [point, point, point] mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points) result = mean.estimate_ expected = point self.assertAllClose(result, expected) points = gs.array([[1.0, 0.0], [2.0, math.sqrt(3)], [3.0, math.sqrt(8)], [4.0, math.sqrt(24)]]) weights = gs.array([1.0, 2.0, 1.0, 2.0]) mean = FrechetMean(metric=self.minkowski.metric) mean.fit(points, weights=weights) result = self.minkowski.belongs(mean.estimate_) self.assertTrue(result) def test_variance_minkowski(self): points = gs.array([[1.0, 0.0], [2.0, math.sqrt(3)], [3.0, math.sqrt(8)], [4.0, math.sqrt(24)]]) weights = gs.array([1.0, 2.0, 1.0, 2.0]) base_point = gs.array([-1.0, 0.0]) var = variance(points, weights=weights, base_point=base_point, metric=self.minkowski.metric) result = var != 0 # we expect the average of the points' Minkowski sq norms. expected = True self.assertAllClose(result, expected) def test_one_point(self): point = gs.array([0.0, 0.0, 0.0, 0.0, 1.0]) mean = FrechetMean(metric=self.sphere.metric, method="default") mean.fit(X=point) result = mean.estimate_ expected = point self.assertAllClose(expected, result) mean = FrechetMean(metric=self.sphere.metric, method="default") mean.fit(X=point) result = mean.estimate_ expected = point self.assertAllClose(expected, result) def test_batched(self): space = SPDMatrices(3) metric = SPDMetricAffine(3) point = space.random_point(4) mean_batch = FrechetMean(metric, method="batch", verbose=True) data = gs.stack([point[:2], point[2:]], axis=1) mean_batch.fit(data) result = mean_batch.estimate_ mean = FrechetMean(metric) mean.fit(data[:, 0]) expected_1 = mean.estimate_ mean.fit(data[:, 1]) expected_2 = mean.estimate_ expected = gs.stack([expected_1, expected_2]) self.assertAllClose(expected, result) @geomstats.tests.np_and_autograd_only def test_stiefel_two_samples(self): space = Stiefel(3, 2) metric = space.metric point = space.random_point(2) mean = FrechetMean(metric) mean.fit(point) result = mean.estimate_ expected = metric.exp(metric.log(point[0], point[1]) / 2, point[1]) self.assertAllClose(expected, result) @geomstats.tests.np_and_autograd_only def test_stiefel_n_samples(self): space = Stiefel(3, 2) metric = space.metric point = space.random_point(2) mean = FrechetMean(metric, method="default", init_step_size=0.5, verbose=True) mean.fit(point) result = space.belongs(mean.estimate_) self.assertTrue(result) def test_circle_mean(self): space = Hypersphere(1) points = space.random_uniform(10) mean_circle = FrechetMean(space.metric) mean_circle.fit(points) estimate_circle = mean_circle.estimate_ # set a wrong dimension so that the extrinsic coordinates are used metric = space.metric metric.dim = 2 mean_extrinsic = FrechetMean(metric) mean_extrinsic.fit(points) estimate_extrinsic = mean_extrinsic.estimate_ self.assertAllClose(estimate_circle, estimate_extrinsic)
class _SpecialEuclidean3Vectors(LieGroup): """Class for the special euclidean group in 3d, SE(3). i.e. the Lie group of rigid transformations. Elements of SE(3) can either be represented as vectors (in 3d) or as matrices in general. The matrix representation corresponds to homogeneous coordinates.This class is specific to the vector representation of rotations. For the matrix representation use the SpecialEuclidean class and set `n=3`. Parameter --------- epsilon : float, optional (defaults to 0) Precision to use for calculations involving potential division by 0 in rotations. """ def __init__(self, epsilon=0.): super(_SpecialEuclidean3Vectors, self).__init__(dim=6, default_point_type='vector') self.n = 3 self.epsilon = epsilon self.rotations = SpecialOrthogonal(n=3, point_type='vector', epsilon=epsilon) self.translations = Euclidean(dim=3) def get_identity(self, point_type=None): """Get the identity of the group. Parameters ---------- point_type : str, {'vector', 'matrix'}, optional The point_type of the returned value. default: self.default_point_type Returns ------- identity : array-like, shape={[dim], [n + 1, n + 1]} """ if point_type is None: point_type = self.default_point_type identity = gs.zeros(self.dim) if point_type == 'matrix': identity = gs.eye(self.n + 1) return identity identity = property(get_identity) def get_point_type_shape(self, point_type=None): """Get the shape of the instance given the default_point_style.""" return self.get_identity(point_type).shape def belongs(self, point): """Evaluate if a point belongs to SE(3). Parameters ---------- point : array-like, shape=[..., 3] The point of which to check whether it belongs to SE(3). Returns ------- belongs : array-like, shape=[..., 1] Boolean indicating whether point belongs to SE(3). """ point_dim = point.shape[-1] point_ndim = point.ndim belongs = gs.logical_and(point_dim == self.dim, point_ndim < 3) belongs = gs.logical_and(belongs, self.rotations.belongs(point[..., :self.n])) return belongs def regularize(self, point): """Regularize a point to the default representation for SE(n). Parameters ---------- point : array-like, shape=[..., 3] The point to regularize. Returns ------- point : array-like, shape=[..., 3] """ rotations = self.rotations dim_rotations = rotations.dim rot_vec = point[..., :dim_rotations] regularized_rot_vec = rotations.regularize(rot_vec) translation = point[..., dim_rotations:] return gs.concatenate([regularized_rot_vec, translation], axis=-1) @geomstats.vectorization.decorator(['else', 'vector', 'else']) def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None): """Regularize a tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[..., 3] metric : RiemannianMetric, optional Returns ------- regularized_vec : the regularized tangent vector """ return self.regularize_tangent_vec(tangent_vec, self.identity, metric) def regularize_tangent_vec(self, tangent_vec, base_point, metric=None): """Regularize a tangent vector at a base point. Parameters ---------- tangent_vec: array-like, shape=[..., 3] base_point : array-like, shape=[..., 3] metric : RiemannianMetric, optional default: self.left_canonical_metric Returns ------- regularized_vec : the regularized tangent vector """ if metric is None: metric = self.left_canonical_metric rotations = self.rotations dim_rotations = rotations.dim rot_tangent_vec = tangent_vec[..., :dim_rotations] rot_base_point = base_point[..., :dim_rotations] metric_mat = metric.inner_product_mat_at_identity rot_metric_mat = metric_mat[:dim_rotations, :dim_rotations] rot_metric = InvariantMetric( group=rotations, inner_product_mat_at_identity=rot_metric_mat, left_or_right=metric.left_or_right) rotations_vec = rotations.regularize_tangent_vec( tangent_vec=rot_tangent_vec, base_point=rot_base_point, metric=rot_metric) return gs.concatenate( [rotations_vec, tangent_vec[..., dim_rotations:]], axis=-1) @geomstats.vectorization.decorator(['else', 'vector']) def matrix_from_vector(self, vec): """Convert point in vector point-type to matrix. Parameters ---------- vec: array-like, shape=[..., 3] Returns ------- mat: array-like, shape=[..., n+1, n+1] """ vec = self.regularize(vec) n_vecs, _ = vec.shape rot_vec = vec[:, :self.rotations.dim] trans_vec = vec[:, self.rotations.dim:] rot_mat = self.rotations.matrix_from_rotation_vector(rot_vec) trans_vec = gs.reshape(trans_vec, (n_vecs, self.n, 1)) mat = gs.concatenate((rot_mat, trans_vec), axis=2) last_lines = gs.array(gs.get_mask_i_float(self.n, self.n + 1)) last_lines = gs.to_ndarray(last_lines, to_ndim=2) last_lines = gs.to_ndarray(last_lines, to_ndim=3) mat = gs.concatenate((mat, last_lines), axis=1) return mat @geomstats.vectorization.decorator(['else', 'vector', 'vector']) def compose(self, point_a, point_b): r"""Compose two elements of SE(3). Parameters ---------- point_a : array-like, shape=[..., 3] Point of the group. point_b : array-like, shape=[..., 3] Point of the group. 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_a and point_b. """ rotations = self.rotations dim_rotations = rotations.dim point_a = self.regularize(point_a) point_b = self.regularize(point_b) 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) @geomstats.vectorization.decorator(['else', 'vector']) 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) @geomstats.vectorization.decorator(['else', 'vector', 'else']) def jacobian_translation(self, point, left_or_right='left'): """Compute the Jacobian matrix resulting from translation. Compute the matrix of the differential of the left/right translations from the identity to point in SE(3). Parameters ---------- point: array-like, shape=[..., 3] left_or_right: str, {'left', 'right'}, optional Whether to compute the jacobian of the left or right translation. Returns ------- jacobian : array-like, shape=[..., 3] The jacobian of the left / right translation. """ if left_or_right not in ('left', 'right'): raise ValueError('`left_or_right` must be `left` or `right`.') rotations = self.rotations translations = self.translations dim_rotations = rotations.dim dim_translations = translations.dim point = self.regularize(point) n_points, _ = point.shape rot_vec = point[:, :dim_rotations] jacobian_rot = self.rotations.jacobian_translation( point=rot_vec, left_or_right=left_or_right) block_zeros_1 = gs.zeros((n_points, dim_rotations, dim_translations)) jacobian_block_line_1 = gs.concatenate([jacobian_rot, block_zeros_1], axis=2) if left_or_right == 'left': rot_mat = self.rotations.matrix_from_rotation_vector(rot_vec) jacobian_trans = rot_mat block_zeros_2 = gs.zeros( (n_points, dim_translations, dim_rotations)) jacobian_block_line_2 = gs.concatenate( [block_zeros_2, jacobian_trans], axis=2) else: inv_skew_mat = -self.rotations.skew_matrix_from_vector(rot_vec) eye = gs.to_ndarray(gs.eye(self.n), to_ndim=3) eye = gs.tile(eye, [n_points, 1, 1]) jacobian_block_line_2 = gs.concatenate([inv_skew_mat, eye], axis=2) return gs.concatenate([jacobian_block_line_1, jacobian_block_line_2], axis=1) @geomstats.vectorization.decorator(['else', 'vector']) def exp_from_identity(self, tangent_vec): """Compute group exponential of the tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[..., 3] Returns ------- group_exp: array-like, shape=[..., 3] The group exponential of the tangent vectors calculated at the identity. """ rotations = self.rotations dim_rotations = rotations.dim rot_vec = tangent_vec[..., :dim_rotations] rot_vec = self.rotations.regularize(rot_vec) translation = tangent_vec[..., dim_rotations:] angle = gs.linalg.norm(rot_vec, axis=-1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) skew_mat = self.rotations.skew_matrix_from_vector(rot_vec) sq_skew_mat = gs.matmul(skew_mat, skew_mat) mask_0 = gs.equal(angle, 0.) mask_close_0 = gs.isclose(angle, 0.) & ~mask_0 mask_else = ~mask_0 & ~mask_close_0 mask_0_float = gs.cast(mask_0, gs.float32) mask_close_0_float = gs.cast(mask_close_0, gs.float32) mask_else_float = gs.cast(mask_else, gs.float32) angle += mask_0_float * gs.ones_like(angle) coef_1 = gs.zeros_like(angle) coef_2 = gs.zeros_like(angle) coef_1 += mask_0_float * 1. / 2. * gs.ones_like(angle) coef_2 += mask_0_float * 1. / 6. * gs.ones_like(angle) coef_1 += mask_close_0_float * (TAYLOR_COEFFS_1_AT_0[0] + TAYLOR_COEFFS_1_AT_0[2] * angle**2 + TAYLOR_COEFFS_1_AT_0[4] * angle**4 + TAYLOR_COEFFS_1_AT_0[6] * angle**6) coef_2 += mask_close_0_float * (TAYLOR_COEFFS_2_AT_0[0] + TAYLOR_COEFFS_2_AT_0[2] * angle**2 + TAYLOR_COEFFS_2_AT_0[4] * angle**4 + TAYLOR_COEFFS_2_AT_0[6] * angle**6) coef_1 += mask_else_float * ((1. - gs.cos(angle)) / angle**2) coef_2 += mask_else_float * ((angle - gs.sin(angle)) / angle**3) n_tangent_vecs, _ = tangent_vec.shape exp_translation = gs.zeros((n_tangent_vecs, self.n)) for i in range(n_tangent_vecs): translation_i = translation[i] term_1_i = coef_1[i] * gs.dot(translation_i, gs.transpose(skew_mat[i])) term_2_i = coef_2[i] * gs.dot(translation_i, gs.transpose(sq_skew_mat[i])) mask_i_float = gs.get_mask_i_float(i, n_tangent_vecs) exp_translation += gs.outer(mask_i_float, translation_i + term_1_i + term_2_i) group_exp = gs.concatenate([rot_vec, exp_translation], axis=1) group_exp = self.regularize(group_exp) return group_exp @geomstats.vectorization.decorator(['else', 'vector']) def log_from_identity(self, point): """Compute the group logarithm of the point at the identity. Parameters ---------- point: array-like, shape=[..., 3] Returns ------- group_log: array-like, shape=[..., 3] the group logarithm in the Lie algbra """ point = self.regularize(point) rotations = self.rotations dim_rotations = rotations.dim rot_vec = point[:, :dim_rotations] angle = gs.linalg.norm(rot_vec, axis=1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) translation = point[:, dim_rotations:] skew_rot_vec = rotations.skew_matrix_from_vector(rot_vec) sq_skew_rot_vec = gs.matmul(skew_rot_vec, skew_rot_vec) mask_close_0 = gs.isclose(angle, 0.) mask_close_pi = gs.isclose(angle, gs.pi) mask_else = ~mask_close_0 & ~mask_close_pi mask_close_0_float = gs.cast(mask_close_0, gs.float32) mask_close_pi_float = gs.cast(mask_close_pi, gs.float32) mask_else_float = gs.cast(mask_else, gs.float32) mask_0 = gs.isclose(angle, 0., atol=1e-6) mask_0_float = gs.cast(mask_0, gs.float32) angle += mask_0_float * gs.ones_like(angle) coef_1 = -0.5 * gs.ones_like(angle) coef_2 = gs.zeros_like(angle) coef_2 += mask_close_0_float * (1. / 12. + angle**2 / 720. + angle**4 / 30240. + angle**6 / 1209600.) delta_angle = angle - gs.pi coef_2 += mask_close_pi_float * ( 1. / PI2 + (PI2 - 8.) * delta_angle / (4. * PI3) - ((PI2 - 12.) * delta_angle**2 / (4. * PI4)) + ((-192. + 12. * PI2 + PI4) * delta_angle**3 / (48. * PI5)) - ((-240. + 12. * PI2 + PI4) * delta_angle**4 / (48. * PI6)) + ((-2880. + 120. * PI2 + 10. * PI4 + PI6) * delta_angle**5 / (480. * PI7)) - ((-3360 + 120. * PI2 + 10. * PI4 + PI6) * delta_angle**6 / (480. * PI8))) psi = 0.5 * angle * gs.sin(angle) / (1 - gs.cos(angle)) coef_2 += mask_else_float * (1 - psi) / (angle**2) n_points, _ = point.shape log_translation = gs.zeros((n_points, self.n)) for i in range(n_points): translation_i = translation[i] term_1_i = coef_1[i] * gs.dot(translation_i, gs.transpose(skew_rot_vec[i])) term_2_i = coef_2[i] * gs.dot(translation_i, gs.transpose(sq_skew_rot_vec[i])) mask_i_float = gs.get_mask_i_float(i, n_points) log_translation += gs.outer(mask_i_float, translation_i + term_1_i + term_2_i) return gs.concatenate([rot_vec, log_translation], axis=1) def random_uniform(self, n_samples=1): """Sample in SE(3) with the uniform distribution. Parameters ---------- n_samples : int, optional default : 1 Returns ------- random_point : array-like, shape=[..., 3] An array of random elements in SE(3) having the given. """ random_translation = self.translations.random_uniform(n_samples) random_rot_vec = self.rotations.random_uniform(n_samples) return gs.concatenate([random_rot_vec, random_translation], axis=-1) def _exponential_matrix(self, rot_vec): """Compute exponential of rotation matrix represented by rot_vec. Parameters ---------- rot_vec : array-like, shape=[..., 3] Returns ------- exponential_mat : The matrix exponential of rot_vec """ # TODO(nguigs): find usecase for this method rot_vec = self.rotations.regularize(rot_vec) n_rot_vecs, _ = rot_vec.shape angle = gs.linalg.norm(rot_vec, axis=1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) skew_rot_vec = self.rotations.skew_matrix_from_vector(rot_vec) coef_1 = gs.empty_like(angle) coef_2 = gs.empty_like(coef_1) mask_0 = gs.equal(angle, 0) mask_0 = gs.squeeze(mask_0, axis=1) mask_close_to_0 = gs.isclose(angle, 0) mask_close_to_0 = gs.squeeze(mask_close_to_0, axis=1) mask_else = ~mask_0 & ~mask_close_to_0 coef_1[mask_close_to_0] = (1. / 2. - angle[mask_close_to_0]**2 / 24.) coef_2[mask_close_to_0] = (1. / 6. - angle[mask_close_to_0]**3 / 120.) # TODO(nina): Check if the discontinuity at 0 is expected. coef_1[mask_0] = 0 coef_2[mask_0] = 0 coef_1[mask_else] = (angle[mask_else]**(-2) * (1. - gs.cos(angle[mask_else]))) coef_2[mask_else] = (angle[mask_else]**(-2) * (1. - (gs.sin(angle[mask_else]) / angle[mask_else]))) term_1 = gs.zeros((n_rot_vecs, self.n, self.n)) term_2 = gs.zeros_like(term_1) for i in range(n_rot_vecs): term_1[i] = gs.eye(self.n) + skew_rot_vec[i] * coef_1[i] term_2[i] = gs.matmul(skew_rot_vec[i], skew_rot_vec[i]) * coef_2[i] exponential_mat = term_1 + term_2 return exponential_mat
class TestSpecialOrthogonal(geomstats.tests.TestCase): def setUp(self): self.n = 2 self.group = SpecialOrthogonal(n=self.n) self.n_samples = 4 def test_dim(self): for n in [2, 3, 4, 5, 6]: group = SpecialOrthogonal(n=n) result = group.dim expected = n * (n - 1) / 2 self.assertAllClose(result, expected) def test_belongs(self): theta = gs.pi / 3 point_1 = gs.array([[gs.cos(theta), - gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_1) self.assertTrue(result) point_2 = gs.array([[gs.cos(theta), gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_2) self.assertFalse(result) point = gs.array([point_1, point_2]) expected = gs.array([True, False]) result = self.group.belongs(point) self.assertAllClose(result, expected) point = point_1[0] result = self.group.belongs(point) self.assertFalse(result) point = gs.zeros((2, 3)) result = self.group.belongs(point) self.assertFalse(result) point = gs.zeros((2, 2, 3)) result = self.group.belongs(point) self.assertFalse(gs.all(result)) def test_random_uniform_and_belongs(self): point = self.group.random_uniform() result = self.group.belongs(point) expected = True self.assertAllClose(result, expected) point = self.group.random_uniform(self.n_samples) result = self.group.belongs(point) expected = gs.array([True] * self.n_samples) self.assertAllClose(result, expected) def test_identity(self): result = self.group.identity expected = gs.eye(self.n) self.assertAllClose(result, expected) def test_is_in_lie_algebra(self): theta = gs.pi / 3 vec_1 = gs.array([[0., - theta], [theta, 0.]]) result = self.group.is_tangent(vec_1) self.assertTrue(result) vec_2 = gs.array([[0., - theta], [theta, 1.]]) result = self.group.is_tangent(vec_2) self.assertFalse(result) vec = gs.array([vec_1, vec_2]) result = self.group.is_tangent(vec) expected = gs.array([True, False]) self.assertAllClose(result, expected) def test_is_tangent(self): point = self.group.random_uniform() theta = 1. vec_1 = gs.array([[0., - theta], [theta, 0.]]) vec_1 = self.group.compose(point, vec_1) result = self.group.is_tangent(vec_1, point) self.assertTrue(result) vec_2 = gs.array([[0., - theta], [theta, 1.]]) vec_2 = self.group.compose(point, vec_2) result = self.group.is_tangent(vec_2, point) self.assertFalse(result) vec = gs.array([vec_1, vec_2]) point = gs.array([point, point]) expected = gs.array([True, False]) result = self.group.is_tangent(vec, point) self.assertAllClose(result, expected) def test_to_tangent(self): theta = 1. vec_1 = gs.array([[0., - theta], [theta, 0.]]) result = self.group.to_tangent(vec_1) expected = vec_1 self.assertAllClose(result, expected) n_samples = self.n_samples base_points = self.group.random_uniform(n_samples=n_samples) tangent_vecs = self.group.compose(base_points, vec_1) result = self.group.to_tangent(tangent_vecs, base_points) expected = tangent_vecs self.assertAllClose(result, expected) def test_projection_and_belongs(self): gs.random.seed(4) shape = (self.n_samples, self.n, self.n) result = helper.test_projection_and_belongs( self.group, shape, gs.atol * 100) for res in result: self.assertTrue(res) def test_skew_to_vec_and_back(self): group = SpecialOrthogonal(n=4) vec = gs.random.rand(group.dim) mat = group.skew_matrix_from_vector(vec) result = group.vector_from_skew_matrix(mat) self.assertAllClose(result, vec) def test_parallel_transport(self): metric = self.group.bi_invariant_metric shape = (self.n_samples, self.group.n, self.group.n) results = helper.test_parallel_transport(self.group, metric, shape) for res in results: self.assertTrue(res) def test_metric_left_invariant(self): group = self.group point = group.random_point() tangent_vec = self.group.lie_algebra.basis[0] expected = group.bi_invariant_metric.norm( tangent_vec) translated = group.tangent_translation_map(point)(tangent_vec) result = group.bi_invariant_metric.norm(translated) self.assertAllClose(result, expected) @geomstats.tests.np_and_tf_only def test_distance_broadcast(self): group = self.group point = group.random_point(5) result = group.bi_invariant_metric.dist_broadcast(point[:3], point) expected = [] for a in point[:3]: expected.append(group.bi_invariant_metric.dist(a, point)) expected = gs.stack(expected) self.assertAllClose(result, expected)
class TestSpecialOrthogonal2(geomstats.tests.TestCase): def setup_method(self): warnings.simplefilter("ignore", category=ImportWarning) warnings.simplefilter("ignore", category=UserWarning) gs.random.seed(1234) self.group = SpecialOrthogonal(n=2, point_type="vector") # -- Set attributes self.n_samples = 4 def test_projection(self): # Test 2D and nD cases rot_mat = gs.eye(2) delta = 1e-12 * gs.ones((2, 2)) rot_mat_plus_delta = rot_mat + delta result = self.group.projection(rot_mat_plus_delta) expected = rot_mat self.assertAllClose(result, expected) def test_projection_vectorization(self): n_samples = self.n_samples mats = gs.ones((n_samples, 2, 2)) result = self.group.projection(mats) self.assertAllClose(gs.shape(result), (n_samples, 2, 2)) def test_skew_matrix_from_vector(self): rot_vec = gs.array([0.9]) skew_matrix = self.group.skew_matrix_from_vector(rot_vec) result = gs.matmul(skew_matrix, skew_matrix) diag = gs.array([-0.81, -0.81]) expected = algebra_utils.from_vector_to_diagonal_matrix(diag) self.assertAllClose(result, expected) def test_skew_matrix_and_vector(self): rot_vec = gs.array([0.8]) skew_mat = self.group.skew_matrix_from_vector(rot_vec) result = self.group.vector_from_skew_matrix(skew_mat) expected = rot_vec self.assertAllClose(result, expected) def test_skew_matrix_from_vector_vectorization(self): n_samples = self.n_samples rot_vecs = self.group.random_uniform(n_samples=n_samples) result = self.group.skew_matrix_from_vector(rot_vecs) self.assertAllClose(gs.shape(result), (n_samples, 2, 2)) def test_random_uniform_shape(self): result = self.group.random_uniform() self.assertAllClose(gs.shape(result), (self.group.dim, )) def test_random_and_belongs(self): point = self.group.random_uniform() result = self.group.belongs(point) expected = True self.assertAllClose(result, expected) def test_random_and_belongs_vectorization(self): n_samples = self.n_samples points = self.group.random_uniform(n_samples=n_samples) result = self.group.belongs(points) expected = gs.array([True] * n_samples) self.assertAllClose(result, expected) def test_regularize(self): angle = 2 * gs.pi + 1 result = self.group.regularize(gs.array([angle])) expected = gs.array([1.0]) self.assertAllClose(result, expected) def test_regularize_vectorization(self): n_samples = self.n_samples rot_vecs = self.group.random_uniform(n_samples=n_samples) result = self.group.regularize(rot_vecs) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) def test_matrix_from_rotation_vector(self): angle = gs.pi / 3 expected = gs.array([[1.0 / 2, -gs.sqrt(3.0) / 2], [gs.sqrt(3.0) / 2, 1.0 / 2]]) result = self.group.matrix_from_rotation_vector(gs.array([angle])) self.assertAllClose(result, expected) def test_matrix_from_rotation_vector_vectorization(self): n_samples = self.n_samples rot_vecs = self.group.random_uniform(n_samples=n_samples) rot_mats = self.group.matrix_from_rotation_vector(rot_vecs) self.assertAllClose(gs.shape(rot_mats), (n_samples, self.group.n, self.group.n)) def test_rotation_vector_from_matrix(self): angle = 0.12 rot_mat = gs.array([[gs.cos(angle), -gs.sin(angle)], [gs.sin(angle), gs.cos(angle)]]) result = self.group.rotation_vector_from_matrix(rot_mat) expected = gs.array([0.12]) self.assertAllClose(result, expected) def test_rotation_vector_and_rotation_matrix(self): """ This tests that the composition of rotation_vector_from_matrix and matrix_from_rotation_vector is the identity. """ # TODO(nguigs): bring back a 1d representation of SO2 point = gs.array([0.78]) rot_mat = self.group.matrix_from_rotation_vector(point) result = self.group.rotation_vector_from_matrix(rot_mat) expected = point self.assertAllClose(result, expected) def test_rotation_vector_and_rotation_matrix_vectorization(self): rot_vecs = gs.array([[2.0], [1.3], [0.8], [0.03]]) rot_mats = self.group.matrix_from_rotation_vector(rot_vecs) result = self.group.rotation_vector_from_matrix(rot_mats) expected = self.group.regularize(rot_vecs) self.assertAllClose(result, expected) def test_compose(self): point_a = gs.array([0.12]) point_b = gs.array([-0.15]) result = self.group.compose(point_a, point_b) expected = self.group.regularize(gs.array([-0.03])) self.assertAllClose(result, expected) def test_compose_and_inverse(self): angle = 0.986 point = gs.array([angle]) inv_point = self.group.inverse(point) result = self.group.compose(point, inv_point) expected = self.group.identity self.assertAllClose(result, expected) result = self.group.compose(inv_point, point) expected = self.group.identity self.assertAllClose(result, expected) def test_compose_vectorization(self): point_type = "vector" self.group.default_point_type = point_type n_samples = self.n_samples n_points_a = self.group.random_uniform(n_samples=n_samples) n_points_b = self.group.random_uniform(n_samples=n_samples) one_point = self.group.random_uniform(n_samples=1) result = self.group.compose(one_point, n_points_a) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) result = self.group.compose(n_points_a, one_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) result = self.group.compose(n_points_a, n_points_b) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) def test_inverse_vectorization(self): n_samples = self.n_samples points = self.group.random_uniform(n_samples=n_samples) result = self.group.inverse(points) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) def test_group_exp(self): """ The Riemannian exp and log are inverse functions of each other. This test is the inverse of test_log's. """ rot_vec_base_point = gs.array([gs.pi / 5]) rot_vec = gs.array([2 * gs.pi / 5]) expected = gs.array([3 * gs.pi / 5]) result = self.group.exp(base_point=rot_vec_base_point, tangent_vec=rot_vec) self.assertAllClose(result, expected) def test_group_exp_vectorization(self): n_samples = self.n_samples one_tangent_vec = self.group.random_uniform(n_samples=1) one_base_point = self.group.random_uniform(n_samples=1) n_tangent_vec = self.group.random_uniform(n_samples=n_samples) n_base_point = self.group.random_uniform(n_samples=n_samples) # Test with the 1 base point, and n tangent vecs result = self.group.exp(n_tangent_vec, one_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) # Test with the several base point, and one tangent vec result = self.group.exp(one_tangent_vec, n_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) # Test with the same number n of base point and n tangent vec result = self.group.exp(n_tangent_vec, n_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) def test_group_log(self): """ The Riemannian exp and log are inverse functions of each other. This test is the inverse of test_exp's. """ rot_vec_base_point = gs.array([gs.pi / 5]) rot_vec = gs.array([2 * gs.pi / 5]) expected = gs.array([1 * gs.pi / 5]) result = self.group.log(point=rot_vec, base_point=rot_vec_base_point) self.assertAllClose(result, expected) def test_group_log_vectorization(self): n_samples = self.n_samples one_point = self.group.random_uniform(n_samples=1) one_base_point = self.group.random_uniform(n_samples=1) n_point = self.group.random_uniform(n_samples=n_samples) n_base_point = self.group.random_uniform(n_samples=n_samples) # Test with the 1 base point, and several different points result = self.group.log(n_point, one_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) # Test with the several base point, and 1 point result = self.group.log(one_point, n_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) # Test with the same number n of base point and point result = self.group.log(n_point, n_base_point) self.assertAllClose(gs.shape(result), (n_samples, self.group.dim)) def test_group_exp_then_log_from_identity(self): """ Test that the group exponential and the group logarithm are inverse. Expect their composition to give the identity function. """ tangent_vec = gs.array([0.12]) result = helper.group_exp_then_log_from_identity( group=self.group, tangent_vec=tangent_vec) expected = self.group.regularize(tangent_vec) self.assertAllClose(result, expected) def test_group_log_then_exp_from_identity(self): """ Test that the group exponential and the group logarithm are inverse. Expect their composition to give the identity function. """ point = gs.array([0.12]) result = helper.group_log_then_exp_from_identity(group=self.group, point=point) expected = self.group.regularize(point) self.assertAllClose(result, expected) def test_group_exp_then_log(self): """ This tests that the composition of log and exp gives identity. """ base_point = gs.array([0.12]) tangent_vec = gs.array([0.35]) result = helper.group_exp_then_log(group=self.group, tangent_vec=tangent_vec, base_point=base_point) expected = self.group.regularize_tangent_vec(tangent_vec=tangent_vec, base_point=base_point) self.assertAllClose(result, expected) def test_group_log_then_exp(self): """ This tests that the composition of log and exp gives identity. """ base_point = gs.array([0.12]) point = gs.array([0.35]) result = helper.group_log_then_exp(group=self.group, point=point, base_point=base_point) expected = self.group.regularize(point) self.assertAllClose(result, expected)
class TestSpecialOrthogonal(geomstats.tests.TestCase): def setUp(self): self.n = 2 self.group = SpecialOrthogonal(n=self.n) self.n_samples = 4 def test_belongs(self): theta = gs.pi / 3 point_1 = gs.array([[gs.cos(theta), -gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_1) expected = True self.assertAllClose(result, expected) point_2 = gs.array([[gs.cos(theta), gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_2) expected = False self.assertAllClose(result, expected) point = gs.array([point_1, point_2]) expected = gs.array([True, False]) result = self.group.belongs(point) self.assertAllClose(result, expected) def test_random_uniform_and_belongs(self): point = self.group.random_uniform() result = self.group.belongs(point) expected = True self.assertAllClose(result, expected) point = self.group.random_uniform(self.n_samples) result = self.group.belongs(point) expected = gs.array([True] * self.n_samples) self.assertAllClose(result, expected) def test_identity(self): result = self.group.identity expected = gs.eye(self.n) self.assertAllClose(result, expected) def test_is_in_lie_algebra(self): theta = gs.pi / 3 vec_1 = gs.array([[0., -theta], [theta, 0.]]) result = self.group.is_tangent(vec_1) expected = True self.assertAllClose(result, expected) vec_2 = gs.array([[0., -theta], [theta, 1.]]) result = self.group.is_tangent(vec_2) expected = False self.assertAllClose(result, expected) vec = gs.array([vec_1, vec_2]) expected = gs.array([True, False]) result = self.group.is_tangent(vec) self.assertAllClose(result, expected) def test_is_tangent(self): point = self.group.random_uniform() theta = 1. vec_1 = gs.array([[0., -theta], [theta, 0.]]) vec_1 = self.group.compose(point, vec_1) result = self.group.is_tangent(vec_1, point) expected = True self.assertAllClose(result, expected) vec_2 = gs.array([[0., -theta], [theta, 1.]]) vec_2 = self.group.compose(point, vec_2) result = self.group.is_tangent(vec_2, point) expected = False self.assertAllClose(result, expected) vec = gs.array([vec_1, vec_2]) point = gs.array([point, point]) expected = gs.array([True, False]) result = self.group.is_tangent(vec, point) self.assertAllClose(result, expected) def test_to_tangent(self): theta = 1. vec_1 = gs.array([[0., -theta], [theta, 0.]]) result = self.group.to_tangent(vec_1) expected = vec_1 self.assertAllClose(result, expected) n_samples = self.n_samples base_points = self.group.random_uniform(n_samples=n_samples) tangent_vecs = self.group.compose(base_points, vec_1) result = self.group.to_tangent(tangent_vecs, base_points) expected = tangent_vecs self.assertAllClose(result, expected) def test_projection_and_belongs(self): gs.random.seed(3) group = SpecialOrthogonal(n=4) mat = gs.random.rand(4, 4) point = group.projection(mat) result = group.belongs(point) self.assertTrue(result) mat = gs.random.rand(2, 4, 4) point = group.projection(mat) result = group.belongs(point, atol=1e-4) self.assertTrue(gs.all(result)) def test_skew_to_vec_and_back(self): group = SpecialOrthogonal(n=4) vec = gs.random.rand(group.dim) mat = group.skew_matrix_from_vector(vec) result = group.vector_from_skew_matrix(mat) self.assertAllClose(result, vec)
class TestExponentialBarycenter(geomstats.tests.TestCase): def setUp(self): logger = logging.getLogger() logger.disabled = True self.se_mat = SpecialEuclidean(n=3) self.so_vec = SpecialOrthogonal(n=3, point_type='vector') self.so = SpecialOrthogonal(n=3) self.n_samples = 4 @geomstats.tests.np_only def test_estimate_and_belongs_se(self): point = self.se_mat.random_point(self.n_samples) estimator = ExponentialBarycenter(self.se_mat) estimator.fit(point) barexp = estimator.estimate_ result = self.se_mat.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) def test_estimate_one_sample_se(self): point = self.se_mat.random_point() estimator = ExponentialBarycenter(self.se_mat) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_reach_max_iter_se(self): point = self.se_mat.random_point(1) estimator = ExponentialBarycenter(self.se_mat, max_iter=2) points = gs.array([point, point]) estimator.fit(points) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec, max_iter=2) points = gs.array([point, point]) estimator.fit(points) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_so_matrix(self): points = self.so.random_uniform(2) mean_vec = ExponentialBarycenter(group=self.so) mean_vec.fit(points) logs = self.so.log(points, mean_vec.estimate_) result = gs.sum(logs, axis=0) expected = gs.zeros_like(points[0]) self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_belongs_so(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so) estimator.fit(point) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_one_sample_so(self): point = self.so.random_uniform(1) estimator = ExponentialBarycenter(self.so) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_reach_max_iter_so(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, max_iter=2) estimator.fit(point) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec, max_iter=2) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_only def test_coincides_with_frechet_so(self): gs.random.seed(0) point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, max_iter=40, epsilon=1e-10) estimator.fit(point) result = estimator.estimate_ frechet_estimator = FrechetMean(self.so.bi_invariant_metric, max_iter=40, epsilon=1e-10, lr=1., method='adaptive') frechet_estimator.fit(point) expected = frechet_estimator.estimate_ self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_weights(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, verbose=True) weights = gs.arange(self.n_samples) estimator.fit(point, weights=weights) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point, weights=weights) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) def test_linear_mean(self): euclidean = Euclidean(3) point = euclidean.random_point(self.n_samples) estimator = ExponentialBarycenter(euclidean) estimator.fit(point) result = estimator.estimate_ expected = gs.mean(point, axis=0) self.assertAllClose(result, expected)
class TestSpecialOrthogonal(geomstats.tests.TestCase): def setUp(self): self.n = 2 self.group = SpecialOrthogonal(n=self.n) self.n_samples = 4 def test_belongs(self): theta = gs.pi / 3 point_1 = gs.array([[gs.cos(theta), -gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_1) expected = True self.assertAllClose(result, expected) point_2 = gs.array([[gs.cos(theta), gs.sin(theta)], [gs.sin(theta), gs.cos(theta)]]) result = self.group.belongs(point_2) expected = False self.assertAllClose(result, expected) point = gs.array([point_1, point_2]) expected = gs.array([True, False]) result = self.group.belongs(point) self.assertAllClose(result, expected) def test_random_uniform_and_belongs(self): point = self.group.random_uniform() result = self.group.belongs(point) expected = True self.assertAllClose(result, expected) point = self.group.random_uniform(self.n_samples) result = self.group.belongs(point) expected = gs.array([True] * self.n_samples) self.assertAllClose(result, expected) def test_identity(self): result = self.group.identity expected = gs.eye(self.n) self.assertAllClose(result, expected) def test_is_in_lie_algebra(self): theta = gs.pi / 3 vec_1 = gs.array([[0., -theta], [theta, 0.]]) result = self.group.is_tangent(vec_1) expected = True self.assertAllClose(result, expected) vec_2 = gs.array([[0., -theta], [theta, 1.]]) result = self.group.is_tangent(vec_2) expected = False self.assertAllClose(result, expected) vec = gs.array([vec_1, vec_2]) expected = gs.array([True, False]) result = self.group.is_tangent(vec) self.assertAllClose(result, expected) def test_is_tangent(self): point = self.group.random_uniform() theta = 1. vec_1 = gs.array([[0., -theta], [theta, 0.]]) vec_1 = self.group.compose(point, vec_1) result = self.group.is_tangent(vec_1, point, atol=1e-6) expected = True self.assertAllClose(result, expected) vec_2 = gs.array([[0., -theta], [theta, 1.]]) vec_2 = self.group.compose(point, vec_2) result = self.group.is_tangent(vec_2, point, atol=1e-6) expected = False self.assertAllClose(result, expected) vec = gs.array([vec_1, vec_2]) point = gs.array([point, point]) expected = gs.array([True, False]) result = self.group.is_tangent(vec, point, atol=1e-6) self.assertAllClose(result, expected) def test_to_tangent(self): theta = 1. vec_1 = gs.array([[0., -theta], [theta, 0.]]) result = self.group.to_tangent(vec_1) expected = vec_1 self.assertAllClose(result, expected) n_samples = self.n_samples base_points = self.group.random_uniform(n_samples=n_samples) tangent_vecs = self.group.compose(base_points, vec_1) result = self.group.to_tangent(tangent_vecs, base_points) expected = tangent_vecs self.assertAllClose(result, expected)
class SpecialEuclidean(LieGroup): """Class for the special euclidean group SE(n). i.e. the Lie group of rigid transformations. Elements of SE(n) can either be represented as vectors (in 3d) or as matrices in general. The matrix representation corresponds to homogeneous coordinates. """ def __init__(self, n, default_point_type=None, epsilon=0.): """Initiate an object of class SpecialEuclidean. Parameter --------- n : int the dimension of the euclidean space that SE(n) acts upon point_type : str, {'vector', 'matrix'}, optional whether to represent elmenents of SE(n) by vectors or matrices if None is given, point_type is set to 'vector' for dimension 3 and 'matrix' otherwise epsilon : float, optional precision to use for calculations involving potential division by rotations default: 0 """ if not (isinstance(n, int) and n > 1): raise ValueError('n must be an integer > 1.') self.n = n self.dimension = int((n * (n - 1)) / 2 + n) self.epsilon = epsilon super(SpecialEuclidean, self).__init__(dim=self.dimension, default_point_type=default_point_type) if default_point_type is None: self.default_point_type = 'vector' if n == 3 else 'matrix' self.rotations = SpecialOrthogonal( n=n, epsilon=epsilon, default_point_type=default_point_type) self.translations = Euclidean(dim=n) def get_identity(self, point_type=None): """Get the identity of the group. Parameters ---------- point_type : str, {'vector', 'matrix'}, optional the point_type of the returned value default: self.default_point_type Returns ------- identity : array-like, shape={[dim], [n + 1, n + 1]} """ if point_type is None: point_type = self.default_point_type identity = gs.zeros(self.dimension) if point_type == 'matrix': identity = gs.eye(self.n + 1) return identity identity = property(get_identity) def get_point_type_shape(self, point_type=None): """Get the shape of the instance given the default_point_style.""" return self.get_identity(point_type).shape @geomstats.vectorization.decorator(['else', 'point', 'point_type']) def belongs(self, point, point_type=None): """Evaluate if a point belongs to SE(n). Parameters ---------- point : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] the point of which to check whether it belongs to SE(n) point_type : str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- belongs : array-like, shape=[n_samples, 1] array of booleans indicating whether point belongs to SE(n) """ if point_type == 'vector': n_points, vec_dim = gs.shape(point) belongs = vec_dim == self.dimension belongs = gs.tile([belongs], (point.shape[0], )) belongs = gs.logical_and(belongs, self.rotations.belongs(point[:, :self.n])) return gs.flatten(belongs) if point_type == 'matrix': n_points, point_dim1, point_dim2 = point.shape belongs = (point_dim1 == point_dim2 == self.n + 1) belongs = [belongs] * n_points rotation = point[:, :self.n, :self.n] rot_belongs = self.rotations.belongs(rotation, point_type=point_type) belongs = gs.logical_and(belongs, rot_belongs) last_line_except_last_term = point[:, self.n:, :-1] all_but_last_zeros = ~gs.any(last_line_except_last_term, axis=(1, 2)) belongs = gs.logical_and(belongs, all_but_last_zeros) last_term = point[:, self.n:, self.n:] belongs = gs.logical_and(belongs, gs.all(last_term == 1, axis=(1, 2))) return gs.flatten(belongs) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'point', 'point_type']) def regularize(self, point, point_type=None): """Regularize a point to the default representation for SE(n). Parameters ---------- point : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] the point which should be regularized point_type : str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- point : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] """ if point_type == 'vector': rotations = self.rotations dim_rotations = rotations.dim rot_vec = point[:, :dim_rotations] regularized_rot_vec = rotations.regularize(rot_vec, point_type=point_type) translation = point[:, dim_rotations:] return gs.concatenate([regularized_rot_vec, translation], axis=1) if point_type == 'matrix': return gs.to_ndarray(point, to_ndim=3) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'point', 'else', 'point_type']) def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None, point_type=None): """Regularize a tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] metric : RiemannianMetric, optional point_type : str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- regularized_vec : the regularized tangent vector """ if point_type == 'vector': return self.regularize_tangent_vec(tangent_vec, self.identity, metric, point_type=point_type) if point_type == 'matrix': translation_mask = gs.hstack( [gs.ones((self.n, ) * 2), 2 * gs.ones((self.n, 1))]) translation_mask = gs.concatenate( [translation_mask, gs.zeros((1, self.n + 1))], axis=0) tangent_vec = tangent_vec * gs.where(translation_mask != 0., gs.array(1.), gs.array(0.)) tangent_vec = (tangent_vec - GeneralLinear.transpose(tangent_vec)) / 2. return tangent_vec * translation_mask raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator( ['else', 'point', 'point', 'else', 'point_type']) def regularize_tangent_vec(self, tangent_vec, base_point, metric=None, point_type=None): """Regularize a tangent vector at a base point. Parameters ---------- tangent_vec: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] base_point : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] metric : RiemannianMetric, optional default: self.left_canonical_metric point_type: str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- regularized_vec : the regularized tangent vector """ if metric is None: metric = self.left_canonical_metric if point_type == 'vector': rotations = self.rotations dim_rotations = rotations.dim rot_tangent_vec = tangent_vec[:, :dim_rotations] rot_base_point = base_point[:, :dim_rotations] metric_mat = metric.inner_product_mat_at_identity rot_metric_mat = metric_mat[:dim_rotations, :dim_rotations] rot_metric = InvariantMetric( group=rotations, inner_product_mat_at_identity=rot_metric_mat, left_or_right=metric.left_or_right) rotations_vec = rotations.regularize_tangent_vec( tangent_vec=rot_tangent_vec, base_point=rot_base_point, metric=rot_metric, point_type=point_type) return gs.concatenate( [rotations_vec, tangent_vec[:, dim_rotations:]], axis=1) if point_type == 'matrix': tangent_vec_at_id = self.compose(self.inverse(base_point), tangent_vec) regularized = self.regularize_tangent_vec_at_identity( tangent_vec_at_id, point_type=point_type) return self.compose(base_point, regularized) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'vector']) def matrix_from_vector(self, vec): """Convert point in vector point-type to matrix. Parameters ---------- vec: array-like, shape=[n_samples, dim] Returns ------- mat: array-like, shape=[n_samples, {dim, [n+1, n+1]}] """ vec = self.regularize(vec, point_type='vector') n_vecs, _ = vec.shape rot_vec = vec[:, :self.rotations.dim] trans_vec = vec[:, self.rotations.dim:] rot_mat = self.rotations.matrix_from_rotation_vector(rot_vec) trans_vec = gs.reshape(trans_vec, (n_vecs, self.n, 1)) mat = gs.concatenate((rot_mat, trans_vec), axis=2) last_lines = gs.array(gs.get_mask_i_float(self.n, self.n + 1)) last_lines = gs.to_ndarray(last_lines, to_ndim=2) last_lines = gs.to_ndarray(last_lines, to_ndim=3) mat = gs.concatenate((mat, last_lines), axis=1) return mat @geomstats.vectorization.decorator( ['else', 'point', 'point', 'point_type']) 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\'.') @geomstats.vectorization.decorator(['else', 'point', 'point_type']) def inverse(self, point, point_type=None): r"""Compute the group inverse in SE(n). Parameters ---------- point: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] point_type: str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- inverse_point : array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] the inverted point Notes ----- :math:`(R, t)^{-1} = (R^{-1}, R^{-1}.(-t))` """ rotations = self.rotations dim_rotations = rotations.dim point = self.regularize(point) if point_type == 'vector': 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, point_type=point_type) if point_type == 'matrix': inv_rot = gs.transpose(point[:, :self.n, :self.n], axes=(0, 2, 1)) inv_trans = gs.matmul(inv_rot, -point[:, :self.n, self.n:]) last_line = point[:, self.n:, :] inverse_point = gs.concatenate((inv_rot, inv_trans), axis=2) return gs.concatenate((inverse_point, last_line), axis=1) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'point', 'else', 'point_type']) def jacobian_translation(self, point, left_or_right='left', point_type=None): """Compute the Jacobian matrix resulting from translation. Compute the matrix of the differential of the left/right translations from the identity to point in SE(n). Parameters ---------- point: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] left_or_right: str, {'left', 'right'}, optional default: 'left' whether to compute the jacobian of the left or right translation point_type : str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- jacobian : array-like, shape=[n_samples, dim] The jacobian of the left / right translation """ if point_type is None: point_type = self.default_point_type if left_or_right not in ('left', 'right'): raise ValueError('`left_or_right` must be `left` or `right`.') rotations = self.rotations translations = self.translations dim_rotations = rotations.dim dim_translations = translations.dim point = self.regularize(point, point_type=point_type) if point_type == 'vector': n_points, _ = point.shape rot_vec = point[:, :dim_rotations] jacobian_rot = self.rotations.jacobian_translation( point=rot_vec, left_or_right=left_or_right, point_type=point_type) block_zeros_1 = gs.zeros( (n_points, dim_rotations, dim_translations)) jacobian_block_line_1 = gs.concatenate( [jacobian_rot, block_zeros_1], axis=2) if left_or_right == 'left': rot_mat = self.rotations.matrix_from_rotation_vector(rot_vec) jacobian_trans = rot_mat block_zeros_2 = gs.zeros( (n_points, dim_translations, dim_rotations)) jacobian_block_line_2 = gs.concatenate( [block_zeros_2, jacobian_trans], axis=2) else: inv_skew_mat = -self.rotations.skew_matrix_from_vector(rot_vec) eye = gs.to_ndarray(gs.eye(self.n), to_ndim=3) eye = gs.tile(eye, [n_points, 1, 1]) jacobian_block_line_2 = gs.concatenate([inv_skew_mat, eye], axis=2) return gs.concatenate( [jacobian_block_line_1, jacobian_block_line_2], axis=1) if point_type == 'matrix': return point raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'point', 'point_type']) def exp_from_identity(self, tangent_vec, point_type=None): """Compute group exponential of the tangent vector at the identity. Parameters ---------- tangent_vec: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] point_type: str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- group_exp: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] the group exponential of the tangent vectors calculated at the identity """ if point_type == 'vector': rotations = self.rotations dim_rotations = rotations.dim rot_vec = tangent_vec[:, :dim_rotations] rot_vec = self.rotations.regularize(rot_vec, point_type=point_type) translation = tangent_vec[:, dim_rotations:] angle = gs.linalg.norm(rot_vec, axis=1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) skew_mat = self.rotations.skew_matrix_from_vector(rot_vec) sq_skew_mat = gs.matmul(skew_mat, skew_mat) mask_0 = gs.equal(angle, 0.) mask_close_0 = gs.isclose(angle, 0.) & ~mask_0 mask_else = ~mask_0 & ~mask_close_0 mask_0_float = gs.cast(mask_0, gs.float32) mask_close_0_float = gs.cast(mask_close_0, gs.float32) mask_else_float = gs.cast(mask_else, gs.float32) angle += mask_0_float * gs.ones_like(angle) coef_1 = gs.zeros_like(angle) coef_2 = gs.zeros_like(angle) coef_1 += mask_0_float * 1. / 2. * gs.ones_like(angle) coef_2 += mask_0_float * 1. / 6. * gs.ones_like(angle) coef_1 += mask_close_0_float * ( TAYLOR_COEFFS_1_AT_0[0] + TAYLOR_COEFFS_1_AT_0[2] * angle**2 + TAYLOR_COEFFS_1_AT_0[4] * angle**4 + TAYLOR_COEFFS_1_AT_0[6] * angle**6) coef_2 += mask_close_0_float * ( TAYLOR_COEFFS_2_AT_0[0] + TAYLOR_COEFFS_2_AT_0[2] * angle**2 + TAYLOR_COEFFS_2_AT_0[4] * angle**4 + TAYLOR_COEFFS_2_AT_0[6] * angle**6) coef_1 += mask_else_float * ((1. - gs.cos(angle)) / angle**2) coef_2 += mask_else_float * ((angle - gs.sin(angle)) / angle**3) n_tangent_vecs, _ = tangent_vec.shape exp_translation = gs.zeros((n_tangent_vecs, self.n)) for i in range(n_tangent_vecs): translation_i = translation[i] term_1_i = coef_1[i] * gs.dot(translation_i, gs.transpose(skew_mat[i])) term_2_i = coef_2[i] * gs.dot(translation_i, gs.transpose(sq_skew_mat[i])) mask_i_float = gs.get_mask_i_float(i, n_tangent_vecs) exp_translation += gs.outer( mask_i_float, translation_i + term_1_i + term_2_i) group_exp = gs.concatenate([rot_vec, exp_translation], axis=1) group_exp = self.regularize(group_exp, point_type=point_type) return group_exp if point_type == 'matrix': return GeneralLinear.exp(tangent_vec) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') @geomstats.vectorization.decorator(['else', 'point', 'point_type']) def log_from_identity(self, point, point_type=None): """Compute the group logarithm of the point at the identity. Parameters ---------- point: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] point_type: str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- group_log: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] the group logarithm in the Lie algbra """ point = self.regularize(point, point_type=point_type) rotations = self.rotations dim_rotations = rotations.dim if point_type == 'vector': rot_vec = point[:, :dim_rotations] angle = gs.linalg.norm(rot_vec, axis=1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) translation = point[:, dim_rotations:] skew_rot_vec = rotations.skew_matrix_from_vector(rot_vec) sq_skew_rot_vec = gs.matmul(skew_rot_vec, skew_rot_vec) mask_close_0 = gs.isclose(angle, 0.) mask_close_pi = gs.isclose(angle, gs.pi) mask_else = ~mask_close_0 & ~mask_close_pi mask_close_0_float = gs.cast(mask_close_0, gs.float32) mask_close_pi_float = gs.cast(mask_close_pi, gs.float32) mask_else_float = gs.cast(mask_else, gs.float32) mask_0 = gs.isclose(angle, 0., atol=1e-6) mask_0_float = gs.cast(mask_0, gs.float32) angle += mask_0_float * gs.ones_like(angle) coef_1 = -0.5 * gs.ones_like(angle) coef_2 = gs.zeros_like(angle) coef_2 += mask_close_0_float * (1. / 12. + angle**2 / 720. + angle**4 / 30240. + angle**6 / 1209600.) delta_angle = angle - gs.pi coef_2 += mask_close_pi_float * ( 1. / PI2 + (PI2 - 8.) * delta_angle / (4. * PI3) - ((PI2 - 12.) * delta_angle**2 / (4. * PI4)) + ((-192. + 12. * PI2 + PI4) * delta_angle**3 / (48. * PI5)) - ((-240. + 12. * PI2 + PI4) * delta_angle**4 / (48. * PI6)) + ((-2880. + 120. * PI2 + 10. * PI4 + PI6) * delta_angle**5 / (480. * PI7)) - ((-3360 + 120. * PI2 + 10. * PI4 + PI6) * delta_angle**6 / (480. * PI8))) psi = 0.5 * angle * gs.sin(angle) / (1 - gs.cos(angle)) coef_2 += mask_else_float * (1 - psi) / (angle**2) n_points, _ = point.shape log_translation = gs.zeros((n_points, self.n)) for i in range(n_points): translation_i = translation[i] term_1_i = coef_1[i] * gs.dot(translation_i, gs.transpose(skew_rot_vec[i])) term_2_i = coef_2[i] * gs.dot(translation_i, gs.transpose(sq_skew_rot_vec[i])) mask_i_float = gs.get_mask_i_float(i, n_points) log_translation += gs.outer( mask_i_float, translation_i + term_1_i + term_2_i) return gs.concatenate([rot_vec, log_translation], axis=1) if point_type == 'matrix': return GeneralLinear.log(point) raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') def random_uniform(self, n_samples=1, point_type=None): """Sample in SE(n) with the uniform distribution. Parameters ---------- n_samples: int, optional default: 1 point_type: str, {'vector', 'matrix'}, optional default: self.default_point_type Returns ------- random_point: array-like, shape=[n_samples, {dim, [n + 1, n + 1]}] An array of random elements in SE(n) having the given point_type. """ if point_type is None: point_type = self.default_point_type random_translation = self.translations.random_uniform(n_samples) if point_type == 'vector': random_rot_vec = self.rotations.random_uniform( n_samples, point_type=point_type) return gs.concatenate([random_rot_vec, random_translation], axis=-1) if point_type == 'matrix': random_rotation = self.rotations.random_uniform( n_samples, point_type=point_type) random_rotation = gs.to_ndarray(random_rotation, to_ndim=3) random_translation = gs.to_ndarray(random_translation, to_ndim=2) random_translation = gs.transpose( gs.to_ndarray(random_translation, to_ndim=3, axis=1), (0, 2, 1)) random_point = gs.concatenate( (random_rotation, random_translation), axis=2) last_line = gs.zeros((n_samples, 1, self.n + 1)) random_point = gs.concatenate((random_point, last_line), axis=1) random_point = gs.assignment(random_point, 1, (-1, -1), axis=0) if gs.shape(random_point)[0] == 1: random_point = gs.squeeze(random_point, axis=0) return random_point raise ValueError('Invalid point_type, expected \'vector\' or ' '\'matrix\'.') def _exponential_matrix(self, rot_vec): """Compute exponential of rotation matrix represented by rot_vec. Parameters ---------- rot_vec : array-like, shape=[n_samples, dim] Returns ------- exponential_mat : The matrix exponential of rot_vec """ # TODO(nguigs): find usecase for this method rot_vec = self.rotations.regularize(rot_vec) n_rot_vecs, _ = rot_vec.shape angle = gs.linalg.norm(rot_vec, axis=1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) skew_rot_vec = self.rotations.skew_matrix_from_vector(rot_vec) coef_1 = gs.empty_like(angle) coef_2 = gs.empty_like(coef_1) mask_0 = gs.equal(angle, 0) mask_0 = gs.squeeze(mask_0, axis=1) mask_close_to_0 = gs.isclose(angle, 0) mask_close_to_0 = gs.squeeze(mask_close_to_0, axis=1) mask_else = ~mask_0 & ~mask_close_to_0 coef_1[mask_close_to_0] = (1. / 2. - angle[mask_close_to_0]**2 / 24.) coef_2[mask_close_to_0] = (1. / 6. - angle[mask_close_to_0]**3 / 120.) # TODO(nina): Check if the discontinuity at 0 is expected. coef_1[mask_0] = 0 coef_2[mask_0] = 0 coef_1[mask_else] = (angle[mask_else]**(-2) * (1. - gs.cos(angle[mask_else]))) coef_2[mask_else] = (angle[mask_else]**(-2) * (1. - (gs.sin(angle[mask_else]) / angle[mask_else]))) term_1 = gs.zeros((n_rot_vecs, self.n, self.n)) term_2 = gs.zeros_like(term_1) for i in range(n_rot_vecs): term_1[i] = gs.eye(self.n) + skew_rot_vec[i] * coef_1[i] term_2[i] = gs.matmul(skew_rot_vec[i], skew_rot_vec[i]) * coef_2[i] exponential_mat = term_1 + term_2 return exponential_mat
class TestExponentialBarycenter(geomstats.tests.TestCase): def setUp(self): self.se_mat = SpecialEuclidean(n=3, default_point_type='matrix') self.so_vec = SpecialOrthogonal(n=3, default_point_type='vector') self.so = SpecialOrthogonal(n=3, default_point_type='matrix') self.n_samples = 3 @geomstats.tests.np_only def test_estimate_and_belongs_se(self): point = self.se_mat.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.se_mat) estimator.fit(point) barexp = estimator.estimate_ result = self.se_mat.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) def test_estimate_one_sample_se(self): point = self.se_mat.random_uniform(1) estimator = ExponentialBarycenter(self.se_mat) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_reach_max_iter_se(self): point = self.se_mat.random_uniform(1) estimator = ExponentialBarycenter(self.se_mat, max_iter=2) points = gs.array([point, point]) estimator.fit(points) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec, max_iter=2) points = gs.array([point, point]) estimator.fit(points) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_belongs_so(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so) estimator.fit(point) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_one_sample_so(self): point = self.so.random_uniform(1) estimator = ExponentialBarycenter(self.so) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) point = self.so_vec.random_uniform(1) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point) result = estimator.estimate_ expected = point self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_and_reach_max_iter_so(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, max_iter=2) estimator.fit(point) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec, max_iter=2) estimator.fit(point) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) @geomstats.tests.np_only def test_coincides_with_frechet_so(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, max_iter=32, epsilon=1e-12) estimator.fit(point) result = estimator.estimate_ print(self.so.default_point_type) so_vector = SpecialOrthogonal(3, default_point_type='vector') frechet_estimator = FrechetMean(so_vector.bi_invariant_metric, max_iter=32, epsilon=1e-10, point_type='vector') vector_point = so_vector.rotation_vector_from_matrix(point) frechet_estimator.fit(vector_point) mean = frechet_estimator.estimate_ expected = so_vector.matrix_from_rotation_vector(mean) result = estimator.estimate_ self.assertAllClose(result, expected) @geomstats.tests.np_only def test_estimate_weights(self): point = self.so.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so, verbose=True) weights = gs.arange(self.n_samples) estimator.fit(point, weights=weights) barexp = estimator.estimate_ result = self.so.belongs(barexp) expected = True self.assertAllClose(result, expected) point = self.so_vec.random_uniform(self.n_samples) estimator = ExponentialBarycenter(self.so_vec) estimator.fit(point, weights=weights) barexp = estimator.estimate_ result = self.so_vec.belongs(barexp) expected = True self.assertAllClose(result, expected) def test_linear_mean(self): euclidean = Euclidean(3) point = euclidean.random_uniform(self.n_samples) estimator = ExponentialBarycenter(euclidean) estimator.fit(point) result = estimator.estimate_ expected = gs.mean(point, axis=0) self.assertAllClose(result, expected)
class _SpecialEuclideanMatrices(GeneralLinear, LieGroup): """Class for special Euclidean group. Parameters ---------- n : int Integer dimension of the underlying Euclidean space. Matrices will be of size: (n+1) x (n+1). """ def __init__(self, n): super().__init__( n=n + 1, dim=int((n * (n + 1)) / 2), default_point_type='matrix', lie_algebra=SpecialEuclideanMatrixLieAlgebra(n=n)) self.rotations = SpecialOrthogonal(n=n) self.translations = Euclidean(dim=n) self.n = n self.left_canonical_metric = \ SpecialEuclideanMatrixCannonicalLeftMetric(group=self) def get_identity(self): """Return the identity matrix.""" return gs.eye(self.n + 1, self.n + 1) identity = property(get_identity) def belongs(self, point): """Check whether point is of the form rotation, translation. Parameters ---------- point : array-like, shape=[..., n, n]. Point to be checked. Returns ------- belongs : array-like, shape=[...,] Boolean denoting if point belongs to the group. """ point_dim1, point_dim2 = point.shape[-2:] belongs = (point_dim1 == point_dim2 == self.n + 1) rotation = point[..., :self.n, :self.n] rot_belongs = self.rotations.belongs(rotation) belongs = gs.logical_and(belongs, rot_belongs) last_line_except_last_term = point[..., self.n:, :-1] all_but_last_zeros = ~ gs.any( last_line_except_last_term, axis=(-2, -1)) belongs = gs.logical_and(belongs, all_but_last_zeros) last_term = point[..., self.n, self.n] belongs = gs.logical_and(belongs, gs.isclose(last_term, 1.)) if point.ndim == 2: return gs.squeeze(belongs) return gs.flatten(belongs) def random_uniform(self, n_samples=1, tol=1e-6): """Sample in SE(n) from the uniform distribution. Parameters ---------- n_samples : int Number of samples. Optional, default: 1. tol : unused Returns ------- samples : array-like, shape=[..., n + 1, n + 1] Sample in SE(n). """ random_translation = self.translations.random_uniform(n_samples) random_rotation = self.rotations.random_uniform(n_samples) output_shape = ( (n_samples, self.n + 1, self.n + 1) if n_samples != 1 else (self.n + 1, ) * 2) random_point = homogeneous_representation( random_rotation, random_translation, output_shape) return random_point @classmethod def inverse(cls, point): """Return the inverse of a point. Parameters ---------- point : array-like, shape=[..., n, n] Point to be inverted. """ n = point.shape[-1] - 1 transposed_rot = cls.transpose(point[..., :n, :n]) translation = point[..., :n, -1] translation = gs.einsum( '...ij,...j->...i', transposed_rot, translation) return homogeneous_representation( transposed_rot, -translation, point.shape)