File size: 2,387 Bytes
26ce2a9 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 |
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]) # 2x2
Cpt1 = C.dot(pt1)
Cpt2 = C.dot(pt2)
Cpts = C.dot(pts)
assert(
np.allclose(Cpt1, Cpts[0])
and np.allclose(Cpt2, Cpts[1])
)
|