Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
Simulated noisy inputs received by the sensor.
observations : array-like, shape=[len(true_inputs)/obs_freq, dim_obs]
Simulated noisy observations of the system.
"""
true_traj = [1 * true_init]
for incr in true_inputs:
true_traj.append(kalman.model.propagate(true_traj[-1], incr))
true_obs = [
kalman.model.observation_model(pose)
for pose in true_traj[obs_freq::obs_freq]]
obs_dtype = true_obs[0].dtype
observations = [
np.random.multivariate_normal(obs, kalman.measurement_noise)
for obs in true_obs]
observations = [gs.cast(obs, obs_dtype) for obs in observations]
input_dtype = true_inputs[0].dtype
inputs = [gs.concatenate(
(incr[:1], np.random.multivariate_normal(
incr[1:], kalman.process_noise)), axis=0)
for incr in true_inputs]
inputs = [gs.cast(incr, input_dtype) for incr in inputs]
return gs.array(true_traj), inputs, gs.array(observations)
-------
rot_mat: array-like, shape=[n_samples, 3]
"""
rot_vec = self.regularize(rot_vec)
angle = gs.linalg.norm(rot_vec, axis=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
skew_rot_vec = self.skew_matrix_from_vector(rot_vec)
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
# 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 * (1. - (angle ** 2) / 6.)
coef_2 += mask_0_float * (1. / 2. - angle ** 2)
# This avoids dividing by 0.
mask_else = ~mask_0
mask_else_float = gs.cast(mask_else, gs.float32) + self.epsilon
angle += mask_0_float
coef_1 += mask_else_float * (gs.sin(angle) / angle)
coef_2 += mask_else_float * (
(1. - gs.cos(angle)) / (angle ** 2))
coef_1 = gs.squeeze(coef_1, axis=1)
coef_2 = gs.squeeze(coef_2, axis=1)
norm_base_point = self.embedding_metric.norm(base_point)
norm_point = self.embedding_metric.norm(point)
inner_prod = self.embedding_metric.inner_product(base_point, point)
cos_angle = inner_prod / (norm_base_point * norm_point)
cos_angle = gs.clip(cos_angle, -1., 1.)
angle = gs.arccos(cos_angle)
angle = gs.to_ndarray(angle, to_ndim=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
mask_0 = gs.isclose(angle, 0.)
mask_else = gs.equal(mask_0, gs.array(False))
mask_0_float = gs.cast(mask_0, gs.float32)
mask_else_float = gs.cast(mask_else, gs.float32)
angle_0 = gs.boolean_mask(angle, mask_0)
angle_0 = gs.to_ndarray(angle_0, to_ndim=1)
angle_0 = gs.to_ndarray(angle_0, to_ndim=2, axis=1)
angle_else = gs.boolean_mask(angle, mask_else)
angle_else = gs.to_ndarray(angle_else, to_ndim=1)
angle_else = gs.to_ndarray(angle_else, to_ndim=2, axis=1)
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
coef_1 += mask_0_float * (
1. + INV_SIN_TAYLOR_COEFFS[1] * angle ** 2
+ INV_SIN_TAYLOR_COEFFS[3] * angle ** 4
+ INV_SIN_TAYLOR_COEFFS[5] * angle ** 6
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)
mask_0_float = gs.cast(mask_0, gs.float32)
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)
mask_pi = gs.isclose(angle, gs.pi)
mask_pi = gs.squeeze(mask_pi, axis=1)
mask_pi_float = gs.cast(mask_pi, gs.float32)
base_point : array-like, shape=[..., dim + 1]
Point in hyperbolic space.
Returns
-------
log : array-like, shape=[..., dim + 1]
Tangent vector at the base point equal to the Riemannian logarithm
of point at the base point.
"""
angle = self.dist(base_point, point) / self.scale
angle = gs.to_ndarray(angle, to_ndim=1)
mask_0 = gs.isclose(angle, 0.)
mask_else = ~mask_0
mask_0_float = gs.cast(mask_0, gs.float32)
mask_else_float = gs.cast(mask_else, gs.float32)
coef_1 = gs.zeros_like(angle)
coef_2 = gs.zeros_like(angle)
coef_1 += mask_0_float * (
1. + INV_SINH_TAYLOR_COEFFS[1] * angle ** 2
+ INV_SINH_TAYLOR_COEFFS[3] * angle ** 4
+ INV_SINH_TAYLOR_COEFFS[5] * angle ** 6
+ INV_SINH_TAYLOR_COEFFS[7] * angle ** 8)
coef_2 += mask_0_float * (
1. + INV_TANH_TAYLOR_COEFFS[1] * angle ** 2
+ INV_TANH_TAYLOR_COEFFS[3] * angle ** 4
+ INV_TANH_TAYLOR_COEFFS[5] * angle ** 6
+ INV_TANH_TAYLOR_COEFFS[7] * angle ** 8)
Returns
-------
quaternion : array-like, shape=[n_samples, 4]
"""
rot_vec = self.regularize(rot_vec)
angle = gs.linalg.norm(rot_vec, axis=1)
angle = gs.to_ndarray(angle, to_ndim=2, axis=1)
mask_0 = gs.isclose(angle, 0.)
mask_not_0 = ~mask_0
rotation_axis = gs.divide(
rot_vec,
angle
* gs.cast(mask_not_0, gs.float32)
+ gs.cast(mask_0, gs.float32))
quaternion = gs.concatenate(
(gs.cos(angle / 2),
gs.sin(angle / 2) * rotation_axis[:]),
axis=1)
return quaternion
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
def curve_on_geodesic(t):
t = gs.cast(t, gs.float32)
t = gs.to_ndarray(t, to_ndim=1)
t = gs.to_ndarray(t, to_ndim=2, axis=1)
new_initial_curve = gs.to_ndarray(initial_curve,
to_ndim=curve_ndim + 1)
new_initial_tangent_vec = gs.to_ndarray(initial_tangent_vec,
to_ndim=curve_ndim + 1)
tangent_vecs = gs.einsum('il,nkm->ikm', t, new_initial_tangent_vec)
curve_shape_at_time_t = gs.hstack([len(t), curve_shape])
curve_at_time_t = gs.zeros(curve_shape_at_time_t)
for k in range(len(t)):
curve_at_time_t[k, :] = self.exp(
tangent_vec=tangent_vecs[k, :],
base_point=new_initial_curve)
return curve_at_time_t
Parameters
----------
mat : array_like, shape=[..., n, n]
Symmetric matrix.
function : callable
Function to apply to eigenvalues.
Returns
-------
mat : array_like, shape=[..., n, n]
Symmetric matrix.
"""
eigvals, eigvecs = gs.linalg.eigh(mat)
if check_positive:
if gs.any(gs.cast(eigvals, gs.float32) < 0.):
logging.warning(
'Negative eigenvalue encountered in'
' {}'.format(function.__name__))
eigvals = function(eigvals)
eigvals = algebra_utils.from_vector_to_diagonal_matrix(eigvals)
transp_eigvecs = Matrices.transpose(eigvecs)
reconstuction = gs.matmul(eigvecs, eigvals)
reconstuction = gs.matmul(reconstuction, transp_eigvecs)
return reconstuction
if point_type == 'vector':
point = gs.to_ndarray(point, to_ndim=2)
assert self.belongs(point, point_type)
n_points, _ = point.shape
regularized_point = gs.copy(point)
if self.n == 3:
angle = gs.linalg.norm(regularized_point, axis=1)
mask_0 = gs.isclose(angle, 0.)
mask_not_0 = ~mask_0
mask_pi = gs.isclose(angle, gs.pi)
mask_0_float = gs.cast(mask_0, gs.float32)
mask_not_0_float = gs.cast(mask_not_0, gs.float32)
mask_pi_float = gs.cast(mask_pi, gs.float32)
k = gs.floor(angle / (2 * gs.pi) + .5)
norms_ratio = gs.zeros_like(angle)
# This avoids division by 0.
angle += mask_0_float * 1.
norms_ratio += mask_not_0_float * (
1. - 2. * gs.pi * k / angle)
norms_ratio += mask_0_float * 1.
norms_ratio += mask_pi_float * gs.pi / angle
regularized_point = gs.einsum(
'n,ni->ni', norms_ratio, regularized_point)