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)