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 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_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 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_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
