def draw(self, n_theta=25, n_phi=13, scale=0.05, elev=60.0, azim=0.0):
"""Draw the sphere regularly sampled with corresponding triangles."""
self.set_ax()
self.set_view(elev=elev, azim=azim)
self.ax.set_axis_off()
plt.tight_layout()
coords_theta = gs.linspace(0.0, 2.0 * gs.pi, n_theta)
coords_phi = gs.linspace(0.0, gs.pi, n_phi)
coords_x = gs.to_numpy(0.5 * gs.outer(gs.sin(coords_phi), gs.cos(coords_theta)))
coords_y = gs.to_numpy(0.5 * gs.outer(gs.sin(coords_phi), gs.sin(coords_theta)))
coords_z = gs.to_numpy(
0.5 * gs.outer(gs.cos(coords_phi), gs.ones_like(coords_theta))
)
self.ax.plot_surface(
coords_x,
coords_y,
coords_z,
rstride=1,
cstride=1,
color="grey",
linewidth=0,
alpha=0.1,
zorder=-1,
)
self.ax.plot_wireframe(
coords_x,
coords_y,
coords_z,
linewidths=0.6,
color="grey",
alpha=0.6,
zorder=-1,
)
def lim(theta):
return (
gs.pi
- self.elev
+ (2.0 * self.elev - gs.pi) / gs.pi * abs(self.azim - theta)
)
for theta in gs.linspace(0.0, 2.0 * gs.pi, n_theta // 2 + 1):
for phi in gs.linspace(0.0, gs.pi, n_phi):
if theta <= self.azim + gs.pi and phi <= lim(theta):
self.draw_triangle(theta, phi, scale)
if theta > self.azim + gs.pi and phi < lim(
2.0 * self.azim + 2.0 * gs.pi - theta
):
self.draw_triangle(theta, phi, scale)
def draw(self, n_r=7, n_theta=25, scale=0.05):
"""Draw the disk regularly sampled with corresponding triangles."""
self.set_ax()
self.ax.set_axis_off()
plt.tight_layout()
coords_r = gs.linspace(0.0, gs.pi / 4.0, n_r)
coords_theta = gs.linspace(0.0, 2.0 * gs.pi, n_theta)
coords_x = gs.to_numpy(gs.outer(coords_r, gs.cos(coords_theta)))
coords_y = gs.to_numpy(gs.outer(coords_r, gs.sin(coords_theta)))
self.ax.fill(
list(coords_x[-1, :]),
list(coords_y[-1, :]),
color="grey",
alpha=0.1,
zorder=-1,
)
for i_r in range(n_r):
self.ax.plot(
coords_x[i_r, :],
coords_y[i_r, :],
linewidth=0.6,
color="grey",
alpha=0.6,
zorder=-1,
)
for i_t in range(n_theta):
self.ax.plot(
coords_x[:, i_t],
coords_y[:, i_t],
linewidth=0.6,
color="grey",
alpha=0.6,
zorder=-1,
)
for r in gs.linspace(0.0, gs.pi / 4, n_r):
for theta in gs.linspace(0.0, 2.0 * gs.pi, n_theta // 2 + 1):
if theta == 0.0:
self.draw_triangle(0.0, 0.0, scale)
else:
self.draw_triangle(r, theta, scale)
def grad(y_pred,
y_true,
metric=SE3.left_canonical_metric,
representation='vector'):
"""
Closed-form for the gradient of pose_loss.
:return: tangent vector at point y_pred.
"""
if y_pred.ndim == 1:
y_pred = gs.expand_dims(y_pred, axis=0)
if y_true.ndim == 1:
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'vector':
grad = lie_group.grad(y_pred, y_true, SE3, metric)
if representation == 'quaternion':
y_pred_rot_vec = SO3.rotation_vector_from_quaternion(y_pred[:, :4])
y_pred_pose = gs.hstack([y_pred_rot_vec, y_pred[:, 4:]])
y_true_rot_vec = SO3.rotation_vector_from_quaternion(y_true[:, :4])
y_true_pose = gs.hstack([y_true_rot_vec, y_true[:, 4:]])
grad = lie_group.grad(y_pred_pose, y_true_pose, SE3, metric)
differential = gs.zeros((1, 6, 7))
upper_left_block = gs.zeros((1, 3, 4))
lower_right_block = gs.zeros((1, 3, 3))
quat_scalar = y_pred[:, :1]
quat_vec = y_pred[:, 1:4]
quat_vec_norm = gs.linalg.norm(quat_vec, axis=1)
quat_sq_norm = quat_vec_norm**2 + quat_scalar**2
# TODO(nina): check that this sq norm is 1?
quat_arctan2 = gs.arctan2(quat_vec_norm, quat_scalar)
differential_scalar = -2 * quat_vec / (quat_sq_norm)
differential_vec = (
2 *
(quat_scalar / quat_sq_norm - 2 * quat_arctan2 / quat_vec_norm) *
gs.outer(quat_vec, quat_vec) / quat_vec_norm**2 +
2 * quat_arctan2 / quat_vec_norm * gs.eye(3))
upper_left_block[0, :, :1] = differential_scalar.transpose()
upper_left_block[0, :, 1:] = differential_vec
lower_right_block[0, :, :] = gs.eye(3)
differential[0, :3, :4] = upper_left_block
differential[0, 3:, 4:] = lower_right_block
grad = gs.matmul(grad, differential)
grad = gs.squeeze(grad, axis=0)
return grad
def riemannian_submersion(point):
"""Compute the correlation matrix associated to an SPD matrix.
Parameters
----------
point : array-like, shape=[..., n, n]
SPD matrix.
Returns
-------
cor : array_like, shape=[..., n, n]
Full rank correlation matrix.
"""
diagonal = Matrices.diagonal(point)**(-0.5)
return point * gs.outer(diagonal, diagonal)
def diag_action(diagonal_vec, point):
r"""Apply a diagonal matrix on an SPD matrices by congruence.
The action of :math:`D` on :math:`\Sigma` is given by :math:`D
\Sigma D. The diagonal matrix must be passed as a vector representing
its diagonal.
Parameters
----------
diagonal_vec : array-like, shape=[..., n]
Vector coefficient of the diagonal matrix.
point : array-like, shape=[..., n, n]
Symmetric Positive definite matrix.
Returns
-------
mat : array-like, shape=[..., n, n]
Symmetric matrix obtained by the action of `diagonal_vec` on
`point`.
"""
return point * gs.outer(diagonal_vec, diagonal_vec)
def grad(y_pred, y_true,
metric=SO3.bi_invariant_metric,
representation='vector'):
y_pred = gs.expand_dims(y_pred, axis=0)
y_true = gs.expand_dims(y_true, axis=0)
if representation == 'vector':
grad = lie_group.grad(y_pred, y_true, SO3, metric)
if representation == 'quaternion':
differential = gs.zeros((1, 6, 7))
differential = gs.zeros((1, 3, 4))
quat_scalar = y_pred[:, :1]
quat_vec = y_pred[:, 1:]
quat_vec_norm = gs.linalg.norm(quat_vec, axis=1)
quat_sq_norm = quat_vec_norm ** 2 + quat_scalar ** 2
quat_arctan2 = gs.arctan2(quat_vec_norm, quat_scalar)
differential_scalar = - 2 * quat_vec / (quat_sq_norm)
differential_vec = (2 * (quat_scalar / quat_sq_norm
- 2 * quat_arctan2 / quat_vec_norm)
* gs.outer(quat_vec, quat_vec) / quat_vec_norm ** 2
+ 2 * quat_arctan2 / quat_vec_norm * gs.eye(3))
differential[0, :, :1] = differential_scalar.transpose()
differential[0, :, 1:] = differential_vec
y_pred = SO3.rotation_vector_from_quaternion(y_pred)
y_true = SO3.rotation_vector_from_quaternion(y_true)
grad = lie_group.grad(y_pred, y_true, SO3, metric)
grad = gs.matmul(grad, differential)
grad = gs.squeeze(grad, axis=0)
return grad
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 SO(n).
"""
assert self.belongs(point)
assert left_or_right in ('left', 'right')
if self.n == 3:
point = self.regularize(point)
n_points, _ = point.shape
angle = gs.linalg.norm(point, axis=1)
angle = gs.expand_dims(angle, axis=1)
coef_1 = gs.zeros([n_points, 1])
coef_2 = gs.zeros([n_points, 1])
mask_0 = gs.isclose(angle, 0)
mask_0 = gs.squeeze(mask_0, axis=1)
coef_1[mask_0] = (1 - angle[mask_0]**2 / 12 -
angle[mask_0]**4 / 720 -
angle[mask_0]**6 / 30240)
coef_2[mask_0] = (1 / 12 + angle[mask_0]**2 / 720 +
angle[mask_0]**4 / 30240 +
angle[mask_0]**6 / 1209600)
mask_pi = gs.isclose(angle, gs.pi)
mask_pi = gs.squeeze(mask_pi, axis=1)
delta_angle = angle[mask_pi] - gs.pi
coef_1[mask_pi] = (-gs.pi * delta_angle / 4 - delta_angle**2 / 4 -
gs.pi * delta_angle**3 / 48 -
delta_angle**4 / 48 -
gs.pi * delta_angle**5 / 480 -
delta_angle**6 / 480)
coef_2[mask_pi] = (1 - coef_1[mask_pi]) / angle[mask_pi]**2
mask_else = ~mask_0 & ~mask_pi
coef_1[mask_else] = ((angle[mask_else] / 2) /
gs.tan(angle[mask_else] / 2))
coef_2[mask_else] = (1 - coef_1[mask_else]) / angle[mask_else]**2
jacobian = gs.zeros((n_points, self.dimension, self.dimension))
for i in range(n_points):
if left_or_right == 'left':
jacobian[i] = (coef_1[i] * gs.identity(self.dimension) +
coef_2[i] * gs.outer(point[i], point[i]) +
skew_matrix_from_vector(point[i]) / 2)
else:
jacobian[i] = (coef_1[i] * gs.identity(self.dimension) +
coef_2[i] * gs.outer(point[i], point[i]) -
skew_matrix_from_vector(point[i]) / 2)
else:
if left_or_right == 'right':
raise NotImplementedError(
'The jacobian of the right translation'
' is not implemented.')
jacobian = self.matrix_from_rotation_vector(point)
assert jacobian.ndim == 3
return jacobian
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 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
def jacobian_translation(self, point, left_or_right='left'):
"""Compute the jacobian matrix corresponding to translation.
Compute the jacobian matrix of the differential
of the left/right translations from the identity to point in SO(3).
Parameters
----------
point : array-like, shape=[..., 3]
left_or_right : str, {'left', 'right'}, optional
default: 'left'
point_type : str, {'vector', 'matrix'}, optional
default: self.default_point_type
Returns
-------
jacobian : array-like, shape=[..., 3, 3]
"""
geomstats.error.check_parameter_accepted_values(
left_or_right, 'left_or_right', ['left', 'right'])
point = self.regularize(point)
n_points, _ = point.shape
angle = gs.linalg.norm(point, axis=-1)
angle = gs.expand_dims(angle, axis=-1)
coef_1 = gs.zeros([n_points, 1])
coef_2 = gs.zeros([n_points, 1])
# This avoids dividing by 0.
mask_0 = gs.isclose(angle, 0.)
mask_0_float = gs.cast(mask_0, gs.float32) + self.epsilon
coef_1 += mask_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_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)
# This avoids dividing by 0.
mask_pi = gs.isclose(angle, gs.pi)
mask_pi_float = gs.cast(mask_pi, gs.float32) + self.epsilon
delta_angle = angle - gs.pi
coef_1 += mask_pi_float * (TAYLOR_COEFFS_1_AT_PI[1] * delta_angle +
TAYLOR_COEFFS_1_AT_PI[2] * delta_angle**2 +
TAYLOR_COEFFS_1_AT_PI[3] * delta_angle**3 +
TAYLOR_COEFFS_1_AT_PI[4] * delta_angle**4 +
TAYLOR_COEFFS_1_AT_PI[5] * delta_angle**5 +
TAYLOR_COEFFS_1_AT_PI[6] * delta_angle**6)
angle += mask_0_float
coef_2 += mask_pi_float * ((1 - coef_1) / angle**2)
# This avoids dividing by 0.
mask_else = ~mask_0 & ~mask_pi
mask_else_float = gs.cast(mask_else, gs.float32) + self.epsilon
# This avoids division by 0.
angle += mask_pi_float
coef_1 += mask_else_float * ((angle / 2) / gs.tan(angle / 2))
coef_2 += mask_else_float * ((1 - coef_1) / angle**2)
jacobian = gs.zeros((n_points, self.dim, self.dim))
n_points_tensor = gs.array(n_points)
for i in range(n_points):
# This avoids dividing by 0.
mask_i_float = (gs.get_mask_i_float(i, n_points_tensor) +
self.epsilon)
sign = -1.
if left_or_right == 'left':
sign = +1.
jacobian_i = (coef_1[i] * gs.eye(self.dim) +
coef_2[i] * gs.outer(point[i], point[i]) +
sign * self.skew_matrix_from_vector(point[i]) / 2.)
jacobian += gs.einsum('n,ij->nij', mask_i_float, jacobian_i)
return jacobian
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\'.')
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 SO(n).
"""
assert left_or_right in ('left', 'right')
if point_type is None:
point_type = self.default_point_type
assert self.belongs(point, point_type)
if point_type == 'vector':
if self.n == 3:
point = self.regularize(point, point_type=point_type)
n_points, _ = point.shape
angle = gs.linalg.norm(point, axis=1)
angle = gs.expand_dims(angle, axis=1)
coef_1 = gs.zeros([n_points, 1])
coef_2 = gs.zeros([n_points, 1])
mask_0 = gs.isclose(angle, 0)
mask_0 = gs.squeeze(mask_0, axis=1)
coef_1[mask_0] = (TAYLOR_COEFFS_1_AT_0[0] +
TAYLOR_COEFFS_1_AT_0[2] * angle[mask_0]**2 +
TAYLOR_COEFFS_1_AT_0[4] * angle[mask_0]**4 +
TAYLOR_COEFFS_1_AT_0[6] * angle[mask_0]**6)
coef_2[mask_0] = (TAYLOR_COEFFS_2_AT_0[0] +
TAYLOR_COEFFS_2_AT_0[2] * angle[mask_0]**2 +
TAYLOR_COEFFS_2_AT_0[4] * angle[mask_0]**4 +
TAYLOR_COEFFS_2_AT_0[6] * angle[mask_0]**6)
mask_pi = gs.isclose(angle, gs.pi)
mask_pi = gs.squeeze(mask_pi, axis=1)
delta_angle = angle[mask_pi] - gs.pi
coef_1[mask_pi] = (TAYLOR_COEFFS_1_AT_PI[1] * delta_angle +
TAYLOR_COEFFS_1_AT_PI[2] * delta_angle**2 +
TAYLOR_COEFFS_1_AT_PI[3] * delta_angle**3 +
TAYLOR_COEFFS_1_AT_PI[4] * delta_angle**4 +
TAYLOR_COEFFS_1_AT_PI[5] * delta_angle**5 +
TAYLOR_COEFFS_1_AT_PI[6] * delta_angle**6)
coef_2[mask_pi] = (1 - coef_1[mask_pi]) / angle[mask_pi]**2
mask_else = ~mask_0 & ~mask_pi
coef_1[mask_else] = ((angle[mask_else] / 2) /
gs.tan(angle[mask_else] / 2))
coef_2[mask_else] = ((1 - coef_1[mask_else]) /
angle[mask_else]**2)
jacobian = gs.zeros((n_points, self.dimension, self.dimension))
for i in range(n_points):
sign = -1
if left_or_right == 'left':
sign = +1
jacobian[i] = (
coef_1[i] * gs.identity(self.dimension) +
coef_2[i] * gs.outer(point[i], point[i]) +
sign * self.skew_matrix_from_vector(point[i]) / 2)
else:
if left_or_right == 'right':
raise NotImplementedError(
'The jacobian of the right translation'
' is not implemented.')
jacobian = self.matrix_from_rotation_vector(point)
assert gs.ndim(jacobian) == 3
elif point_type == 'matrix':
raise NotImplementedError()
return jacobian
