class SpecialEuclideanGroup(LieGroup):
"""
Class for the special euclidean group SE(n),
i.e. the Lie group of rigid transformations.
"""
def __init__(self, n):
assert isinstance(n, int) and n > 1
self.n = n
self.dimension = int((n * (n - 1)) / 2 + n)
super(SpecialEuclideanGroup, self).__init__(
dimension=self.dimension,
identity=gs.zeros(self.dimension))
# TODO(nina): keep the names rotations and translations here?
self.rotations = SpecialOrthogonalGroup(n=n)
self.translations = EuclideanSpace(dimension=n)
self.point_representation = 'vector' if n == 3 else 'matrix'
def belongs(self, point):
"""
Evaluate if a point belongs to SE(n).
"""
point = gs.to_ndarray(point, to_ndim=2)
_, point_dim = point.shape
return point_dim == self.dimension
def regularize(self, point):
"""
Regularize a point to the canonical representation
chosen for SE(n).
"""
assert self.point_representation == 'vector'
point = gs.to_ndarray(point, to_ndim=2)
assert self.belongs(point)
rotations = self.rotations
dim_rotations = rotations.dimension
regularized_point = gs.zeros_like(point)
rot_vec = point[:, :dim_rotations]
regularized_point[:, :dim_rotations] = rotations.regularize(rot_vec)
regularized_point[:, dim_rotations:] = point[:, dim_rotations:]
return regularized_point
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None):
return self.regularize_tangent_vec(tangent_vec, self.identity, metric)
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
if metric is None:
metric = self.left_canonical_metric
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
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)
regularized_vec = gs.zeros_like(tangent_vec)
regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric)
regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:]
return regularized_vec
def compose(self, point_1, point_2):
"""
Compose two elements of SE(n).
Formula:
point_1 . point_2 = [R1 * R2, (R1 * t2) + t1]
where:
R1, R2 are rotation matrices,
t1, t2 are translation vectors.
"""
rotations = self.rotations
dim_rotations = rotations.dimension
point_1 = self.regularize(point_1)
point_2 = self.regularize(point_2)
n_points_1, _ = point_1.shape
n_points_2, _ = point_2.shape
assert (point_1.shape == point_2.shape
or n_points_1 == 1
or n_points_2 == 1)
rot_vec_1 = point_1[:, :dim_rotations]
rot_mat_1 = rotations.matrix_from_rotation_vector(rot_vec_1)
rot_mat_1 = so_group.closest_rotation_matrix(rot_mat_1)
rot_vec_2 = point_2[:, :dim_rotations]
rot_mat_2 = rotations.matrix_from_rotation_vector(rot_vec_2)
rot_mat_2 = so_group.closest_rotation_matrix(rot_mat_2)
translation_1 = point_1[:, dim_rotations:]
translation_2 = point_2[:, dim_rotations:]
n_compositions = gs.maximum(n_points_1, n_points_2)
composition_rot_mat = gs.matmul(rot_mat_1, rot_mat_2)
composition_rot_vec = rotations.rotation_vector_from_matrix(
composition_rot_mat)
composition_translation = gs.zeros((n_compositions, self.n))
for i in range(n_compositions):
translation_1_i = (translation_1[0] if n_points_1 == 1
else translation_1[i])
rot_mat_1_i = (rot_mat_1[0] if n_points_1 == 1
else rot_mat_1[i])
translation_2_i = (translation_2[0] if n_points_2 == 1
else translation_2[i])
composition_translation[i] = (gs.dot(translation_2_i,
gs.transpose(rot_mat_1_i))
+ translation_1_i)
composition = gs.zeros((n_compositions, self.dimension))
composition[:, :dim_rotations] = composition_rot_vec
composition[:, dim_rotations:] = composition_translation
composition = self.regularize(composition)
return composition
def inverse(self, point):
"""
Compute the group inverse in SE(n).
Formula:
(R, t)^{-1} = (R^{-1}, R^{-1}.(-t))
"""
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
translation = point[:, dim_rotations:]
inverse_point = gs.zeros_like(point)
inverse_rotation = -rot_vec
inv_rot_mat = rotations.matrix_from_rotation_vector(inverse_rotation)
inverse_translation = gs.zeros((n_points, self.n))
for i in range(n_points):
inverse_translation[i] = gs.dot(-translation[i],
gs.transpose(inv_rot_mat[i]))
inverse_point[:, :dim_rotations] = inverse_rotation
inverse_point[:, dim_rotations:] = inverse_translation
inverse_point = self.regularize(inverse_point)
return inverse_point
def jacobian_translation(self, point, left_or_right='left'):
"""
Compute the jacobian matrix of the differential
of the left/right translations from the identity to point in SE(n).
"""
assert self.belongs(point)
assert left_or_right in ('left', 'right')
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
jacobian = gs.zeros((n_points,) + (dim,) * 2)
if left_or_right == 'left':
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='left')
jacobian_trans = self.rotations.matrix_from_rotation_vector(
rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, dim_rotations:] = jacobian_trans
else:
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='right')
inv_skew_mat = - so_group.skew_matrix_from_vector(rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, :dim_rotations] = inv_skew_mat
jacobian[:, dim_rotations:, dim_rotations:] = gs.eye(self.n)
assert jacobian.ndim == 3
return jacobian
def group_exp_from_identity(self, tangent_vec):
"""
Compute the group exponential of the tangent vector at the identity.
"""
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
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)
mask_close_pi = gs.isclose(angle, gs.pi)
mask_close_pi = gs.squeeze(mask_close_pi, axis=1)
rot_vec[mask_close_pi] = rotations.regularize(
rot_vec[mask_close_pi])
skew_mat = so_group.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_0 = gs.squeeze(mask_0, axis=1)
mask_close_0 = gs.squeeze(mask_close_0, axis=1)
mask_else = ~mask_0 & ~mask_close_0
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
coef_1[mask_0] = 1. / 2.
coef_2[mask_0] = 1. / 6.
coef_1[mask_close_0] = (1. / 2. - angle[mask_close_0] ** 2 / 24.
+ angle[mask_close_0] ** 4 / 720.
- angle[mask_close_0] ** 6 / 40320.)
coef_2[mask_close_0] = (1. / 6. - angle[mask_close_0] ** 2 / 120.
+ angle[mask_close_0] ** 4 / 5040.
- angle[mask_close_0] ** 6 / 362880.)
coef_1[mask_else] = ((1. - gs.cos(angle[mask_else]))
/ angle[mask_else] ** 2)
coef_2[mask_else] = ((angle[mask_else] - gs.sin(angle[mask_else]))
/ angle[mask_else] ** 3)
n_tangent_vecs, _ = tangent_vec.shape
group_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]))
group_exp_translation[i] = translation_i + term_1_i + term_2_i
group_exp = gs.zeros_like(tangent_vec)
group_exp[:, :dim_rotations] = rot_vec
group_exp[:, dim_rotations:] = group_exp_translation
group_exp = self.regularize(group_exp)
return group_exp
def group_log_from_identity(self, point):
"""
Compute the group logarithm of the point at the identity.
"""
assert self.belongs(point)
point = self.regularize(point)
rotations = self.rotations
dim_rotations = rotations.dimension
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:]
group_log = gs.zeros_like(point)
group_log[:, :dim_rotations] = rot_vec
skew_rot_vec = so_group.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_0 = gs.squeeze(mask_close_0, axis=1)
mask_close_pi = gs.isclose(angle, gs.pi)
mask_close_pi = gs.squeeze(mask_close_pi, axis=1)
mask_else = ~mask_close_0 & ~mask_close_pi
coef_1 = - 0.5 * gs.ones_like(angle)
coef_2 = gs.zeros_like(angle)
coef_2[mask_close_0] = (1. / 12. + angle[mask_close_0] ** 2 / 720.
+ angle[mask_close_0] ** 4 / 30240.
+ angle[mask_close_0] ** 6 / 1209600.)
delta_angle = angle[mask_close_pi] - gs.pi
coef_2[mask_close_pi] = (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[mask_else]
* gs.sin(angle[mask_else]) / (1 - gs.cos(angle[mask_else])))
coef_2[mask_else] = (1 - psi) / (angle[mask_else] ** 2)
n_points, _ = point.shape
group_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]))
group_log_translation[i] = translation_i + term_1_i + term_2_i
group_log[:, dim_rotations:] = group_log_translation
assert group_log.ndim == 2
return group_log
def random_uniform(self, n_samples=1):
"""
Sample in SE(n) with the uniform distribution.
"""
random_rot_vec = self.rotations.random_uniform(n_samples)
random_translation = self.translations.random_uniform(n_samples)
random_transfo = gs.concatenate([random_rot_vec, random_translation],
axis=1)
random_transfo = self.regularize(random_transfo)
return random_transfo
def exponential_matrix(self, rot_vec):
"""
Compute the exponential of the rotation matrix represented by rot_vec.
"""
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 = so_group.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 discountinuity as 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
assert exponential_mat.ndim == 3
return exponential_mat
def group_exponential_barycenter(self, points, weights=None):
"""
Compute the group exponential barycenter in SE(n).
"""
n_points = points.shape[0]
assert n_points > 0
if weights is None:
weights = gs.ones((n_points, 1))
weights = gs.to_ndarray(weights, to_ndim=2, axis=1)
n_weights, _ = weights.shape
assert n_points == n_weights
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
rotation_vectors = points[:, :dim_rotations]
translations = points[:, dim_rotations:dim]
assert rotation_vectors.shape == (n_points, dim_rotations)
assert translations.shape == (n_points, self.n)
mean_rotation = rotations.group_exponential_barycenter(
points=rotation_vectors,
weights=weights)
mean_rotation_mat = rotations.matrix_from_rotation_vector(
mean_rotation)
matrix = gs.zeros((1,) + (self.n,) * 2)
translation_aux = gs.zeros((1, self.n))
inv_rot_mats = rotations.matrix_from_rotation_vector(
-rotation_vectors)
# TODO(nina): this is the same mat multiplied several times
matrix_aux = gs.matmul(mean_rotation_mat, inv_rot_mats)
assert matrix_aux.shape == (n_points,) + (dim_rotations,) * 2
vec_aux = rotations.rotation_vector_from_matrix(matrix_aux)
matrix_aux = self.exponential_matrix(vec_aux)
matrix_aux = gs.linalg.inv(matrix_aux)
for i in range(n_points):
matrix += weights[i] * matrix_aux[i]
translation_aux += weights[i] * gs.dot(gs.matmul(
matrix_aux[i],
inv_rot_mats[i]),
translations[i])
mean_translation = gs.dot(translation_aux,
gs.transpose(gs.linalg.inv(matrix),
axes=(0, 2, 1)))
exp_bar = gs.zeros((1, dim))
exp_bar[0, :dim_rotations] = mean_rotation
exp_bar[0, dim_rotations:dim] = mean_translation
return exp_bar
class TestEuclideanSpaceMethods(unittest.TestCase):
_multiprocess_can_split_ = True
def setUp(self):
gs.random.seed(1234)
self.dimension = 2
self.space = EuclideanSpace(self.dimension)
self.metric = self.space.metric
self.n_samples = 10
def test_belongs(self):
point = self.space.random_uniform()
belongs = self.space.belongs(point)
gs.testing.assert_allclose(belongs.shape, (1, 1))
def test_random_uniform(self):
point = self.space.random_uniform()
gs.testing.assert_allclose(point.shape, (1, self.dimension))
def test_random_uniform_and_belongs(self):
point = self.space.random_uniform()
self.assertTrue(self.space.belongs(point))
def test_inner_product_matrix(self):
result = self.metric.inner_product_matrix()
expected = gs.eye(self.dimension)
expected = helper.to_matrix(expected)
gs.testing.assert_allclose(result, expected)
def test_inner_product(self):
point_a = gs.array([0, 1])
point_b = gs.array([2, 10])
result = self.metric.inner_product(point_a, point_b)
expected = gs.dot(point_a, point_b)
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
def test_inner_product_vectorization(self):
n_samples = self.n_samples
one_point_a = self.space.random_uniform(n_samples=1)
one_point_b = self.space.random_uniform(n_samples=1)
n_points_a = self.space.random_uniform(n_samples=n_samples)
n_points_b = self.space.random_uniform(n_samples=n_samples)
result = self.metric.inner_product(one_point_a, one_point_b)
expected = gs.dot(one_point_a, one_point_b.transpose())
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
result = self.metric.inner_product(n_points_a, one_point_b)
expected = gs.dot(n_points_a, one_point_b.transpose())
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
result = self.metric.inner_product(one_point_a, n_points_b)
expected = gs.dot(one_point_a, n_points_b.transpose()).transpose()
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
result = self.metric.inner_product(n_points_a, n_points_b)
expected = gs.zeros(n_samples)
for i in range(n_samples):
expected[i] = gs.dot(n_points_a[i], n_points_b[i])
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
def test_squared_norm(self):
point = gs.array([-2, 4])
result = self.metric.squared_norm(point)
expected = gs.linalg.norm(point) ** 2
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
def test_squared_norm_vectorization(self):
n_samples = self.n_samples
n_points = self.space.random_uniform(n_samples=n_samples)
result = self.metric.squared_norm(n_points)
expected = gs.linalg.norm(n_points, axis=-1) ** 2
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
def test_norm(self):
point = gs.array([-2, 4])
result = self.metric.norm(point)
expected = gs.linalg.norm(point)
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
def test_norm_vectorization(self):
n_samples = self.n_samples
n_points = self.space.random_uniform(n_samples=n_samples)
result = self.metric.norm(n_points)
expected = gs.linalg.norm(n_points, axis=1)
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
def test_exp(self):
base_point = gs.array([0, 1])
vector = gs.array([2, 10])
result = self.metric.exp(tangent_vec=vector,
base_point=base_point)
expected = base_point + vector
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
def test_exp_vectorization(self):
n_samples = self.n_samples
dim = self.dimension
one_tangent_vec = self.space.random_uniform(n_samples=1)
one_base_point = self.space.random_uniform(n_samples=1)
n_tangent_vecs = self.space.random_uniform(n_samples=n_samples)
n_base_points = self.space.random_uniform(n_samples=n_samples)
result = self.metric.exp(one_tangent_vec, one_base_point)
expected = one_tangent_vec + one_base_point
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
result = self.metric.exp(n_tangent_vecs, one_base_point)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
result = self.metric.exp(one_tangent_vec, n_base_points)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
result = self.metric.exp(n_tangent_vecs, n_base_points)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
def test_log(self):
base_point = gs.array([0, 1])
point = gs.array([2, 10])
result = self.metric.log(point=point, base_point=base_point)
expected = point - base_point
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
def test_log_vectorization(self):
n_samples = self.n_samples
dim = self.dimension
one_point = self.space.random_uniform(n_samples=1)
one_base_point = self.space.random_uniform(n_samples=1)
n_points = self.space.random_uniform(n_samples=n_samples)
n_base_points = self.space.random_uniform(n_samples=n_samples)
result = self.metric.log(one_point, one_base_point)
expected = one_point - one_base_point
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
result = self.metric.log(n_points, one_base_point)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
result = self.metric.log(one_point, n_base_points)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
result = self.metric.log(n_points, n_base_points)
gs.testing.assert_allclose(result.shape, (n_samples, dim))
def test_squared_dist(self):
point_a = gs.array([-1, 4])
point_b = gs.array([1, 1])
result = self.metric.squared_dist(point_a, point_b)
vec = point_b - point_a
expected = gs.dot(vec, vec)
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
def test_squared_dist_vectorization(self):
n_samples = self.n_samples
one_point_a = self.space.random_uniform(n_samples=1)
one_point_b = self.space.random_uniform(n_samples=1)
n_points_a = self.space.random_uniform(n_samples=n_samples)
n_points_b = self.space.random_uniform(n_samples=n_samples)
result = self.metric.squared_dist(one_point_a, one_point_b)
vec = one_point_a - one_point_b
expected = gs.dot(vec, vec.transpose())
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
result = self.metric.squared_dist(n_points_a, one_point_b)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
result = self.metric.squared_dist(one_point_a, n_points_b)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
result = self.metric.squared_dist(n_points_a, n_points_b)
expected = gs.zeros(n_samples)
for i in range(n_samples):
vec = n_points_a[i] - n_points_b[i]
expected[i] = gs.dot(vec, vec.transpose())
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
def test_dist(self):
point_a = gs.array([0, 1])
point_b = gs.array([2, 10])
result = self.metric.dist(point_a, point_b)
expected = gs.linalg.norm(point_b - point_a)
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
def test_dist_vectorization(self):
n_samples = self.n_samples
one_point_a = self.space.random_uniform(n_samples=1)
one_point_b = self.space.random_uniform(n_samples=1)
n_points_a = self.space.random_uniform(n_samples=n_samples)
n_points_b = self.space.random_uniform(n_samples=n_samples)
result = self.metric.dist(one_point_a, one_point_b)
vec = one_point_a - one_point_b
expected = gs.sqrt(gs.dot(vec, vec.transpose()))
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
result = self.metric.dist(n_points_a, one_point_b)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
result = self.metric.dist(one_point_a, n_points_b)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
result = self.metric.dist(n_points_a, n_points_b)
expected = gs.zeros(n_samples)
for i in range(n_samples):
vec = n_points_a[i] - n_points_b[i]
expected[i] = gs.sqrt(gs.dot(vec, vec.transpose()))
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result.shape, (n_samples, 1))
gs.testing.assert_allclose(result, expected)
def test_geodesic_and_belongs(self):
initial_point = self.space.random_uniform()
initial_tangent_vec = gs.array([2., 0.])
geodesic = self.metric.geodesic(
initial_point=initial_point,
initial_tangent_vec=initial_tangent_vec)
t = gs.linspace(start=0, stop=1, num=100)
points = geodesic(t)
self.assertTrue(gs.all(self.space.belongs(points)))
def test_mean(self):
point = gs.array([1, 4])
result = self.metric.mean(points=[point, point, point])
expected = point
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
points = gs.array([[1, 2],
[2, 3],
[3, 4],
[4, 5]])
weights = gs.array([1, 2, 1, 2])
result = self.metric.mean(points, weights)
expected = gs.array([16., 22.]) / 6.
expected = helper.to_vector(expected)
gs.testing.assert_allclose(result, expected)
def test_variance(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 = self.metric.variance(points, weights, base_point)
# we expect the average of the points' sq norms.
expected = (1 * 5. + 2 * 13. + 1 * 25. + 2 * 41.) / 6.
expected = helper.to_scalar(expected)
gs.testing.assert_allclose(result, expected)
class TestEuclideanSpaceMethods(geomstats.tests.TestCase):
_multiprocess_can_split_ = True
def setUp(self):
gs.random.seed(1234)
self.dimension = 2
self.space = EuclideanSpace(self.dimension)
self.metric = self.space.metric
self.n_samples = 3
self.one_point_a = gs.array([0., 1.])
self.one_point_b = gs.array([2., 10.])
self.n_points_a = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
self.n_points_b = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
def test_random_uniform_and_belongs(self):
point = self.space.random_uniform()
result = self.space.belongs(point)
expected = gs.array([[True]])
self.assertAllClose(result, expected)
def test_squared_norm_vectorization(self):
n_samples = self.n_samples
n_points = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
result = self.metric.squared_norm(n_points)
expected = gs.array([[5.], [20.], [26.]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
def test_norm_vectorization(self):
n_samples = self.n_samples
n_points = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
result = self.metric.norm(n_points)
expected = gs.array([[2.2360679775], [4.472135955], [5.09901951359]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
def test_exp_vectorization(self):
n_samples = self.n_samples
dim = self.dimension
one_tangent_vec = gs.array([0., 1.])
one_base_point = gs.array([2., 10.])
n_tangent_vecs = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
n_base_points = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.exp(one_tangent_vec, one_base_point)
expected = one_tangent_vec + one_base_point
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
result = self.metric.exp(n_tangent_vecs, one_base_point)
self.assertAllClose(gs.shape(result), (n_samples, dim))
result = self.metric.exp(one_tangent_vec, n_base_points)
self.assertAllClose(gs.shape(result), (n_samples, dim))
result = self.metric.exp(n_tangent_vecs, n_base_points)
self.assertAllClose(gs.shape(result), (n_samples, dim))
def test_log_vectorization(self):
n_samples = self.n_samples
dim = self.dimension
one_point = gs.array([0., 1.])
one_base_point = gs.array([2., 10.])
n_points = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
n_base_points = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.log(one_point, one_base_point)
expected = one_point - one_base_point
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
result = self.metric.log(n_points, one_base_point)
self.assertAllClose(gs.shape(result), (n_samples, dim))
result = self.metric.log(one_point, n_base_points)
self.assertAllClose(gs.shape(result), (n_samples, dim))
result = self.metric.log(n_points, n_base_points)
self.assertAllClose(gs.shape(result), (n_samples, dim))
def test_squared_dist_vectorization(self):
n_samples = self.n_samples
one_point_a = gs.array([0., 1.])
one_point_b = gs.array([2., 10.])
n_points_a = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
n_points_b = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.squared_dist(one_point_a, one_point_b)
vec = one_point_a - one_point_b
expected = gs.dot(vec, gs.transpose(vec))
expected = helper.to_scalar(expected)
self.assertAllClose(result, expected)
result = self.metric.squared_dist(n_points_a, one_point_b)
self.assertAllClose(gs.shape(result), (n_samples, 1))
result = self.metric.squared_dist(one_point_a, n_points_b)
self.assertAllClose(gs.shape(result), (n_samples, 1))
result = self.metric.squared_dist(n_points_a, n_points_b)
expected = gs.array([[81.], [109.], [29.]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
def test_dist_vectorization(self):
n_samples = self.n_samples
one_point_a = gs.array([0., 1.])
one_point_b = gs.array([2., 10.])
n_points_a = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
n_points_b = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.dist(one_point_a, one_point_b)
vec = one_point_a - one_point_b
expected = gs.sqrt(gs.dot(vec, gs.transpose(vec)))
expected = helper.to_scalar(expected)
self.assertAllClose(result, expected)
result = self.metric.dist(n_points_a, one_point_b)
self.assertAllClose(gs.shape(result), (n_samples, 1))
result = self.metric.dist(one_point_a, n_points_b)
self.assertAllClose(gs.shape(result), (n_samples, 1))
result = self.metric.dist(n_points_a, n_points_b)
expected = gs.array([[9.], [gs.sqrt(109.)], [gs.sqrt(29.)]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
def test_belongs(self):
point = gs.array([0., 1.])
result = self.space.belongs(point)
expected = gs.array([[True]])
self.assertAllClose(result, expected)
def test_random_uniform(self):
result = self.space.random_uniform()
self.assertAllClose(gs.shape(result), (1, self.dimension))
def test_inner_product_matrix(self):
result = self.metric.inner_product_matrix()
expected = gs.eye(self.dimension)
expected = helper.to_matrix(expected)
self.assertAllClose(result, expected)
def test_inner_product(self):
point_a = gs.array([0., 1.])
point_b = gs.array([2., 10.])
result = self.metric.inner_product(point_a, point_b)
expected = gs.array([[10.]])
self.assertAllClose(result, expected)
def test_inner_product_vectorization(self):
n_samples = 3
one_point_a = gs.array([0., 1.])
one_point_b = gs.array([2., 10.])
n_points_a = gs.array([[2., 1.], [-2., -4.], [-5., 1.]])
n_points_b = gs.array([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.inner_product(one_point_a, one_point_b)
expected = gs.array([[10.]])
self.assertAllClose(gs.shape(result), (1, 1))
self.assertAllClose(result, expected)
result = self.metric.inner_product(n_points_a, one_point_b)
expected = gs.array([[14.], [-44.], [0.]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
result = self.metric.inner_product(one_point_a, n_points_b)
expected = gs.array([[10.], [-1.], [6.]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
result = self.metric.inner_product(n_points_a, n_points_b)
expected = gs.array([[14.], [-12.], [21.]])
self.assertAllClose(gs.shape(result), (n_samples, 1))
self.assertAllClose(result, expected)
def test_squared_norm(self):
point = gs.array([-2., 4.])
result = self.metric.squared_norm(point)
expected = gs.array([[20.]])
self.assertAllClose(result, expected)
def test_norm(self):
point = gs.array([-2., 4.])
result = self.metric.norm(point)
expected = gs.array([[4.472135955]])
self.assertAllClose(result, expected)
def test_exp(self):
base_point = gs.array([0., 1.])
vector = gs.array([2., 10.])
result = self.metric.exp(tangent_vec=vector, base_point=base_point)
expected = base_point + vector
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
def test_log(self):
base_point = gs.array([0., 1.])
point = gs.array([2., 10.])
result = self.metric.log(point=point, base_point=base_point)
expected = point - base_point
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
def test_squared_dist(self):
point_a = gs.array([-1., 4.])
point_b = gs.array([1., 1.])
result = self.metric.squared_dist(point_a, point_b)
vec = point_b - point_a
expected = gs.dot(vec, vec)
expected = helper.to_scalar(expected)
self.assertAllClose(result, expected)
def test_dist(self):
point_a = gs.array([0., 1.])
point_b = gs.array([2., 10.])
result = self.metric.dist(point_a, point_b)
expected = gs.linalg.norm(point_b - point_a)
expected = helper.to_scalar(expected)
self.assertAllClose(result, expected)
def test_geodesic_and_belongs(self):
n_geodesic_points = 100
initial_point = gs.array([[2., -1.]])
initial_tangent_vec = gs.array([2., 0.])
geodesic = self.metric.geodesic(
initial_point=initial_point,
initial_tangent_vec=initial_tangent_vec)
t = gs.linspace(start=0., stop=1., num=n_geodesic_points)
points = geodesic(t)
result = self.space.belongs(points)
expected = gs.array(n_geodesic_points * [[True]])
self.assertAllClose(expected, result)
def test_mean(self):
point = gs.array([[1., 4.]])
result = self.metric.mean(points=[point, point, point])
expected = point
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
points = gs.array([[1., 2.], [2., 3.], [3., 4.], [4., 5.]])
weights = gs.array([1., 2., 1., 2.])
result = self.metric.mean(points, weights)
expected = gs.array([16. / 6., 22. / 6.])
expected = helper.to_vector(expected)
self.assertAllClose(result, expected)
def test_variance(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 = self.metric.variance(points, weights, base_point)
# we expect the average of the points' sq norms.
expected = (1 * 5. + 2 * 13. + 1 * 25. + 2 * 41.) / 6.
expected = helper.to_scalar(expected)
self.assertAllClose(result, expected)
class SpecialEuclideanGroup(LieGroup):
"""
Class for the special euclidean group SE(n),
i.e. the Lie group of rigid transformations.
"""
def __init__(self, n, point_type=None, epsilon=0.):
assert isinstance(n, int) and n > 1
self.n = n
self.dimension = int((n * (n - 1)) / 2 + n)
self.epsilon = epsilon
self.default_point_type = point_type
if point_type is None:
self.default_point_type = 'vector' if n == 3 else 'matrix'
super(SpecialEuclideanGroup, self).__init__(dimension=self.dimension)
self.rotations = SpecialOrthogonalGroup(n=n, epsilon=epsilon)
self.translations = EuclideanSpace(dimension=n)
def get_identity(self, point_type=None):
"""
Get the identity of the group,
as a vector if point_type == 'vector',
as a matrix if point_type == 'matrix'.
"""
if point_type is None:
point_type = self.default_point_type
identity = gs.zeros(self.dimension)
if self.default_point_type == 'matrix':
identity = gs.eye(self.n)
return identity
identity = property(get_identity)
def belongs(self, point, point_type=None):
"""
Evaluate if a point belongs to SE(n).
"""
if point_type is None:
point_type = self.default_point_type
if point_type == 'vector':
point = gs.to_ndarray(point, to_ndim=2)
n_points, point_dim = point.shape
belongs = point_dim == self.dimension
belongs = gs.to_ndarray(belongs, to_ndim=1)
belongs = gs.to_ndarray(belongs, to_ndim=2, axis=1)
belongs = gs.tile(belongs, (n_points, 1))
elif point_type == 'matrix':
point = gs.to_ndarray(point, to_ndim=3)
raise NotImplementedError()
return belongs
def regularize(self, point, point_type=None):
"""
Regularize a point to the canonical representation
chosen for SE(n).
"""
if point_type is None:
point_type = self.default_point_type
if point_type == 'vector':
point = gs.to_ndarray(point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_vec = point[:, :dim_rotations]
regularized_rot_vec = rotations.regularize(rot_vec,
point_type=point_type)
translation = point[:, dim_rotations:]
regularized_point = gs.concatenate(
[regularized_rot_vec, translation], axis=1)
elif point_type == 'matrix':
point = gs.to_ndarray(point, to_ndim=3)
regularized_point = gs.copy(point)
return regularized_point
def regularize_tangent_vec_at_identity(self,
tangent_vec,
metric=None,
point_type=None):
if point_type is None:
point_type = self.default_point_type
return self.regularize_tangent_vec(tangent_vec,
self.identity,
metric,
point_type=point_type)
def regularize_tangent_vec(self,
tangent_vec,
base_point,
metric=None,
point_type=None):
if point_type is None:
point_type = self.default_point_type
if metric is None:
metric = self.left_canonical_metric
if point_type == 'vector':
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
base_point = gs.to_ndarray(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
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)
regularized_vec = gs.zeros_like(tangent_vec)
rotations_vec = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric,
point_type=point_type)
regularized_vec = gs.concatenate(
[rotations_vec, tangent_vec[:, dim_rotations:]], axis=1)
elif point_type == 'matrix':
regularized_vec = tangent_vec
return regularized_vec
def compose(self, point_1, point_2, point_type=None):
"""
Compose two elements of SE(n).
Formula:
point_1 . point_2 = [R1 * R2, (R1 * t2) + t1]
where:
R1, R2 are rotation matrices,
t1, t2 are translation vectors.
"""
if point_type is None:
point_type = self.default_point_type
rotations = self.rotations
dim_rotations = rotations.dimension
point_1 = self.regularize(point_1, point_type=point_type)
point_2 = self.regularize(point_2, point_type=point_type)
if point_type == 'vector':
n_points_1, _ = point_1.shape
n_points_2, _ = point_2.shape
assert (point_1.shape == point_2.shape or n_points_1 == 1
or n_points_2 == 1)
if n_points_1 == 1:
point_1 = gs.stack([point_1[0]] * n_points_2)
if n_points_2 == 1:
point_2 = gs.stack([point_2[0]] * n_points_1)
rot_vec_1 = point_1[:, :dim_rotations]
rot_mat_1 = rotations.matrix_from_rotation_vector(rot_vec_1)
rot_vec_2 = point_2[:, :dim_rotations]
rot_mat_2 = rotations.matrix_from_rotation_vector(rot_vec_2)
translation_1 = point_1[:, dim_rotations:]
translation_2 = point_2[:, dim_rotations:]
composition_rot_mat = gs.matmul(rot_mat_1, rot_mat_2)
composition_rot_vec = rotations.rotation_vector_from_matrix(
composition_rot_mat)
composition_translation = gs.einsum('ij,ikj->ik', translation_2,
rot_mat_1) + translation_1
composition = gs.concatenate(
(composition_rot_vec, composition_translation), axis=1)
elif point_type == 'matrix':
raise NotImplementedError()
composition = self.regularize(composition, point_type=point_type)
return composition
def inverse(self, point, point_type=None):
"""
Compute the group inverse in SE(n).
Formula:
(R, t)^{-1} = (R^{-1}, R^{-1}.(-t))
"""
if point_type is None:
point_type = self.default_point_type
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
if point_type == 'vector':
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
translation = point[:, dim_rotations:]
inverse_point = gs.zeros_like(point)
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)
elif point_type == 'matrix':
raise NotImplementedError()
inverse_point = self.regularize(inverse_point, point_type=point_type)
return inverse_point
def jacobian_translation(self,
point,
left_or_right='left',
point_type=None):
"""
Compute the jacobian matrix of the differential
of the left/right translations from the identity to point in SE(n).
"""
if point_type is None:
point_type = self.default_point_type
assert left_or_right in ('left', 'right')
dim = self.dimension
rotations = self.rotations
translations = self.translations
dim_rotations = rotations.dimension
dim_translations = translations.dimension
point = self.regularize(point, point_type=point_type)
if point_type == 'vector':
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
jacobian = gs.zeros((n_points, ) + (dim, ) * 2)
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)
jacobian = gs.concatenate(
[jacobian_block_line_1, jacobian_block_line_2], axis=1)
assert gs.ndim(jacobian) == 3
elif point_type == 'matrix':
raise NotImplementedError()
return jacobian
def group_exp_from_identity(self, tangent_vec, point_type=None):
"""
Compute the group exponential of the tangent vector at the identity.
"""
if point_type is None:
point_type = self.default_point_type
if point_type == 'vector':
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
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
group_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)
group_exp_translation += mask_i_float * (translation_i +
term_1_i + term_2_i)
group_exp = gs.concatenate([rot_vec, group_exp_translation],
axis=1)
group_exp = self.regularize(group_exp, point_type=point_type)
return group_exp
elif point_type == 'matrix':
raise NotImplementedError()
def group_log_from_identity(self, point, point_type=None):
"""
Compute the group logarithm of the point at the identity.
"""
if point_type is None:
point_type = self.default_point_type
point = self.regularize(point, point_type=point_type)
rotations = self.rotations
dim_rotations = rotations.dimension
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
group_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)
group_log_translation += mask_i_float * (translation_i +
term_1_i + term_2_i)
group_log = gs.concatenate([rot_vec, group_log_translation],
axis=1)
assert gs.ndim(group_log) == 2
elif point_type == 'matrix':
raise NotImplementedError()
return group_log
def random_uniform(self, n_samples=1, point_type=None):
"""
Sample in SE(n) with the uniform distribution.
"""
if point_type is None:
point_type = self.default_point_type
random_rot_vec = self.rotations.random_uniform(n_samples,
point_type=point_type)
random_translation = self.translations.random_uniform(n_samples)
if point_type == 'vector':
random_transfo = gs.concatenate(
[random_rot_vec, random_translation], axis=1)
elif point_type == 'matrix':
raise NotImplementedError()
random_transfo = self.regularize(random_transfo, point_type=point_type)
return random_transfo
def exponential_matrix(self, rot_vec):
"""
Compute the exponential of the rotation matrix represented by rot_vec.
"""
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 discountinuity 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
assert exponential_mat.ndim == 3
return exponential_mat
def group_exponential_barycenter(self,
points,
weights=None,
point_type=None):
"""
Compute the group exponential barycenter in SE(n).
"""
if point_type is None:
point_type = self.default_point_type
n_points = points.shape[0]
assert n_points > 0
if weights is None:
weights = gs.ones((n_points, 1))
weights = gs.to_ndarray(weights, to_ndim=2, axis=1)
n_weights, _ = weights.shape
assert n_points == n_weights
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
if point_type == 'vector':
rotation_vectors = points[:, :dim_rotations]
translations = points[:, dim_rotations:dim]
assert rotation_vectors.shape == (n_points, dim_rotations)
assert translations.shape == (n_points, self.n)
mean_rotation = rotations.group_exponential_barycenter(
points=rotation_vectors, weights=weights)
mean_rotation_mat = rotations.matrix_from_rotation_vector(
mean_rotation)
matrix = gs.zeros((1, ) + (self.n, ) * 2)
translation_aux = gs.zeros((1, self.n))
inv_rot_mats = rotations.matrix_from_rotation_vector(
-rotation_vectors)
matrix_aux = gs.matmul(mean_rotation_mat, inv_rot_mats)
assert matrix_aux.shape == (n_points, ) + (dim_rotations, ) * 2
vec_aux = rotations.rotation_vector_from_matrix(matrix_aux)
matrix_aux = self.exponential_matrix(vec_aux)
matrix_aux = gs.linalg.inv(matrix_aux)
for i in range(n_points):
matrix += weights[i] * matrix_aux[i]
translation_aux += weights[i] * gs.dot(
gs.matmul(matrix_aux[i], inv_rot_mats[i]), translations[i])
mean_translation = gs.dot(
translation_aux,
gs.transpose(gs.linalg.inv(matrix), axes=(0, 2, 1)))
exp_bar = gs.zeros((1, dim))
exp_bar[0, :dim_rotations] = mean_rotation
exp_bar[0, dim_rotations:dim] = mean_translation
elif point_type == 'matrix':
vector_points = self.rotation_vector_from_matrix(points)
vector_exp_bar = self.group_exponential_barycenter(
vector_points, weights, point_type='vector')
exp_bar = self.matrix_from_rotation_vector(vector_exp_bar)
return exp_bar
class TestEuclideanSpaceMethodsTensorFlow(tf.test.TestCase):
_multiprocess_can_split_ = True
def setUp(self):
gs.random.seed(1234)
self.dimension = 2
self.space = EuclideanSpace(self.dimension)
self.metric = self.space.metric
self.n_samples = 3
@classmethod
def setUpClass(cls):
os.environ['GEOMSTATS_BACKEND'] = 'tensorflow'
importlib.reload(gs)
@classmethod
def tearDownClass(cls):
os.environ['GEOMSTATS_BACKEND'] = 'numpy'
importlib.reload(gs)
def test_belongs(self):
point = self.space.random_uniform()
belongs = self.space.belongs(point)
expected = tf.convert_to_tensor([[True]])
with self.test_session():
self.assertAllClose(gs.eval(belongs), gs.eval(expected))
def test_random_uniform(self):
point = self.space.random_uniform()
point_numpy = np.random.uniform(size=(1, self.dimension))
with self.test_session():
self.assertShapeEqual(point_numpy, point)
def test_inner_product_matrix(self):
result = self.metric.inner_product_matrix()
expected = gs.eye(self.dimension)
expected = helper.to_matrix(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_inner_product(self):
point_a = tf.convert_to_tensor([0., 1.])
point_b = tf.convert_to_tensor([2., 10.])
result = self.metric.inner_product(point_a, point_b)
expected = gs.dot(point_a, point_b)
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_inner_product_vectorization(self):
n_samples = 3
one_point_a = tf.convert_to_tensor([0., 1.])
one_point_b = tf.convert_to_tensor([2., 10.])
n_points_a = tf.convert_to_tensor([[2., 1.], [-2., -4.], [-5., 1.]])
n_points_b = tf.convert_to_tensor([[2., 10.], [8., -1.], [-3., 6.]])
result = self.metric.inner_product(one_point_a, one_point_b)
expected = gs.dot(one_point_a, gs.transpose(one_point_b))
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
result = self.metric.inner_product(n_points_a, one_point_b)
point_numpy = np.random.uniform(size=(n_samples, 1))
# TODO(nina): Fix this test with assertShapeEqual
with self.test_session():
self.assertAllClose(point_numpy.shape, gs.eval(gs.shape(result)))
result = self.metric.inner_product(one_point_a, n_points_b)
point_numpy = np.random.uniform(size=(n_samples, 1))
# TODO(nina): Fix this test with assertShapeEqual
with self.test_session():
self.assertAllClose(point_numpy.shape, gs.eval(gs.shape(result)))
result = self.metric.inner_product(n_points_a, n_points_b)
point_numpy = np.random.uniform(size=(n_samples, 1))
# TODO(nina): Fix this test with assertShapeEqual
with self.test_session():
self.assertAllClose(point_numpy.shape, gs.eval(gs.shape(result)))
def test_squared_norm(self):
point = tf.convert_to_tensor([-2., 4.])
result = self.metric.squared_norm(point)
expected = gs.linalg.norm(point)**2
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_norm(self):
point = tf.convert_to_tensor([-2., 4.])
result = self.metric.norm(point)
expected = gs.linalg.norm(point)
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_exp(self):
base_point = tf.convert_to_tensor([0., 1.])
vector = tf.convert_to_tensor([2., 10.])
result = self.metric.exp(tangent_vec=vector, base_point=base_point)
expected = base_point + vector
expected = helper.to_vector(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_log(self):
base_point = tf.convert_to_tensor([0., 1.])
point = tf.convert_to_tensor([2., 10.])
result = self.metric.log(point=point, base_point=base_point)
expected = point - base_point
expected = helper.to_vector(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_squared_dist(self):
point_a = tf.convert_to_tensor([-1., 4.])
point_b = tf.convert_to_tensor([1., 1.])
result = self.metric.squared_dist(point_a, point_b)
vec = point_b - point_a
expected = gs.dot(vec, vec)
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_dist(self):
point_a = tf.convert_to_tensor([0., 1.])
point_b = tf.convert_to_tensor([2., 10.])
result = self.metric.dist(point_a, point_b)
expected = gs.linalg.norm(point_b - point_a)
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_geodesic_and_belongs(self):
n_geodesic_points = 100
initial_point = self.space.random_uniform()
initial_tangent_vec = tf.convert_to_tensor([2., 0.])
geodesic = self.metric.geodesic(
initial_point=initial_point,
initial_tangent_vec=initial_tangent_vec)
t = gs.linspace(start=0., stop=1., num=n_geodesic_points)
points = geodesic(t)
bool_belongs = self.space.belongs(points)
expected = tf.convert_to_tensor(n_geodesic_points * [[True]])
with self.test_session():
self.assertAllClose(gs.eval(expected), gs.eval(bool_belongs))
def test_mean(self):
# TODO(nina): Fix the fact that it doesn't work for [1., 4.]
point = tf.convert_to_tensor([[1., 4.]])
result = self.metric.mean(points=[point, point, point])
expected = point
expected = helper.to_vector(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
points = tf.convert_to_tensor([[1., 2.], [2., 3.], [3., 4.], [4., 5.]])
weights = gs.array([1., 2., 1., 2.])
result = self.metric.mean(points, weights)
expected = tf.convert_to_tensor([16. / 6., 22. / 6.])
expected = helper.to_vector(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
def test_variance(self):
points = tf.convert_to_tensor([[1., 2.], [2., 3.], [3., 4.], [4., 5.]])
weights = tf.convert_to_tensor([1., 2., 1., 2.])
base_point = gs.zeros(2)
result = self.metric.variance(points, weights, base_point)
# we expect the average of the points' sq norms.
expected = (1 * 5. + 2 * 13. + 1 * 25. + 2 * 41.) / 6.
expected = helper.to_scalar(expected)
with self.test_session():
self.assertAllClose(gs.eval(result), gs.eval(expected))
class SpecialEuclideanGroup(LieGroup):
def __init__(self, n):
assert n > 1
if n is not 3:
raise NotImplementedError('Only SE(3) is implemented.')
self.n = n
self.dimension = int((n * (n - 1)) / 2 + n)
super(SpecialEuclideanGroup, self).__init__(
dimension=self.dimension,
identity=np.zeros(self.dimension))
# TODO(nina): keep the names rotations and translations here?
self.rotations = SpecialOrthogonalGroup(n=n)
self.translations = EuclideanSpace(dimension=n)
def belongs(self, point):
"""
Check that the transformation belongs to
the special euclidean group.
"""
point = vectorization.expand_dims(point, to_ndim=2)
_, point_dim = point.shape
return point_dim == self.dimension
def regularize(self, point):
"""
Regularize an element of the group SE(3),
by extracting the rotation vector r from the input [r t]
and using self.rotations.regularize.
:param point: 6d vector, element in SE(3) represented as [r t].
:returns self.regularized_point: 6d vector, element in SE(3)
with self.regularized rotation.
"""
point = vectorization.expand_dims(point, to_ndim=2)
assert self.belongs(point)
rotations = self.rotations
dim_rotations = rotations.dimension
regularized_point = np.zeros_like(point)
rot_vec = point[:, :dim_rotations]
regularized_point[:, :dim_rotations] = rotations.regularize(rot_vec)
regularized_point[:, dim_rotations:] = point[:, dim_rotations:]
return regularized_point
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None):
return self.regularize_tangent_vec(tangent_vec, self.identity, metric)
def regularize_tangent_vec(self, tangent_vec, base_point, metric=None):
"""
Regularize an element of the group SE(3),
by extracting the rotation vector r from the input [r t]
and using self.rotations.regularize.
:param point: 6d vector, element in SE(3) represented as [r t].
:returns self.regularized_point: 6d vector, element in SE(3)
with self.regularized rotation.
"""
if metric is None:
metric = self.left_canonical_metric
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
base_point = vectorization.expand_dims(base_point, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
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)
regularized_vec = np.zeros_like(tangent_vec)
regularized_vec[:, :dim_rotations] = rotations.regularize_tangent_vec(
tangent_vec=rot_tangent_vec,
base_point=rot_base_point,
metric=rot_metric)
regularized_vec[:, dim_rotations:] = tangent_vec[:, dim_rotations:]
return regularized_vec
def compose(self, point_1, point_2):
"""
Compose two elements of group SE(3).
Formula:
point_1 . point_2 = [R1 * R2, (R1 * t2) + t1]
where:
R1, R2 are rotation matrices,
t1, t2 are translation vectors.
:param point_1, point_2: 6d vectors elements of SE(3)
:return composition: composition of point_1 and point_2
"""
rotations = self.rotations
dim_rotations = rotations.dimension
point_1 = self.regularize(point_1)
point_2 = self.regularize(point_2)
n_points_1, _ = point_1.shape
n_points_2, _ = point_2.shape
assert (point_1.shape == point_2.shape
or n_points_1 == 1
or n_points_2 == 1)
rot_vec_1 = point_1[:, :dim_rotations]
rot_mat_1 = rotations.matrix_from_rotation_vector(rot_vec_1)
rot_mat_1 = so_group.closest_rotation_matrix(rot_mat_1)
rot_vec_2 = point_2[:, :dim_rotations]
rot_mat_2 = rotations.matrix_from_rotation_vector(rot_vec_2)
rot_mat_2 = so_group.closest_rotation_matrix(rot_mat_2)
translation_1 = point_1[:, dim_rotations:]
translation_2 = point_2[:, dim_rotations:]
n_compositions = np.maximum(n_points_1, n_points_2)
composition_rot_mat = np.matmul(rot_mat_1, rot_mat_2)
composition_rot_vec = rotations.rotation_vector_from_matrix(
composition_rot_mat)
composition_translation = np.zeros((n_compositions, self.n))
for i in range(n_compositions):
translation_1_i = (translation_1[0] if n_points_1 == 1
else translation_1[i])
rot_mat_1_i = (rot_mat_1[0] if n_points_1 == 1
else rot_mat_1[i])
translation_2_i = (translation_2[0] if n_points_2 == 1
else translation_2[i])
composition_translation[i] = (np.dot(translation_2_i,
np.transpose(rot_mat_1_i))
+ translation_1_i)
composition = np.zeros((n_compositions, self.dimension))
composition[:, :dim_rotations] = composition_rot_vec
composition[:, dim_rotations:] = composition_translation
composition = self.regularize(composition)
return composition
def inverse(self, point):
"""
Compute the group inverse in SE(3).
Formula:
(R, t)^{-1} = (R^{-1}, R^{-1}.(-t))
:param point: 6d vector element in SE(3)
:returns inverse_point: 6d vector inverse of point
"""
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
translation = point[:, dim_rotations:]
inverse_point = np.zeros_like(point)
inverse_rotation = -rot_vec
inv_rot_mat = rotations.matrix_from_rotation_vector(inverse_rotation)
inverse_translation = np.zeros((n_points, self.n))
for i in range(n_points):
inverse_translation[i] = np.dot(-translation[i],
np.transpose(inv_rot_mat[i]))
inverse_point[:, :dim_rotations] = inverse_rotation
inverse_point[:, dim_rotations:] = inverse_translation
inverse_point = self.regularize(inverse_point)
return inverse_point
def jacobian_translation(self, point, left_or_right='left'):
"""
Compute the jacobian matrix of the differential
of the left/right translations
from the identity to point in the Lie group SE(3).
:param point: 6D vector element of SE(3)
:returns jacobian: 6x6 matrix
"""
assert self.belongs(point)
assert left_or_right in ('left', 'right')
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
point = self.regularize(point)
n_points, _ = point.shape
rot_vec = point[:, :dim_rotations]
jacobian = np.zeros((n_points,) + (dim,) * 2)
if left_or_right == 'left':
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='left')
jacobian_trans = self.rotations.matrix_from_rotation_vector(
rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, dim_rotations:] = jacobian_trans
else:
jacobian_rot = self.rotations.jacobian_translation(
point=rot_vec,
left_or_right='right')
inv_skew_mat = - so_group.skew_matrix_from_vector(rot_vec)
jacobian[:, :dim_rotations, :dim_rotations] = jacobian_rot
jacobian[:, dim_rotations:, :dim_rotations] = inv_skew_mat
jacobian[:, dim_rotations:, dim_rotations:] = np.eye(self.n)
assert jacobian.ndim == 3
return jacobian
def group_exp_from_identity(self,
tangent_vec):
"""
Compute the group exponential of vector tangent_vector,
at point base_point.
:param tangent_vector: tangent vector of SE(3) at base_point.
:param base_point: 6d vector element of SE(3).
:returns group_exp: 6d vector element of SE(3).
"""
tangent_vec = vectorization.expand_dims(tangent_vec, to_ndim=2)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_vec = tangent_vec[:, :dim_rotations]
rot_vec = self.rotations.regularize(rot_vec)
translation = tangent_vec[:, dim_rotations:]
angle = np.linalg.norm(rot_vec, axis=1)
angle = vectorization.expand_dims(angle, to_ndim=2, axis=1)
mask_close_pi = np.isclose(angle, np.pi)
mask_close_pi = np.squeeze(mask_close_pi, axis=1)
rot_vec[mask_close_pi] = rotations.regularize(
rot_vec[mask_close_pi])
skew_mat = so_group.skew_matrix_from_vector(rot_vec)
sq_skew_mat = np.matmul(skew_mat, skew_mat)
mask_0 = np.equal(angle, 0)
mask_close_0 = np.isclose(angle, 0) & ~mask_0
mask_0 = np.squeeze(mask_0, axis=1)
mask_close_0 = np.squeeze(mask_close_0, axis=1)
mask_else = ~mask_0 & ~mask_close_0
coef_1 = np.zeros_like(angle)
coef_2 = np.zeros_like(angle)
coef_1[mask_0] = 1. / 2.
coef_2[mask_0] = 1. / 6.
coef_1[mask_close_0] = (1. / 2. - angle[mask_close_0] ** 2 / 24.
+ angle[mask_close_0] ** 4 / 720.
- angle[mask_close_0] ** 6 / 40320.)
coef_2[mask_close_0] = (1. / 6. - angle[mask_close_0] ** 2 / 120.
+ angle[mask_close_0] ** 4 / 5040.
- angle[mask_close_0] ** 6 / 362880.)
coef_1[mask_else] = ((1. - np.cos(angle[mask_else]))
/ angle[mask_else] ** 2)
coef_2[mask_else] = ((angle[mask_else] - np.sin(angle[mask_else]))
/ angle[mask_else] ** 3)
n_tangent_vecs, _ = tangent_vec.shape
group_exp_translation = np.zeros((n_tangent_vecs, self.n))
for i in range(n_tangent_vecs):
translation_i = translation[i]
term_1_i = coef_1[i] * np.dot(translation_i,
np.transpose(skew_mat[i]))
term_2_i = coef_2[i] * np.dot(translation_i,
np.transpose(sq_skew_mat[i]))
group_exp_translation[i] = translation_i + term_1_i + term_2_i
group_exp = np.zeros_like(tangent_vec)
group_exp[:, :dim_rotations] = rot_vec
group_exp[:, dim_rotations:] = group_exp_translation
group_exp = self.regularize(group_exp)
return group_exp
def group_log_from_identity(self,
point):
"""
Compute the group logarithm of point point,
from the identity.
"""
assert self.belongs(point)
point = self.regularize(point)
rotations = self.rotations
dim_rotations = rotations.dimension
rot_vec = point[:, :dim_rotations]
angle = np.linalg.norm(rot_vec, axis=1)
angle = vectorization.expand_dims(angle, to_ndim=2, axis=1)
translation = point[:, dim_rotations:]
group_log = np.zeros_like(point)
group_log[:, :dim_rotations] = rot_vec
skew_rot_vec = so_group.skew_matrix_from_vector(rot_vec)
sq_skew_rot_vec = np.matmul(skew_rot_vec, skew_rot_vec)
mask_close_0 = np.isclose(angle, 0)
mask_close_0 = np.squeeze(mask_close_0, axis=1)
mask_close_pi = np.isclose(angle, np.pi)
mask_close_pi = np.squeeze(mask_close_pi, axis=1)
mask_else = ~mask_close_0 & ~mask_close_pi
coef_1 = - 0.5 * np.ones_like(angle)
coef_2 = np.zeros_like(angle)
coef_2[mask_close_0] = (1. / 12. + angle[mask_close_0] ** 2 / 720.
+ angle[mask_close_0] ** 4 / 30240.
+ angle[mask_close_0] ** 6 / 1209600.)
delta_angle = angle[mask_close_pi] - np.pi
coef_2[mask_close_pi] = (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[mask_else]
* np.sin(angle[mask_else]) / (1 - np.cos(angle[mask_else])))
coef_2[mask_else] = (1 - psi) / (angle[mask_else] ** 2)
n_points, _ = point.shape
group_log_translation = np.zeros((n_points, self.n))
for i in range(n_points):
translation_i = translation[i]
term_1_i = coef_1[i] * np.dot(translation_i,
np.transpose(skew_rot_vec[i]))
term_2_i = coef_2[i] * np.dot(translation_i,
np.transpose(sq_skew_rot_vec[i]))
group_log_translation[i] = translation_i + term_1_i + term_2_i
group_log[:, dim_rotations:] = group_log_translation
assert group_log.ndim == 2
return group_log
def random_uniform(self, n_samples=1):
"""
Generate an 6d vector element of SE(3) uniformly,
by generating separately a rotation vector uniformly
on the hypercube of sides [-1, 1] in the tangent space,
and a translation in the hypercube of side [-1, 1] in
the euclidean space.
"""
random_rot_vec = self.rotations.random_uniform(n_samples)
random_translation = self.translations.random_uniform(n_samples)
random_transfo = np.concatenate([random_rot_vec, random_translation],
axis=1)
random_transfo = self.regularize(random_transfo)
return random_transfo
def exponential_matrix(self, rot_vec):
"""
Compute the exponential of the rotation matrix
represented by rot_vec.
:param rot_vec: 3D rotation vector
:returns exponential_mat: 3x3 matrix
"""
rot_vec = self.rotations.regularize(rot_vec)
n_rot_vecs, _ = rot_vec.shape
angle = np.linalg.norm(rot_vec, axis=1)
angle = vectorization.expand_dims(angle, to_ndim=2, axis=1)
skew_rot_vec = so_group.skew_matrix_from_vector(rot_vec)
coef_1 = np.empty_like(angle)
coef_2 = np.empty_like(coef_1)
mask_0 = np.equal(angle, 0)
mask_0 = np.squeeze(mask_0, axis=1)
mask_close_to_0 = np.isclose(angle, 0)
mask_close_to_0 = np.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 discountinuity as 0 is expected.
coef_1[mask_0] = 0
coef_2[mask_0] = 0
coef_1[mask_else] = (angle[mask_else] ** (-2)
* (1. - np.cos(angle[mask_else])))
coef_2[mask_else] = (angle[mask_else] ** (-2)
* (1. - (np.sin(angle[mask_else])
/ angle[mask_else])))
term_1 = np.zeros((n_rot_vecs, self.n, self.n))
term_2 = np.zeros_like(term_1)
for i in range(n_rot_vecs):
term_1[i] = np.eye(self.n) + skew_rot_vec[i] * coef_1[i]
term_2[i] = np.matmul(skew_rot_vec[i], skew_rot_vec[i]) * coef_2[i]
exponential_mat = term_1 + term_2
assert exponential_mat.ndim == 3
return exponential_mat
def group_exponential_barycenter(self, points, weights=None):
"""
Compute the group exponential barycenter.
:param points: SE3 data points, Nx6 array
:param weights: data point weights, Nx1 array
"""
n_points = points.shape[0]
assert n_points > 0
if weights is None:
weights = np.ones((n_points, 1))
weights = vectorization.expand_dims(weights, to_ndim=2, axis=1)
n_weights, _ = weights.shape
assert n_points == n_weights
dim = self.dimension
rotations = self.rotations
dim_rotations = rotations.dimension
rotation_vectors = points[:, :dim_rotations]
translations = points[:, dim_rotations:dim]
assert rotation_vectors.shape == (n_points, dim_rotations)
assert translations.shape == (n_points, self.n)
mean_rotation = rotations.group_exponential_barycenter(
points=rotation_vectors,
weights=weights)
mean_rotation_mat = rotations.matrix_from_rotation_vector(
mean_rotation)
matrix = np.zeros((1,) + (self.n,) * 2)
translation_aux = np.zeros((1, self.n))
inv_rot_mats = rotations.matrix_from_rotation_vector(
-rotation_vectors)
# TODO(nina): this is the same mat multiplied several times
matrix_aux = np.matmul(mean_rotation_mat, inv_rot_mats)
assert matrix_aux.shape == (n_points,) + (dim_rotations,) * 2
vec_aux = rotations.rotation_vector_from_matrix(matrix_aux)
matrix_aux = self.exponential_matrix(vec_aux)
matrix_aux = np.linalg.inv(matrix_aux)
for i in range(n_points):
matrix += weights[i] * matrix_aux[i]
translation_aux += weights[i] * np.dot(np.matmul(
matrix_aux[i],
inv_rot_mats[i]),
translations[i])
mean_translation = np.dot(translation_aux,
np.transpose(np.linalg.inv(matrix),
axes=(0, 2, 1)))
exp_bar = np.zeros((1, dim))
exp_bar[0, :dim_rotations] = mean_rotation
exp_bar[0, dim_rotations:dim] = mean_translation
return exp_bar