def __mul__(self, element2): x1 = self.tx x2 = element2.tx y1 = self.ty y2 = element2.ty theta_1 = angles.mod_pipi(self.rotation_angle) c1 = cos(theta_1) s1 = sin(theta_1) alpha = angles.mod_pipi(self.rotation_angle + element2.rotation_angle) x = x1 + x2 * c1 - y2 * s1 y = y1 + x2 * s1 + y2 * c1 return Se2G(alpha, x, y)
def test_se2_a_norm_translation_fixed_input_non0(): angle, tx, ty = se2.Se2A( 4, 5, 6).quot.get # input data has to be considered in the quotient space element = se2.Se2A(angle, tx, ty) factmod = angles.mod_pipi(angle) / angle assert_equal(element.norm('translation'), factmod * np.sqrt(tx**2 + ty**2))
def test_se2_a_comparing_sum_restricted_form_and_matrix_form(): any_angle_1 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_1 = uniform(-10, 10) any_ty_1 = uniform(-10, 10) any_angle_2 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_2 = uniform(-10, 10) any_ty_2 = uniform(-10, 10) elem1 = se2.Se2A(any_angle_1, any_tx_1, any_ty_1) elem2 = se2.Se2A(any_angle_2, any_tx_2, any_ty_2) print(elem1.get) print(elem2.get) matrix_output_of_sums = (elem1 + elem2).get_matrix # the matrix here must be constructed carefully. It is not the sum of the matrix # but some operations (_mod) must be done on its elements to have the quotient. matrix_sum_output = \ se2.Se2A(any_angle_1, any_tx_1, any_ty_1).get_matrix + se2.Se2A(any_angle_2, any_tx_2, any_ty_2).get_matrix theta_mod = angles.mod_pipi(matrix_sum_output[1, 0]) if not matrix_sum_output[1, 0] == theta_mod: modfact = theta_mod / matrix_sum_output[1, 0] matrix_sum_output[0, 2] = matrix_sum_output[0, 2] * modfact matrix_sum_output[1, 2] = matrix_sum_output[1, 2] * modfact matrix_sum_output[0, 1] = -theta_mod matrix_sum_output[1, 0] = theta_mod assert_array_almost_equal(matrix_output_of_sums, matrix_sum_output)
def test_se2_a_add_random_input(): any_angle_1 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_1 = uniform(-10, 10) any_ty_1 = uniform(-10, 10) any_angle_2 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_2 = uniform(-10, 10) any_ty_2 = uniform(-10, 10) elem1 = se2.Se2A(any_angle_1, any_tx_1, any_ty_1) elem2 = se2.Se2A(any_angle_2, any_tx_2, any_ty_2) # this sum is must be done mod the relation given by exp sum_angle = any_angle_1 + any_angle_2 sum_tx = any_tx_1 + any_tx_2 sum_ty = any_ty_1 + any_ty_2 sum_angle_mod = angles.mod_pipi(sum_angle) if not sum_angle == sum_angle_mod: modfact = sum_angle_mod / sum_angle sum_tx = modfact * sum_tx sum_ty = modfact * sum_ty elem_sum = se2.Se2A(sum_angle_mod, sum_tx, sum_ty) print(elem1.get) print(elem2.get) print(elem_sum.get) assert_array_equal((elem1 + elem2).get, elem_sum.get)
def test_se2_a_norm_fro_fixed_input_non0(): angle, tx, ty = se2.Se2A( 4, 5, 6).quot.get # input data has to be considered in the quotient space element = se2.Se2A(angle, tx, ty) factmod = angles.mod_pipi(angle) / angle assert_equal(element.norm('fro'), np.sqrt(2 * angle**2 + factmod**2 * (tx**2 + ty**2)))
def test_se2_a_norm_standard_fixed_input_non0(): angle, tx, ty = se2.Se2A( 4, 5, 6).quot.get # input data has to be considered in the quotient space lamb = 1 element = se2.Se2A(angle, tx, ty) assert_equal(element.norm('lamb', lamb), np.sqrt(angles.mod_pipi(angle)**2 + lamb * (tx**2 + ty**2)))
def test_se2_a_quotient_projection_greater_than_pi(): theta_in = np.pi + 0.3 tx_in = 3 ty_in = 4 fact = angles.mod_pipi(theta_in) / theta_in theta_out = -np.pi + 0.3 tx_out = tx_in * fact ty_out = ty_in * fact assert_array_equal( se2.Se2A(theta_in, tx_in, ty_in).get, se2.Se2A(theta_out, tx_out, ty_out).get)
def __quotient_projection__(self): """ Maps the elements of se2_a in the quotient over the equivalence relation defined by exp, in order to have exp well defined. a ~ b iff exp(a) == exp(b) Here se2_a is intended as the quotient se2_a over the above equivalence relation. :param self: element of se2_a """ theta_quot = angles.mod_pipi(self.rotation_angle) if self.rotation_angle == theta_quot: tx_quot = self.tx ty_quot = self.ty else: modfact = angles.mod_pipi( self.rotation_angle) / self.rotation_angle tx_quot = self.tx * modfact ty_quot = self.ty * modfact return Se2A(theta_quot, tx_quot, ty_quot)
def __init__(self, theta, tx, ty): self.rotation_angle = angles.mod_pipi(theta) if self.rotation_angle == theta: self.tx = tx self.ty = ty else: modfact = self.rotation_angle / theta self.tx = modfact * tx self.ty = modfact * ty
def test_se2_a_lie_bracket_one_element_out_quotient(): any_angle_1 = np.pi + uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_1 = uniform(-10, 10) any_ty_1 = uniform(-10, 10) any_angle_2 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_2 = uniform(-10, 10) any_ty_2 = uniform(-10, 10) # the first input manually reduced to quotient space mod_fact = angles.mod_pipi(any_angle_1) / any_angle_1 any_angle_1 = angles.mod_pipi(any_angle_1) any_tx_1 *= mod_fact any_ty_1 *= mod_fact # elem1 = se2.Se2A(any_angle_1, any_tx_1, any_ty_1) elem2 = se2.Se2A(any_angle_2, any_tx_2, any_ty_2) exp_ans = [ 0, any_angle_2 * any_ty_1 - any_angle_1 * any_ty_2, any_angle_1 * any_tx_2 - any_angle_2 * any_tx_1 ] assert_array_almost_equal(se2.se2a_lie_bracket(elem1, elem2).get, exp_ans)
def test_se2_a_add_0_translation(): any_angle_1 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) tx_1 = 0 ty_1 = 0 any_angle_2 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) tx_2 = 0 ty_2 = 0 expected_output_angle = angles.mod_pipi(any_angle_1 + any_angle_2) expected_output_tx = 0 expected_output_ty = 0 expected_output = [ expected_output_angle, expected_output_tx, expected_output_ty ] given_output = (se2.Se2A(any_angle_1, tx_1, ty_1) + se2.Se2A(any_angle_2, tx_2, ty_2)).get assert_array_equal(given_output, expected_output)
def test_se2_g_mul_random_input(): any_angle_1 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_1 = uniform(-10, 10) any_ty_1 = uniform(-10, 10) any_angle_2 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi) any_tx_2 = uniform(-10, 10) any_ty_2 = uniform(-10, 10) expected_output_angle = angles.mod_pipi(any_angle_1 + any_angle_2) expected_output_tx = any_tx_1 + any_tx_2 * np.cos( any_angle_1) - any_ty_2 * np.sin(any_angle_1) expected_output_ty = any_ty_1 + any_tx_2 * np.sin( any_angle_1) + any_ty_2 * np.cos(any_angle_1) expected_output = [ expected_output_angle, expected_output_tx, expected_output_ty ] given_output = (se2.Se2G(any_angle_1, any_tx_1, any_ty_1) * se2.Se2G(any_angle_2, any_tx_2, any_ty_2)).get assert_array_equal(given_output, expected_output)
def se2g_log(element): """ log(element) \n group logarithm :param: instance of SE2 :return: corresponding element in Lie algebra se2 """ theta = angles.mod_pipi(element.rotation_angle) v1 = element.tx v2 = element.ty c = cos(theta) prec = np.abs(np.spacing([0.0]))[0] if abs(c - 1.0) <= 10 * prec: x1 = v1 x2 = v2 theta = 0 else: factor = (theta / 2.0) * sin(theta) / (1.0 - c) x1 = factor * v1 + (theta / 2.0) * v2 x2 = factor * v2 - (theta / 2.0) * v1 return Se2A(theta, x1, x2)
def test_se2_g_norm_standard_fixed_input_non0(): angle, tx, ty = 4, 5, 6 lamb = 1 element = se2.Se2G(angle, tx, ty) assert_equal(element.norm('standard', lamb), np.sqrt(angles.mod_pipi(angle)**2 + lamb * (tx**2 + ty**2)))
def test_se2_a_list2se2_a_good_input(): tt = [angles.mod_pipi(3.2), 1, 2] ans = se2.list2se2a(tt) assert_array_almost_equal(ans.get, tt)
def __init__(self, theta, tx, ty): self.rotation_angle = angles.mod_pipi(theta) self.tx = tx self.ty = ty