def quaternion_from_tait_bryan_angles(self,
tait_bryan_angles,
extrinsic_or_intrinsic='extrinsic',
order='zyx'):
"""Convert a rotation given by Tait-Bryan angles into unit quaternion.
Parameters
----------
tait_bryan_angles : array-like, shape=[..., 3]
extrinsic_or_intrinsic : str, {'extrinsic', 'intrinsic'}, optional
default: 'extrinsic'
order : str, {'xyz', 'zyx'}, optional
default: 'zyx'
Returns
-------
quat : array-like, shape=[..., 4]
"""
extrinsic_zyx = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'zyx')
intrinsic_xyz = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'xyz')
extrinsic_xyz = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'xyz')
intrinsic_zyx = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'zyx')
if extrinsic_zyx:
tait_bryan_angles_reversed = gs.flip(tait_bryan_angles, axis=1)
quat = self.quaternion_from_tait_bryan_angles_intrinsic_xyz(
tait_bryan_angles_reversed)
elif intrinsic_xyz:
quat = self.quaternion_from_tait_bryan_angles_intrinsic_xyz(
tait_bryan_angles)
elif extrinsic_xyz:
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_xyz(
tait_bryan_angles)
quat = self.quaternion_from_matrix(rot_mat)
elif intrinsic_zyx:
tait_bryan_angles_reversed = gs.flip(tait_bryan_angles, axis=1)
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_xyz(
tait_bryan_angles_reversed)
quat = self.quaternion_from_matrix(rot_mat)
else:
raise ValueError('extrinsic_or_intrinsic should be'
' \'extrinsic\' or \'intrinsic\''
' and order should be \'xyz\' or \'zyx\'.')
return quat
def tait_bryan_angles_from_quaternion(self,
quaternion,
extrinsic_or_intrinsic='extrinsic',
order='zyx'):
"""Convert quaternion to a rotation in form angle_1, angle_2, angle_3.
Parameters
----------
quaternion : array-like, shape=[..., 4]
extrinsic_or_intrinsic : str, {'extrinsic', 'intrinsic'}, optional
default: 'extrinsic'
order : str, {'xyz', 'zyx'}, optional
default: 'zyx'
Returns
-------
tait_bryan : array-like, shape=[..., 3]
"""
extrinsic_zyx = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'zyx')
intrinsic_xyz = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'xyz')
extrinsic_xyz = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'xyz')
intrinsic_zyx = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'zyx')
if extrinsic_zyx:
tait_bryan = self.tait_bryan_angles_from_quaternion_intrinsic_xyz(
quaternion)
tait_bryan = gs.flip(tait_bryan, axis=1)
elif intrinsic_xyz:
tait_bryan = self.tait_bryan_angles_from_quaternion_intrinsic_xyz(
quaternion)
elif extrinsic_xyz:
tait_bryan = self.tait_bryan_angles_from_quaternion_intrinsic_zyx(
quaternion)
tait_bryan = gs.flip(tait_bryan, axis=1)
elif intrinsic_zyx:
tait_bryan = self.tait_bryan_angles_from_quaternion_intrinsic_zyx(
quaternion)
else:
raise ValueError('extrinsic_or_intrinsic should be'
' \'extrinsic\' or \'intrinsic\''
' and order should be \'xyz\' or \'zyx\'.')
return tait_bryan
def submersion(point, k):
r"""Submersion that defines the Grassmann manifold.
The Grassmann manifold is defined here as embedded in the set of
symmetric matrices, as the pre-image of the function defined around the
projector on the space spanned by the first k columns of the identity
matrix by (see Exercise E.25 in [Pau07]_).
.. math:
\begin{pmatrix} I_k + A & B^T \\ B & D \end{pmatrix} \mapsto
(D - B(I_k + A)^{-1}B^T, A + A^2 + B^TB
This map is a submersion and its zero space is the set of orthogonal
rank-k projectors.
References
----------
.. [Pau07] Paulin, Frédéric. “Géométrie différentielle élémentaire,” 2007.
https://www.imo.universite-paris-saclay.fr/~paulin
/notescours/cours_geodiff.pdf.
"""
_, eigvecs = gs.linalg.eigh(point)
eigvecs = gs.flip(eigvecs, -1)
flipped_point = Matrices.mul(Matrices.transpose(eigvecs), point, eigvecs)
b = flipped_point[..., k:, :k]
d = flipped_point[..., k:, k:]
a = flipped_point[..., :k, :k] - gs.eye(k)
first = d - Matrices.mul(b, GeneralLinear.inverse(a + gs.eye(k)),
Matrices.transpose(b))
second = a + Matrices.mul(a, a) + Matrices.mul(Matrices.transpose(b), b)
row_1 = gs.concatenate([first, gs.zeros_like(b)], axis=-1)
row_2 = gs.concatenate([Matrices.transpose(gs.zeros_like(b)), second],
axis=-1)
return gs.concatenate([row_1, row_2], axis=-2)
def normalization_factor(self, variances):
"""Return normalization factor.
Parameters
----------
variances : array-like, shape=[n,]
Array of equally distant values of the
variance precision time.
Returns
-------
norm_func : array-like, shape=[n,]
Normalisation factor for all given variances.
"""
binomial_coefficient = None
n_samples = variances.shape[0]
expand_variances = gs.expand_dims(variances, axis=0)
expand_variances = gs.repeat(expand_variances, self.dim, axis=0)
if binomial_coefficient is None:
dim_range = gs.arange(self.dim)
dim_range[0] = 1
n_fact = dim_range.prod()
k_fact = gs.concatenate([
gs.expand_dims(dim_range[:i].prod(), 0)
for i in range(1, dim_range.shape[0] + 1)
], 0)
nmk_fact = gs.flip(k_fact, 0)
binomial_coefficient = n_fact / (k_fact * nmk_fact)
binomial_coefficient = gs.expand_dims(binomial_coefficient, -1)
binomial_coefficient = gs.repeat(binomial_coefficient,
n_samples,
axis=1)
range_ = gs.expand_dims(gs.arange(self.dim), -1)
range_ = gs.repeat(range_, n_samples, axis=1)
ones_ = gs.expand_dims(gs.ones(self.dim), -1)
ones_ = gs.repeat(ones_, n_samples, axis=1)
alternate_neg = (-ones_)**(range_)
erf_arg = ((
(self.dim - 1) - 2 * range_) * expand_variances) / gs.sqrt(2)
exp_arg = ((((self.dim - 1) - 2 * range_) * expand_variances) /
gs.sqrt(2))**2
norm_func_1 = (1 + gs.erf(erf_arg)) * gs.exp(exp_arg)
norm_func_2 = binomial_coefficient * norm_func_1
norm_func_3 = alternate_neg * norm_func_2
norm_func = NORMALIZATION_FACTOR_CST * variances * \
norm_func_3.sum(0) * (1 / (2 ** (self.dim - 1)))
return norm_func
def tait_bryan_angles_quaternion_test_data(self):
xyz = gs.array([
[cos_angle_pi_12, 0.0, 0.0, sin_angle_pi_12],
[cos_angle_pi_12, 0.0, sin_angle_pi_12, 0.0],
[cos_angle_pi_12, sin_angle_pi_12, 0.0, 0.0],
])
zyx = gs.flip(xyz, axis=0)
data = {"xyz": xyz, "zyx": zyx}
smoke_data = []
e1 = gs.array([1.0, 0.0, 0.0, 0.0])
for coord, order in itertools.product(["intrinsic", "extrinsic"],
orders):
for i in range(3):
vec = gs.squeeze(
gs.array_from_sparse([(0, i)], [angle_pi_6], (1, 3)))
smoke_data += [
dict(coord=coord,
order=order,
vec=vec,
quat=data[order][i])
]
smoke_data += [
dict(coord=coord, order=order, vec=gs.zeros(3), quat=e1)
]
return self.generate_tests(smoke_data)
def tait_bryan_angles_matrix_test_data(self):
xyz = gs.array([
[
[cos_angle_pi_6, -sin_angle_pi_6, 0.0],
[sin_angle_pi_6, cos_angle_pi_6, 0.0],
[0.0, 0.0, 1.0],
],
[
[cos_angle_pi_6, 0.0, sin_angle_pi_6],
[0.0, 1.0, 0.0],
[-sin_angle_pi_6, 0.0, cos_angle_pi_6],
],
[
[1.0, 0.0, 0.0],
[0.0, cos_angle_pi_6, -sin_angle_pi_6],
[0.0, sin_angle_pi_6, cos_angle_pi_6],
],
])
zyx = gs.flip(xyz, axis=0)
data = {"xyz": xyz, "zyx": zyx}
smoke_data = []
for coord, order in itertools.product(coords, orders):
for i in range(3):
vec = gs.squeeze(
gs.array_from_sparse([(0, i)], [angle_pi_6], (1, 3)))
smoke_data += [
dict(coord=coord, order=order, vec=vec, mat=data[order][i])
]
smoke_data += [
dict(coord=coord, order=order, vec=gs.zeros(3), mat=gs.eye(3))
]
return self.generate_tests(smoke_data)
def matrix_from_tait_bryan_angles(self,
tait_bryan_angles,
extrinsic_or_intrinsic='extrinsic',
order='zyx'):
"""Convert Tait-Bryan angles to rot mat in extr or intr coords.
Convert a rotation given in terms of the tait bryan angles,
[angle_1, angle_2, angle_3] in extrinsic (fixed) or
intrinsic (moving) coordinate frame into a rotation matrix.
If the order is zyx, into the rotation matrix rot_mat:
rot_mat = X(angle_1).Y(angle_2).Z(angle_3)
where:
- X(angle_1) is a rotation of angle angle_1 around axis x.
- Y(angle_2) is a rotation of angle angle_2 around axis y.
- Z(angle_3) is a rotation of angle angle_3 around axis z.
Exchanging 'extrinsic' and 'intrinsic' amounts to
exchanging the order.
Parameters
----------
tait_bryan_angles : array-like, shape=[..., 3]
extrinsic_or_intrinsic : str, {'extrensic', 'intrinsic'} optional
default: 'extrinsic'
order : str, {'xyz', 'zyx'}, optional
default: 'zyx'
Returns
-------
rot_mat : array-like, shape=[..., n, n]
"""
geomstats.error.check_parameter_accepted_values(
extrinsic_or_intrinsic, 'extrinsic_or_intrinsic',
['extrinsic', 'intrinsic'])
geomstats.error.check_parameter_accepted_values(
order, 'order', ['xyz', 'zyx'])
tait_bryan_angles = gs.to_ndarray(tait_bryan_angles, to_ndim=2)
extrinsic_zyx = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'zyx')
intrinsic_xyz = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'xyz')
extrinsic_xyz = (extrinsic_or_intrinsic == 'extrinsic'
and order == 'xyz')
intrinsic_zyx = (extrinsic_or_intrinsic == 'intrinsic'
and order == 'zyx')
if extrinsic_zyx:
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_zyx(
tait_bryan_angles)
elif intrinsic_xyz:
tait_bryan_angles_reversed = gs.flip(tait_bryan_angles, axis=1)
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_zyx(
tait_bryan_angles_reversed)
elif extrinsic_xyz:
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_xyz(
tait_bryan_angles)
elif intrinsic_zyx:
tait_bryan_angles_reversed = gs.flip(tait_bryan_angles, axis=1)
rot_mat = self.matrix_from_tait_bryan_angles_extrinsic_xyz(
tait_bryan_angles_reversed)
else:
raise ValueError('extrinsic_or_intrinsic should be'
' \'extrinsic\' or \'intrinsic\''
' and order should be \'xyz\' or \'zyx\'.')
return rot_mat
