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)