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