def test_se2_a_lie_multi_bracket():
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)
any_angle_3 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
any_tx_3 = uniform(-10, 10)
any_ty_3 = uniform(-10, 10)
any_angle_4 = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
any_tx_4 = uniform(-10, 10)
any_ty_4 = 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)
elem3 = se2.Se2A(any_angle_3, any_tx_3, any_ty_3)
elem4 = se2.Se2A(any_angle_4, any_tx_4, any_ty_4)
l = [elem1, elem2, elem3, elem4]
ans_provided = se2.se2a_lie_multi_bracket(l).get
ans_expected = se2.se2a_lie_bracket(
elem1, se2.se2a_lie_bracket(elem2, se2.se2a_lie_bracket(elem3,
elem4))).get
assert_array_almost_equal(ans_provided, ans_expected)
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_add_element_to_null_equals_element():
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)
null_element = se2.Se2A(0, 0, 0)
element1 = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
assert_array_equal((element1 + null_element).get, element1.get)
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_element_plus_opposite_equals_null_element():
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)
element1 = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
element1_opp = se2.Se2A(-any_angle_1, -any_tx_1, -any_ty_1)
assert_array_equal((element1_opp + element1).get, [0, 0, 0])
def test_se2_a_opposite_random_element_verification_with_matrix():
angle = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
tx = uniform(-10, 10)
ty = uniform(-10, 10)
element = se2.Se2A(angle, tx, ty)
element_m = element.get_matrix
element_opp_m = se2.Se2A(-angle, -tx, -ty).get_matrix
assert_array_almost_equal(element_m + element_opp_m,
np.array([0] * 9).reshape(3, 3))
def test_se2_a_norm_translation_and_fro_not_invariance_for_angle_greater_than_pi(
):
angle_1, tx, ty = 2 * np.pi + 0.5, 5, 6
angle_2 = angle_1 + uniform(-np.pi, np.pi)
element_1 = se2.Se2A(angle_1, tx, ty)
element_2 = se2.Se2A(angle_2, tx, ty)
if element_1.norm('translation') == element_2.norm('translation') \
or element_1.norm('fro') == element_2.norm('fro'):
assert False
else:
assert True
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 test_se2_a_scalarpr_smaller_than_pi():
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)
scalar = 0.3
if any_angle_1 != 0:
scalar = uniform(0, np.pi / any_angle_1)
elem = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
expected_ans = se2.Se2A(scalar * any_angle_1, scalar * any_tx_1,
scalar * any_ty_1)
scalarpr_ans = elem.scalarprod(scalar)
assert_array_almost_equal(expected_ans.get, scalarpr_ans.get)
def test_se2_a_lie_bracket_both_zero_rotation_input():
angle_1 = 0
any_tx_1 = uniform(-10, 10)
any_ty_1 = uniform(-10, 10)
angle_2 = 0
any_tx_2 = uniform(-10, 10)
any_ty_2 = uniform(-10, 10)
elem1 = se2.Se2A(angle_1, any_tx_1, any_ty_1)
elem2 = se2.Se2A(angle_2, any_tx_2, any_ty_2)
exp_ans = [0, 0.0, 0.0]
assert_array_almost_equal(se2.se2a_lie_bracket(elem1, elem2).get, exp_ans)
def test_se2_a_lie_bracket_both_zero_translation_input():
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
elem1 = se2.Se2A(any_angle_1, tx_1, ty_1)
elem2 = se2.Se2A(any_angle_2, tx_2, ty_2)
exp_ans = [0, 0.0, 0.0]
assert_array_almost_equal(se2.se2a_lie_bracket(elem1, elem2).get, exp_ans)
def test_bch_ground_random_input_comparing_matrices_step_1():
any_angle_1 = uniform(-np.pi + 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 + abs(np.spacing(-np.pi)), np.pi)
any_tx_2 = uniform(-10, 10)
any_ty_2 = uniform(-10, 10)
a = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
da = se2.Se2A(any_angle_2, any_tx_2, any_ty_2)
a_matrix = a.get_matrix
da_matrix = da.get_matrix
exp_exp_pade = lin.expm(a_matrix).real.dot(lin.expm(da_matrix).real).real
exp_exp_my = se2.se2a_exp(a) * se2.se2a_exp(da)
assert_array_almost_equal(exp_exp_pade, exp_exp_my.get_matrix)
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_a_add_0_rotation():
angle_1 = 0
any_tx_1 = uniform(-10, 10)
any_ty_1 = uniform(-10, 10)
angle_2 = 0
any_tx_2 = uniform(-10, 10)
any_ty_2 = uniform(-10, 10)
expected_output_angle = 0
expected_output_tx = any_tx_1 + any_tx_2
expected_output_ty = any_ty_1 + any_ty_2
expected_output = [
expected_output_angle, expected_output_tx, expected_output_ty
]
given_output = (se2.Se2A(angle_1, any_tx_1, any_ty_1) +
se2.Se2A(angle_2, any_tx_2, any_ty_2)).get
assert_array_equal(given_output, expected_output)
def test_init_se2_a_random_input():
any_angle_in_pi_pi = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
any_tx = uniform(-10, 10)
any_ty = uniform(-10, 10)
assert_array_equal(
se2.Se2A(any_angle_in_pi_pi, any_tx, any_ty).get,
[any_angle_in_pi_pi, any_tx, any_ty])
def test_se2_a_lie_bracket_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)
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_theory_inverse_log_exp():
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)
a = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
ans = se2.se2g_log(se2.se2a_exp(a))
assert_array_almost_equal(a.get, ans.get)
def test_se2_a_exp_pade_approx_comparison():
theta = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
tx = uniform(-5, 5)
ty = uniform(-5, 5)
element = se2.Se2A(theta, tx, ty)
ans_exp = se2.se2a_exp(element).get_matrix
ans_pade = lin.expm(element.get_matrix)
assert_array_almost_equal(ans_exp, ans_pade)
def test_se2_a_matrix2se2_a_sane_input():
theta = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
tx = uniform(-5, 5)
ty = uniform(-5, 5)
m = np.array([[0, -theta, tx], [theta, 0, ty], [0, 0, 0]])
ans = se2.matrix2se2a(m)
exp_ans = se2.Se2A(theta, tx, ty)
assert_array_almost_equal(ans.get, exp_ans.get)
def test_se2_a_norm_standard_random_input():
angle = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
tx = uniform(-10, 10)
ty = uniform(-10, 10)
lamb = 1.4
element = se2.Se2A(angle, tx, ty)
assert_equal(element.norm('lamb', lamb),
np.sqrt(angle**2 + lamb * (tx**2 + ty**2)))
def test_se2_a_get_matrix():
any_angle_in_pi_pi = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
any_tx = uniform(-10, 10)
any_ty = uniform(-10, 10)
m = se2.Se2A(any_angle_in_pi_pi, any_tx, any_ty).get_matrix
assert m[0, 0] == m[1, 1] == 0 \
and -m[0, 1] == m[1, 0] == any_angle_in_pi_pi \
and m[2, 2] == 0 and m[0, 2] == any_tx and m[1, 2] == any_ty
def test_se2_a_scalarpr_greater_than_pi():
any_angle_1 = 2 * 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)
scalar = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
# obtain quotient and its list
elem_quot = se2.Se2A(any_angle_1, any_tx_1, any_ty_1)
elem_quot_get = elem_quot.get
# (scalar * elem).quot :
expected_ans = se2.Se2A(scalar * elem_quot_get[0],
scalar * elem_quot_get[1],
scalar * elem_quot_get[2])
# scalar * (elem.quot) :
scalarpr_ans = elem_quot.scalarprod(scalar)
assert_array_almost_equal(expected_ans.get, scalarpr_ans.get)
def test_se2_g_log_for_matrices_pade_approx_comparison():
theta = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
tx = uniform(-5, 5)
ty = uniform(-5, 5)
element_m = se2.Se2A(theta, tx, ty).get_matrix
ans_exp_m = se2.se2a_exp_matrices(element_m)
ans_pade = np.around(lin.expm(element_m), 10).real
print(ans_exp_m)
print(ans_pade)
assert_array_almost_equal(ans_exp_m, ans_pade)
def test_se2_a_get_matrix_det():
any_angle_in_pi_pi = uniform(-np.pi + np.abs(np.spacing(-np.pi)), np.pi)
any_tx = uniform(-10, 10)
any_ty = uniform(-10, 10)
m = se2.Se2A(any_angle_in_pi_pi, any_tx, any_ty).get_matrix
given_output = np.linalg.det(m)
expected_output = 0.0
global meaningful_decimals
assert round(np.abs(given_output - expected_output),
meaningful_decimals) == 0.0
def test_se2_a_rmul_():
angle = 0.98
tx = 5
ty = 7
scalar = 0.5
ans_expected = [scalar * angle, scalar * tx, scalar * ty]
scalar_prod = scalar * se2.Se2A(angle, tx, ty)
assert isinstance(scalar_prod, se2.Se2A)
ans_provided = [scalar_prod.rotation_angle, scalar_prod.tx, scalar_prod.ty]
assert_array_almost_equal(ans_provided, ans_expected)
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_exp_0_angle():
theta = 0
tx = uniform(-5, 5)
ty = uniform(-5, 5)
element = se2.Se2A(theta, tx, ty)
tx1 = tx
ty1 = ty
ans_exp = se2.se2a_exp(element)
if ans_exp.rotation_angle == 0 and ans_exp.tx == tx1 and ans_exp.ty == ty1:
assert True
else:
assert False
