def test_basic_datatypes():
"test basic data types: Vector2-8, Matrix33-44, Pose, Quaternion"
b = utmath.ScalarInt(1)
assert b.value == 1
d = utmath.ScalarDouble(1.0)
assert d.value == 1.0
q = utmath.Quaternion()
assert q.x() == 0 and q.y() == 0 and q.z() == 0 and q.w() == 1
m = utmath.Matrix33d.identity()
assert np.all(m == np.identity(3))
# from vector
p = utmath.Pose(q, np.asarray(m)[0, :])
q1 = utmath.Quaternion(np.asarray(m)[0, :], 1.0)
# from matrix
m1 = np.identity(4)
p1 = utmath.Pose(m1)
#test accessors
assert np.all(p1.translation() == np.asarray([0., 0., 0.]))
assert np.all(p1.rotation().toVector() == np.array([0., 0., 0., -1.]))
# needs proper verification
p2 = p * p1
p3 = p.invert()
v = p3 * utmath.Vector3d(1., 2., 3.)
def test_measurement_pos3d():
"test measurement Position(3D)"
i = measurement.Position()
assert i.invalid()
assert i.get() == None
b = measurement.Position(123, utmath.Vector3d(1.0, 2.0, 3.0))
assert np.all(b.get() == np.array([1.0, 2.0, 3.0]))
assert b.time() == 123
assert b.invalid() == False
def test_measurement_poselist():
"test measurement poselist"
poses = utmath.PoseList()
for i in range(5):
poses.push_back(
utmath.Pose(utmath.Quaternion(0.0, 0.0, 0.0, 1.0),
utmath.Vector3d(1.0, 2.0, 3.0)))
m = measurement.PoseList(measurement.now(), poses)
result = m.get()
assert len(result) == 5
element = result[0]
assert np.all(element.translation() == np.array([1.0, 2.0, 3.0]))
def test_apply_to_vector_identity():
quat = utmath.Quaternion()
vec = utmath.Vector3d(1.0, 0.0, 0.0)
result = quat * vec
expected = vec
assert np.array_equal(result, expected)
def test_apply_to_vector_rotated_z():
quat = utmath.Quaternion(math.pi, 0.0, 0.0)
vec = utmath.Vector3d(1.0, 0.0, 0.0)
result = quat * vec
expected = -np.asarray(vec)
assert np.allclose(result, expected)