File size: 3,180 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 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 |
import copy
import numpy as np
from liegroups.numpy import SE3
def test_identity():
T = SE3.identity()
assert isinstance(T, SE3)
def test_dot():
T = np.array([[0, 0, -1, 0.1],
[0, 1, 0, 0.5],
[1, 0, 0, -0.5],
[0, 0, 0, 1]])
T2 = T.dot(T)
assert np.allclose(
(SE3.from_matrix(T).dot(SE3.from_matrix(T))).as_matrix(), T2)
def test_wedge_vee():
xi = [1, 2, 3, 4, 5, 6]
Xi = SE3.wedge(xi)
xis = np.array([[1, 2, 3, 4, 5, 6], [7, 8, 9, 10, 11, 12]])
Xis = SE3.wedge(xis)
assert np.array_equal(xi, SE3.vee(Xi))
assert np.array_equal(xis, SE3.vee(Xis))
def test_curlywedge_curlyvee():
xi = [1, 2, 3, 4, 5, 6]
Psi = SE3.curlywedge(xi)
xis = np.array([[1, 2, 3, 4, 5, 6], [7, 8, 9, 10, 11, 12]])
Psis = SE3.curlywedge(xis)
assert np.array_equal(xi, SE3.curlyvee(Psi))
assert np.array_equal(xis, SE3.curlyvee(Psis))
def test_odot():
p1 = [1, 2, 3]
p2 = [1, 2, 3, 1]
p3 = [1, 2, 3, 0]
odot12 = np.vstack([SE3.odot(p1), np.zeros([1, 6])])
odot13 = np.vstack([SE3.odot(p1, directional=True), np.zeros([1, 6])])
odot2 = SE3.odot(p2)
odot3 = SE3.odot(p3)
assert np.array_equal(odot12, odot2)
assert np.array_equal(odot13, odot3)
def test_odot_vectorized():
p1 = [1, 2, 3]
p2 = [2, 3, 4]
ps = np.array([p1, p2])
odot1 = SE3.odot(p1)
odot2 = SE3.odot(p2)
odots = SE3.odot(ps)
assert np.array_equal(odot1, odots[0, :, :])
assert np.array_equal(odot2, odots[1, :, :])
def test_exp_log():
T = SE3.exp([1, 2, 3, 4, 5, 6])
assert np.allclose(SE3.exp(SE3.log(T)).as_matrix(), T.as_matrix())
def test_left_jacobian():
xi1 = [1, 2, 3, 4, 5, 6]
assert np.allclose(
SE3.left_jacobian(xi1).dot(SE3.inv_left_jacobian(xi1)),
np.identity(6)
)
xi2 = [0, 0, 0, 0, 0, 0]
assert np.allclose(
SE3.left_jacobian(xi2).dot(SE3.inv_left_jacobian(xi2)),
np.identity(6)
)
def test_perturb():
T = SE3.exp([1, 2, 3, 4, 5, 6])
T_copy = copy.deepcopy(T)
xi = [0.6, 0.5, 0.4, 0.3, 0.2, 0.1]
T.perturb(xi)
assert np.allclose(T.as_matrix(), (SE3.exp(xi).dot(T_copy)).as_matrix())
def test_normalize():
T = SE3.exp([1, 2, 3, 4, 5, 6])
T.rot.mat += 0.1
T.normalize()
assert SE3.is_valid_matrix(T.as_matrix())
def test_inv():
T = SE3.exp([1, 2, 3, 4, 5, 6])
assert np.allclose((T.dot(T.inv())).as_matrix(), np.identity(4))
def test_adjoint():
T = SE3.exp([1, 2, 3, 4, 5, 6])
assert T.adjoint().shape == (6, 6)
def test_transform_vectorized():
T = SE3.exp([1, 2, 3, 4, 5, 6])
pt1 = np.array([1, 2, 3])
pt2 = np.array([4, 5, 6])
pt3 = np.array([1, 2, 3, 1])
pt4 = np.array([4, 5, 6, 1])
pts12 = np.array([pt1, pt2]) # 2x3
pts34 = np.array([pt3, pt4]) # 2x4
Tpt1 = T.dot(pt1)
Tpt2 = T.dot(pt2)
Tpt3 = T.dot(pt3)
Tpt4 = T.dot(pt4)
Tpts12 = T.dot(pts12)
Tpts34 = T.dot(pts34)
assert np.allclose(Tpt1, Tpts12[0])
assert np.allclose(Tpt2, Tpts12[1])
assert np.allclose(Tpt3, Tpts34[0])
assert np.allclose(Tpt4, Tpts34[1])
|