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