def _exponential_matrix(self, rot_vec): """Compute exponential of rotation matrix represented by rot_vec. Parameters ---------- rot_vec : array-like, shape=[..., 3] Returns ------- exponential_mat : Matrix exponential of rot_vec """ # TODO (nguigs): find usecase for this method rot_vec = self.rotations.regularize(rot_vec) n_rot_vecs = 1 if rot_vec.ndim == 1 else len(rot_vec) angle = gs.linalg.norm(rot_vec, axis=-1) angle = gs.to_ndarray(angle, to_ndim=2, axis=1) skew_rot_vec = self.rotations.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 discontinuity at 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 return exponential_mat
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 regularize_tangent_vec_at_identity(self, tangent_vec, metric=None, point_type=None): """ In 3D, regularize a tangent_vector by getting its norm at the identity, determined by the metric, to be less than pi. """ if point_type is None: point_type = self.default_point_type if point_type == 'vector': tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2) if self.n == 3: if metric is None: metric = self.left_canonical_metric tangent_vec_metric_norm = metric.norm(tangent_vec) tangent_vec_canonical_norm = gs.linalg.norm(tangent_vec, axis=1) if gs.ndim(tangent_vec_canonical_norm) == 1: tangent_vec_canonical_norm = gs.expand_dims( tangent_vec_canonical_norm, axis=1) mask_norm_0 = gs.isclose(tangent_vec_metric_norm, 0) mask_canonical_norm_0 = gs.isclose(tangent_vec_canonical_norm, 0) mask_0 = mask_norm_0 | mask_canonical_norm_0 mask_else = ~mask_0 mask_0 = gs.squeeze(mask_0, axis=1) mask_else = gs.squeeze(mask_else, axis=1) coef = gs.empty_like(tangent_vec_metric_norm) regularized_vec = tangent_vec regularized_vec[mask_0] = tangent_vec[mask_0] coef[mask_else] = (tangent_vec_metric_norm[mask_else] / tangent_vec_canonical_norm[mask_else]) regularized_vec[mask_else] = self.regularize( coef[mask_else] * tangent_vec[mask_else]) regularized_vec[mask_else] = (regularized_vec[mask_else] / coef[mask_else]) else: # TODO(nina): regularization needed in nD? regularized_vec = tangent_vec elif point_type == 'matrix': # TODO(nina): regularization in terms # of skew-symmetric matrices? regularized_vec = tangent_vec return regularized_vec
def regularize_tangent_vec_at_identity(self, tangent_vec, metric=None): """ In 3D, regularize a tangent_vector by getting its norm at the identity, determined by the metric, to be less than pi, following the regularization convention. """ assert self.point_representation in ('vector', 'matrix') if self.point_representation is 'vector': tangent_vec = gs.to_ndarray(tangent_vec, to_ndim=2) _, vec_dim = tangent_vec.shape if metric is None: metric = self.left_canonical_metric tangent_vec_metric_norm = metric.norm(tangent_vec) tangent_vec_canonical_norm = gs.linalg.norm(tangent_vec, axis=1) if tangent_vec_canonical_norm.ndim == 1: tangent_vec_canonical_norm = gs.expand_dims( tangent_vec_canonical_norm, axis=1) mask_norm_0 = gs.isclose(tangent_vec_metric_norm, 0) mask_canonical_norm_0 = gs.isclose(tangent_vec_canonical_norm, 0) mask_0 = mask_norm_0 | mask_canonical_norm_0 mask_else = ~mask_0 mask_0 = gs.squeeze(mask_0, axis=1) mask_else = gs.squeeze(mask_else, axis=1) coef = gs.empty_like(tangent_vec_metric_norm) regularized_vec = tangent_vec regularized_vec[mask_0] = tangent_vec[mask_0] coef[mask_else] = (tangent_vec_metric_norm[mask_else] / tangent_vec_canonical_norm[mask_else]) regularized_vec[mask_else] = self.regularize( coef[mask_else] * tangent_vec[mask_else]) regularized_vec[mask_else] = (regularized_vec[mask_else] / coef[mask_else]) else: # TODO(nina): regularization needed in nD? regularized_vec = tangent_vec return regularized_vec