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 : Matrix exponential of rot_vec
"""
# TODO (nguigs): find usecase for this method
rot_vec = self.rotations.regularize(rot_vec)
n_rot_vecs = 1 if rot_vec.ndim == 1 else len(rot_vec)
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
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 = 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 regularize_tangent_vec_at_identity(self,
tangent_vec,
metric=None,
point_type=None):
"""
In 3D, regularize a tangent_vector by getting its norm at the identity,
determined by the metric, to be less than pi.
"""
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)
if self.n == 3:
if metric is None:
metric = self.left_canonical_metric
tangent_vec_metric_norm = metric.norm(tangent_vec)
tangent_vec_canonical_norm = gs.linalg.norm(tangent_vec,
axis=1)
if gs.ndim(tangent_vec_canonical_norm) == 1:
tangent_vec_canonical_norm = gs.expand_dims(
tangent_vec_canonical_norm, axis=1)
mask_norm_0 = gs.isclose(tangent_vec_metric_norm, 0)
mask_canonical_norm_0 = gs.isclose(tangent_vec_canonical_norm,
0)
mask_0 = mask_norm_0 | mask_canonical_norm_0
mask_else = ~mask_0
mask_0 = gs.squeeze(mask_0, axis=1)
mask_else = gs.squeeze(mask_else, axis=1)
coef = gs.empty_like(tangent_vec_metric_norm)
regularized_vec = tangent_vec
regularized_vec[mask_0] = tangent_vec[mask_0]
coef[mask_else] = (tangent_vec_metric_norm[mask_else] /
tangent_vec_canonical_norm[mask_else])
regularized_vec[mask_else] = self.regularize(
coef[mask_else] * tangent_vec[mask_else])
regularized_vec[mask_else] = (regularized_vec[mask_else] /
coef[mask_else])
else:
# TODO(nina): regularization needed in nD?
regularized_vec = tangent_vec
elif point_type == 'matrix':
# TODO(nina): regularization in terms
# of skew-symmetric matrices?
regularized_vec = tangent_vec
return regularized_vec
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None):
"""
In 3D, regularize a tangent_vector by getting its norm at the identity,
determined by the metric, to be less than pi,
following the regularization convention.
"""
assert self.point_representation in ('vector', 'matrix')
if self.point_representation is 'vector':
tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2)
_, vec_dim = tangent_vec.shape
if metric is None:
metric = self.left_canonical_metric
tangent_vec_metric_norm = metric.norm(tangent_vec)
tangent_vec_canonical_norm = gs.linalg.norm(tangent_vec, axis=1)
if tangent_vec_canonical_norm.ndim == 1:
tangent_vec_canonical_norm = gs.expand_dims(
tangent_vec_canonical_norm, axis=1)
mask_norm_0 = gs.isclose(tangent_vec_metric_norm, 0)
mask_canonical_norm_0 = gs.isclose(tangent_vec_canonical_norm, 0)
mask_0 = mask_norm_0 | mask_canonical_norm_0
mask_else = ~mask_0
mask_0 = gs.squeeze(mask_0, axis=1)
mask_else = gs.squeeze(mask_else, axis=1)
coef = gs.empty_like(tangent_vec_metric_norm)
regularized_vec = tangent_vec
regularized_vec[mask_0] = tangent_vec[mask_0]
coef[mask_else] = (tangent_vec_metric_norm[mask_else] /
tangent_vec_canonical_norm[mask_else])
regularized_vec[mask_else] = self.regularize(
coef[mask_else] * tangent_vec[mask_else])
regularized_vec[mask_else] = (regularized_vec[mask_else] /
coef[mask_else])
else:
# TODO(nina): regularization needed in nD?
regularized_vec = tangent_vec
return regularized_vec
