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
|
import numpy as np
import pytest
from manifpy import SE3, SE3Tangent
def test_constructor():
state = SE3(0,0,0,0,0,0)
assert 0 == state.x()
assert 0 == state.y()
assert 0 == state.z()
# assert 1 == state.quat()
state = SE3(position=np.array([1,2,3]), quaternion=np.array([0,0,0,1]))
assert 1 == state.x()
assert 2 == state.y()
assert 3 == state.z()
assert ([0, 0, 0, 1] == state.quat()).all()
with pytest.raises(ValueError):
state = SE3(position=np.array([1,2,3]), quaternion=np.array([1, 0, 0, 1]))
# state = SE3(np.array([1,2,3]), AngleAxis(0, UnitX()))
# assert 0 == state.x()
# assert 0 == state.y()
# assert 0 == state.z()
# delta = SE3Tangent(1,2,3,4,5,6)
# assert 1 == delta.x()
# assert 2 == delta.y()
# assert 3 == delta.z()
def test_accessors():
state = SE3.Identity()
assert 0 == state.x()
assert 0 == state.y()
assert 0 == state.z()
assert (np.zeros([1,3]) == state.translation()).all()
assert ([0, 0, 0, 1] == state.quat()).all()
state.translation([1, 2, 3])
assert ([1, 2, 3] == state.translation()).all()
state.quat([0, 1, 0, 0])
assert ([0, 1, 0, 0] == state.quat()).all()
with pytest.raises(ValueError):
state.quat([0, 1, 0, 1])
delta = SE3Tangent.Zero()
# assert 0 == delta.x()
# assert 0 == delta.y()
# assert 0 == delta.z()
def test_transform():
state = SE3.Identity()
transform = state.transform()
assert (4, 4) == transform.shape
assert (np.identity(4) == transform).all()
def test_rotation():
state = SE3.Identity()
rotation = state.rotation()
assert (3, 3) == rotation.shape
assert (np.identity(3) == rotation).all()
# def test_quaternion():
# state = SO3.Identity()
# quaternion = state.quaternion()
#
# assert (4,) == quaternion.shape
# assert (np.zeros(4,) == quaternion).all()
# def test_translation():
# state = SE3.Identity()
# translation = state.translation()
#
# assert (3,) == translation.shape
# assert (np.zeros(3,) == translation).all()
|
