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
