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 __iter__(self):
if not self.check_input():
raise StopIteration()
rcls = self.make_record_class()
parent_fields = self.input_fieldnames
absolute_orientation_inv = self.absolute_orientation.invert()
for record in self.recordsource:
attrs = dict((k, getattr(record, k)) for k in parent_fields)
attrs['haptic_pose'] = self.forward_kinematics.calculate_pose(
record.jointangles, record.gimbalangles, record=record)
# HIP target pose in HDorigin
hiptarget_rotation = math.Quaternion(
(absolute_orientation_inv * record.externaltracker_pose *
self.tooltip_offset).rotation())
ht_pose_no_trans = math.Pose(hiptarget_rotation,
np.array([0, 0, 0]))
# re-orient zrefaxis_calib using hiptarget pose
zrefaxis = ht_pose_no_trans * self.zrefaxis_calib
# zrefaxis = hiptarget_rotation.transformVector(self.zrefaxis_calib)
attrs['zrefaxis'] = zrefaxis / np.linalg.norm(zrefaxis)
yield rcls(**attrs)
def test_create_quaternion():
m = np.array([[1., 2., 3., 4.], [1., 2., 3., 4.], [1., 2., 3., 4.],
[1., 2., 3., 4.]])
p = utmath.Pose(m)
axis, angle = p.rotation().toAxisAngle()
q = utmath.Quaternion(axis, angle)
assert str(q) == str(p.rotation())
#axis angle initialization not working
v = np.array([1., 0., 0.])
q = utmath.Quaternion(v, 0.)
assert np.all(q.toVector() == np.array([
0.,
0.,
0.,
1.,
]))
def disabled_t_e_s_t_s():
return
assert b.value == 1
d = utmath.ScalarDouble(1.0)
assert d.value == 1.0
q = utmath.test_quat()
assert q.x() == 0 and q.y() == 0 and q.z() == 0 and q.w() == 1
m = utmath.test_mat33()
print m
print np.identity(3)
assert np.all(m == np.identity(3))
# from vector
p = utmath.Pose(q, m[0, :])
q1 = utmath.Quaternion(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 * np.asarray([1., 2., 3.])
def pull_cb(ts):
global pullsrc_ctr
from ubitrack.core import math, measurement
import numpy as np
pullsrc_ctr += 1
p = math.Pose(math.Quaternion(), np.array([1, 2, 3]))
return measurement.Pose(ts, p)
def test_create_identity(self):
result = utmath.Quaternion().toVector()
expected = np.array([0.0, 0.0, 0.0, 1.0])
self.assertTrue(np.array_equal(result, expected),
"Quaternion identity incorrect")
def identity():
quat = utmath.Quaternion()
result = utmath.abs(quat)
expected = 1.0
self.assertEqual(
result, expected,
"Identity quaternion length calculation incorrect")
def test_normalise_identity():
# normalise an identity quaternion
quat = utmath.Quaternion()
quat.normalize()
expected = np.array([0.0, 0.0, 0.0, 1.0])
# assert np.array_equal(
# expected,
# quat / math.sqrt( np.sum( quat ** 2 ) )
# )
assert np.array_equal(quat.toVector(), expected)
def handle_data(self, c):
if self.connector.calib_absolute_orientation is not None:
ao = self.connector.calib_absolute_orientation.get()
self.results_txt.text = "Result:\n%s" % str(ao)
if self.last_result is not None:
t_error = norm(ao.translation() -
self.last_result.translation())
self.errors_translation[0] = t_error
# implement simple ringbuffer
self.errors_translation = np.roll(self.errors_translation, 1)
if self.initial_error_translation == -1:
self.initial_error_translation = t_error
r_error = abs(
math.Quaternion(ao.rotation().inverted() *
self.last_result.rotation()).angle())
self.errors_rotation[0] = r_error
# implement simple ringbuffer
self.errors_rotation = np.roll(self.errors_rotation, 1)
if self.initial_error_rotation == -1:
self.initial_error_rotation = r_error
self.last_result = ao
# update progress bar
if self.initial_error_translation != -1 and self.initial_error_translation != -1:
t_p = t_error / (self.initial_error_translation -
self.max_error_translation)
r_p = r_error / (self.initial_error_rotation -
self.max_error_rotation)
pv = int(np.sqrt(1 - max(0, min(max(t_p, r_p), 1))) * 100)
if pv > self.progress_bar.value:
self.progress_bar.value = pv
# check if the minimum of self.result_count results have been received
if not np.isnan(np.sum(self.errors_translation)) and not np.isnan(
np.sum(self.errors_rotation)):
if np.all(self.errors_translation < self.max_error_translation) and \
np.all(self.errors_rotation < self.max_error_rotation):
log.info(
"Absolute Orientation: Results are satisfactory for translation (<%s) min: %s max: %s and rotation (<%s) min: %s max %s"
%
(self.max_error_translation,
np.min(self.errors_translation),
np.max(self.errors_translation),
self.max_error_rotation, np.min(self.errors_rotation),
np.max(self.errors_rotation)))
self.result_ok = True
self.progress_bar.value = 100
if self.autocomplete_enable:
self.stopCalibration()
def rotated_z():
quat = utmath.Quaternion(math.pi, 0.0, 0.0)
vec = np.array([1.0, 0.0, 0.0])
result = quat.transformVector(vec)
expected = -vec
self.assertTrue(
np.allclose(result, expected),
"Quaternion apply_to_vector incorrect with rotation about Y")
def identity():
quat = utmath.Quaternion()
vec = np.array([1.0, 0.0, 0.0])
result = quat.transformVector(vec)
expected = vec
self.assertTrue(
np.array_equal(result, expected),
"Quaternion apply_to_vector incorrect with identity")
def test_create_quaternion(self):
m = np.array([[1., 2., 3., 4.], [1., 2., 3., 4.], [1., 2., 3., 4.],
[1., 2., 3., 4.]])
p = utmath.Pose(m)
axis, angle = p.rotation().toAxisAngle()
q = utmath.Quaternion(axis, angle)
self.assertTrue(
str(q) == str(p.rotation()),
"quaternion created from axis angle is not equal to the original")
#axis angle initialization not working
v = np.array([1, 0, 0])
q = utmath.Quaternion(v, 0.)
print q.toVector()
self.assertTrue(np.all(q.toVector() == np.array([
0.,
0.,
0.,
1.,
])), "quaternion created from axis angle has wrong data")
def identity():
# normalise an identity quaternion
quat = utmath.Quaternion()
quat.normalize()
expected = np.array([0.0, 0.0, 0.0, 1.0])
# assert np.array_equal(
# expected,
# quat / math.sqrt( np.sum( quat ** 2 ) )
# )
self.assertTrue(np.array_equal(quat.toVector(), expected),
"Normalise identity quaternion incorrect")
def test_measurement_rotation():
"test measurement Rotation"
i = measurement.Rotation()
assert i.invalid()
assert i.get() == None
q = utmath.Quaternion()
b = measurement.Rotation(123, q)
quat = b.get()
assert np.all(quat.toVector() == q.toVector())
assert b.time() == 123
assert b.invalid() == False
def check_item(self, item):
if isinstance(item, math.Pose):
item = item.rotation()
if self.last_rotation is None:
self.last_rotation = item
return False
d = abs(math.Quaternion(self.last_rotation.inverted() * item).angle())
if d >= self.min_distance and d <= self.max_distance:
self.last_rotation = item
return True
return False
def test_basic_datatypes():
"test basic data types: Vector2-8, Matrix33-44, Pose, Quaternion"
ts = measurement.now()
p = measurement.Position(ts, np.array([1.0, 2.0, 3.0]))
util.writeCalibMeasurementPosition(tmpfile, p)
pr = util.readCalibMeasurementPosition(tmpfile)
assert np.all(p.get() == pr)
p = measurement.Rotation(ts, utmath.Quaternion())
util.writeCalibMeasurementRotation(tmpfile, p)
pr = util.readCalibMeasurementRotation(tmpfile)
assert p.get() == pr
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 loadCalibrationFiles(root_dir):
if isinstance(root_dir, unicode):
root_dir = root_dir.encode(sys.getdefaultencoding())
fname = os.path.join(root_dir, "phantom_jointangle_correction.calib")
if os.path.isfile(fname):
phantom_jointangle_calib = util.readCalibMeasurementMatrix3x3(fname)
log.info(
"Phantom Joint Angle Calibration\n%s" %
(phantom_jointangle_calib), )
else:
log.warn("Phantom Joint Angle Calibration NOT FOUND")
phantom_jointangle_calib = np.array([0.0, 1.0, 0.0] * 3).reshape(
(3, 3))
phantom_gimbalangle_calib = np.array([0.0, 1.0, 0.0] * 3).reshape((3, 3))
fname = os.path.join(root_dir, "absolute_orientation_calibration.calib")
if os.path.isfile(fname):
externaltracker_to_device = util.readCalibMeasurementPose(fname)
log.info("OptiTrack to Device Transform\n%s" %
(externaltracker_to_device, ))
else:
log.warn("Absolute Orientation Calibration NOT FOUND")
externaltracker_to_device = math.Pose(math.Quaternion(),
np.array([0.0, 0.0, 0.0]))
fname = os.path.join(root_dir, "tooltip_calibration.calib")
if os.path.isfile(fname):
tooltip_calib = util.readCalibMeasurementPosition(fname)
log.info("Tooltip Calibration\n%s" % (tooltip_calib, ))
else:
log.warn("Tooltip Calibration NOT FOUND")
tooltip_calib = np.array([0.0, 0.0, 0.0])
return dict(
phantom_jointangle_calib=phantom_jointangle_calib,
phantom_gimbalangle_calib=phantom_gimbalangle_calib,
externaltracker_to_device=externaltracker_to_device,
tooltip_calib=tooltip_calib,
)
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)
def test_create_identity():
result = utmath.Quaternion().toVector()
expected = np.array([0.0, 0.0, 0.0, 1.0])
assert np.array_equal(result, expected)
def pull_cb(ts):
from ubitrack.core import math, measurement
import numpy as np
p = math.Pose(math.Quaternion(), np.array([1., 2., 3.],
dtype=np.double))
return measurement.Pose(ts, p)
def position3d_to_pose(pos):
return math.Pose(math.Quaternion(), pos)
def test_length_identity():
quat = utmath.Quaternion()
result = utmath.abs(quat)
expected = 1.0
assert result == expected
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)
