| import copy |
|
|
| import numpy as np |
|
|
| from liegroups.numpy import SO2 |
|
|
|
|
| def test_identity(): |
| C = SO2.identity() |
| assert isinstance(C, SO2) |
|
|
|
|
| def test_from_angle_to_angle(): |
| angle = np.pi / 2. |
| assert np.isclose(SO2.from_angle(angle).to_angle(), angle) |
|
|
|
|
| def test_dot(): |
| C = np.array([[0, -1], |
| [1, 0]]) |
| C2 = C.dot(C) |
| assert np.allclose((SO2(C).dot(SO2(C))).mat, C2) |
|
|
|
|
| def test_wedge(): |
| phi = 1 |
| Phi = SO2.wedge(phi) |
| phis = [1, 2] |
| Phis = SO2.wedge(phis) |
| assert np.array_equal(Phi, -Phi.T) |
| assert np.array_equal(Phis[0, :, :], SO2.wedge(phis[0])) |
| assert np.array_equal(Phis[1, :, :], SO2.wedge(phis[1])) |
|
|
|
|
| def test_wedge_vee(): |
| phi = 1 |
| Phi = SO2.wedge(phi) |
| phis = [1, 2] |
| Phis = SO2.wedge(phis) |
| assert phi == SO2.vee(Phi) |
| assert np.array_equal(phis, SO2.vee(Phis)) |
|
|
|
|
| def test_left_jacobians(): |
| phi_small = 0. |
| phi_big = np.pi / 2 |
|
|
| left_jacobian_small = SO2.left_jacobian(phi_small) |
| inv_left_jacobian_small = SO2.inv_left_jacobian(phi_small) |
| assert np.allclose(left_jacobian_small.dot(inv_left_jacobian_small), |
| np.identity(2)) |
|
|
| left_jacobian_big = SO2.left_jacobian(phi_big) |
| inv_left_jacobian_big = SO2.inv_left_jacobian(phi_big) |
| assert np.allclose(left_jacobian_big.dot(inv_left_jacobian_big), |
| np.identity(2)) |
|
|
|
|
| def test_exp_log(): |
| C = SO2.exp(np.pi / 4) |
| assert np.allclose(SO2.exp(SO2.log(C)).mat, C.mat) |
|
|
|
|
| def test_exp_log_zeros(): |
| C = SO2.exp(0) |
| assert np.allclose(SO2.exp(SO2.log(C)).mat, C.mat) |
|
|
|
|
| def test_perturb(): |
| C = SO2.exp(np.pi / 4) |
| C_copy = copy.deepcopy(C) |
| phi = 0.1 |
| C.perturb(phi) |
| assert np.allclose(C.as_matrix(), (SO2.exp(phi).dot(C_copy)).as_matrix()) |
|
|
|
|
| def test_normalize(): |
| C = SO2.exp(np.pi / 4) |
| C.mat += 0.1 |
| C.normalize() |
| assert SO2.is_valid_matrix(C.mat) |
|
|
|
|
| def test_inv(): |
| C = SO2.exp(np.pi / 4) |
| assert np.allclose(C.dot(C.inv()).mat, np.identity(2)) |
|
|
|
|
| def test_adjoint(): |
| C = SO2.exp(np.pi / 4) |
| assert C.adjoint() == 1. |
|
|
|
|
| def test_transform_vectorized(): |
| C = SO2.exp(np.pi / 4) |
| pt1 = np.array([1, 2]) |
| pt2 = np.array([4, 5]) |
| pts = np.array([pt1, pt2]) |
| Cpt1 = C.dot(pt1) |
| Cpt2 = C.dot(pt2) |
| Cpts = C.dot(pts) |
| assert( |
| np.allclose(Cpt1, Cpts[0]) |
| and np.allclose(Cpt2, Cpts[1]) |
| ) |
|
|
