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
|
import numpy as np
import pytest
from manifpy import SE_2_3, SE_2_3Tangent
def test_constructor():
state = SE_2_3(0,0,0, 0,0,0, 0,0,0)
assert 0 == state.x()
assert 0 == state.y()
assert 0 == state.z()
# assert 0 == state.quat()
assert 0 == state.vx()
assert 0 == state.vy()
assert 0 == state.vz()
# state = SE_2_3(np.array([1,2,3]), Quaternion(1,0,0,0), np.array([4,5,6]))
# assert 1 == state.x()
# assert 2 == state.y()
# assert 3 == state.z()
# assert 1 == state.quat()
# assert 4 == state.vx()
# assert 5 == state.vy()
# assert 6 == state.vz()
# state = SE_2_3(np.array([1,2,3]), AngleAxis(0, UnitX()), np.array([4,5,6]))
# assert 1 == state.x()
# assert 2 == state.y()
# assert 3 == state.z()
# assert 1 == state.w()
# assert 4 == state.vx()
# assert 5 == state.vy()
# assert 6 == state.vz()
# delta = SE_2_3Tangent(1,2,3)
# assert 1 == delta.x()
# assert 2 == delta.y()
# assert 3 == delta.z()
def test_accessors():
state = SE_2_3.Identity()
assert 0 == state.x()
assert 0 == state.y()
assert 0 == state.z()
# assert 0 == state.quat()
assert 0 == state.vx()
assert 0 == state.vy()
assert 0 == state.vz()
delta = SE_2_3Tangent.Zero()
# assert 0 == delta.x()
# assert 0 == delta.y()
# assert 0 == delta.z()
# def test_isometry():
# state = SE_2_3.Identity()
# isometry = state.isometry()
#
# assert (4, 4) == isometry.shape
# assert (np.identity(4) == isometry).all()
def test_rotation():
state = SE_2_3.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 = SE_2_3.Identity()
translation = state.translation()
assert (3,) == translation.shape
assert (np.zeros(3,) == translation).all()
|
