def plot(x, y=None, *args, **kwargs): """ Smart version of pylab.plot(...) that understands types used in IMUsim. The supported y value types are: - L{TimeSeries} data - Lists or tuples of Quaternions - Lists or tuples of column vectors - Arrays of column vectors """ singleArg = (y is None) if singleArg: if isinstance(x, TimeSeries): plotTimeSeries(x, *args, **kwargs) else: y = x if isinstance(y, (list, tuple)): if isinstance(y[0], Quaternion): y = QuaternionArray(y).array elif isinstance(y[0], np.ndarray): y = np.hstack(y).T elif isinstance(y, QuaternionArray): y = y.array elif isinstance(y, np.ndarray): if y.ndim == 2: y = y.T
def checkStaticConvergence(filterClass, performTest=True): """ Test that an orientation estimation algorithm converges within a reasonable number of samples """ gyro = np.zeros((3, 1)) accel = -GRAVITY mag = NORTH SAMPLES = 500 initialRotation = Quaternion.fromEuler((60, 45, 0)) filter = filterClass(initialRotation=initialRotation, initialTime=0, initialRotationalVelocity=np.zeros((3, 1)), **filterParameters.get(filterClass, {})) estimatedQuaternions = QuaternionArray(np.empty((SAMPLES, 4))) for i in range(SAMPLES): filter(accel, mag, gyro, dt * (1 + i)) estimatedQuaternions[i] = filter.rotation.latestValue if performTest: assertMaximumErrorAngle(Quaternion(), filter.rotation.latestValue, 2) return estimatedQuaternions
def rotation(self, t): if np.shape(t) == (): return self.staticRotation else: rot = np.empty((len(t),4)) rot[:] = self.staticRotation.components return QuaternionArray(rot)
def _makeValuesArray(self, values): if self.dtype is Quaternion: return QuaternionArray(values) elif issubclass(self._dtype, np.ndarray) and len( self._dshape) == 2 and self._dshape[1] == 1: return np.hstack(values) else: return np.array(values)
def rotation(self, t): if len(self.rotationKeyFrames) == 0: if np.ndim(t) == 0: return Quaternion.nan() else: return QuaternionArray.nan(len(t)) else: return self.rotationKeyFrames(t)
def load_trajectory(group): pos_ts = load_timeseries(group['position']) rot_array_ts = load_timeseries(group['rotation']) rot_ts = TimeSeries(rot_array_ts.timestamps, QuaternionArray(rot_array_ts.values.T)) sampled = SampledTrajectory(positionKeyFrames=pos_ts, rotationKeyFrames=rot_ts) splined = SplinedTrajectory(sampled, smoothRotations=False) return splined
def randomRotationSequence(t): """ A random curving path of rotation values at given times. """ theta = randomAngles() omega = rng.uniform(-2, 2, (3,1)) phi = randomAngles() omicron = rng.uniform(-10, 10, (3,1)) a = rng.uniform(-1, 1, (3,1)) angles = theta + omega*t + a*np.sin(omicron*t + phi) return QuaternionArray([ QuaternionFromEuler(angles[:,i].T, inDegrees=False) for i in range(len(t))])
def geodesicQuaternionPath(): """ Generate an array of quaternions following a geodesic path Returns ------- t : :class:`~numpy.ndarray` timestamps of quaternions quaternionPath : :class:`~imusim.maths.quaternions.QuaternionArray` array of quaternions following a geodesic path """ t = np.arange(0,10,0.1) q = Quaternion.fromEuler((45,0,0)) return t, QuaternionArray([q**T for T in t])
def runOrientationFilter(filter, trajectory): t = np.arange(trajectory.startTime + dt, trajectory.endTime, dt) trueOrientations = trajectory.rotation(t) gyroSamples = trueOrientations.rotateFrame( trajectory.rotationalVelocity(t)) accelSamples = trueOrientations.rotateFrame(-GRAVITY) magSamples = trueOrientations.rotateFrame(NORTH) estimatedOrientations = QuaternionArray( np.empty_like(trueOrientations.array)) for i in range(gyroSamples.shape[1]): accel = accelSamples[:, i:i + 1] mag = magSamples[:, i:i + 1] gyro = gyroSamples[:, i:i + 1] estimatedOrientations[i] = filter.rotation.latestValue filter(accel, mag, gyro, t[i]) return trueOrientations, estimatedOrientations
def __init__(self, posMarker, refMarkers, refVectors=None, refTime=0): self.posMarker = posMarker self.refMarkers = refMarkers if refVectors is None: self.refVectors = [ refMarker.position(refTime) - posMarker.position(refTime) for refMarker in self.refMarkers ] else: self.refVectors = refVectors assert (~np.any(np.isnan(self.refVectors))) self.positionKeyFrames = self.posMarker.positionKeyFrames t = self.positionKeyFrames.timestamps vectorObservation = DavenportQ(self.refVectors) measVectors = np.array([ ref.position(t) - self.posMarker.position(t) for ref in self.refMarkers ]) rotations = QuaternionArray([ vectorObservation(*vectors).conjugate for vectors in np.split(measVectors, len(t), axis=2) ]) self.rotationKeyFrames = TimeSeries(t, rotations)
def loadQualisysTSVFile(filename): """ Load 3DOF or 6DOF marker data from a Qualisys Track Manager TSV file. @param filename: Name of TSV file to load. @return: A L{MarkerCapture} instance. """ # Open file to read header. datafile = open(filename, 'r') # Count of header lines in file. headerLines = 0 capture = MarkerCapture() markerIndices = dict() while True: # Read and count each file header line. line = datafile.readline().strip('\r\n') headerLines = headerLines + 1 # Key and values are tab-separated. items = line.split('\t') key = items[0] values = items[1:] # Handle relevant fields. if key == 'FREQUENCY': # Frame rate. capture.frameRate = int(values[0]) capture.framePeriod = 1/capture.frameRate elif key == 'DATA_INCLUDED': # 3D or 6D data. type = values[0] if (type == '3D'): # 3D data fields are implicitly XYZ co-ordinates. fieldNames = ['X', 'Y', 'Z'] elif key == 'BODY_NAMES' or key == 'MARKER_NAMES': # List of markers or 6DOF body names. for i in range(len(values)): name = values[i] if type == '6D': markerClass = Marker6DOF else: markerClass = Marker3DOF markerIndices[markerClass(capture, name)] = i if (type == '6D'): # Field names are on a line after the body names, each # beginning with a space. fieldNames = [x.strip(' ') for x in datafile.readline().strip('\r\n').split('\t')] headerLines = headerLines + 1 # The above keys are always on the last line in the header. datafile.close() break # Load values from file, skipping header. data = np.loadtxt(filename, skiprows = headerLines) capture.frameCount = len(data) capture.frameTimes = np.arange(0, capture.frameCount / capture.frameRate, capture.framePeriod) # Find index in data array for a given marker and marker field name. def index(marker, name): offset = markerIndices[marker] * len(fieldNames) return offset + fieldNames.index(name) # Return data for a given marker and marker field names. def fields(marker, names): indices = [index(marker, name) for name in names] return data[:,indices[0]:indices[-1]+1] # Iterate through markers to fill in their data. for marker in capture.markers: # Get position data. positions = fields(marker, ['X', 'Y', 'Z']) / 1000 if type == '6D': # Get rotation matrices. matrix_coeffs = fields(marker, ['Rot[%d]' % i for i in range(9)]) # Mark invalid data (residual = -1) with NaNs. invalid = fields(marker, ['Residual'])[:,0] == -1.0 matrix_coeffs[invalid] = np.nan positions[invalid] = np.nan # Convert to quaternions and unflip. quats = QuaternionArray(np.empty((capture.frameCount,4))) for i in range(len(quats)): q = Quaternion() q.setFromMatrix(matrix_coeffs[i].reshape((3,3)).T) quats[i] = q rotations = quats.unflipped() # Add rotation data to marker. for time, rotation in zip(capture.frameTimes, rotations): marker.rotationKeyFrames.add(time, rotation) # Add position data to marker. for time, position in zip(capture.frameTimes, positions): marker.positionKeyFrames.add(time, position.reshape(3,1)) return capture
def rotation(self, t): if np.ndim(t) == 0: return self.qW ** t else: return QuaternionArray([self.qW ** ti for ti in t])
def loadQualisysTSVFile(filename): """ Load 3DOF or 6DOF marker data from a Qualisys Track Manager TSV file. @param filename: Name of TSV file to load. @return: A L{MarkerCapture} instance. """ # Open file to read header. datafile = open(filename, 'r') # Count of header lines in file. headerLines = 0 capture = MarkerCapture() markerIndices = dict() while True: # Read and count each file header line. line = datafile.readline().strip('\r\n') headerLines = headerLines + 1 # Key and values are tab-separated. items = line.split('\t') key = items[0] values = items[1:] # Handle relevant fields. if key == 'FREQUENCY': # Frame rate. capture.frameRate = int(values[0]) capture.framePeriod = 1 / capture.frameRate elif key == 'DATA_INCLUDED': # 3D or 6D data. type = values[0] if (type == '3D'): # 3D data fields are implicitly XYZ co-ordinates. fieldNames = ['X', 'Y', 'Z'] elif key == 'BODY_NAMES' or key == 'MARKER_NAMES': # List of markers or 6DOF body names. for i in range(len(values)): name = values[i] if type == '6D': markerClass = Marker6DOF else: markerClass = Marker3DOF markerIndices[markerClass(capture, name)] = i if (type == '6D'): # Field names are on a line after the body names, each # beginning with a space. fieldNames = [ x.strip(' ') for x in datafile.readline().strip('\r\n').split('\t') ] headerLines = headerLines + 1 # The above keys are always on the last line in the header. datafile.close() break # Load values from file, skipping header. data = np.loadtxt(filename, skiprows=headerLines) capture.frameCount = len(data) capture.frameTimes = np.arange(0, capture.frameCount / capture.frameRate, capture.framePeriod) # Find index in data array for a given marker and marker field name. def index(marker, name): offset = markerIndices[marker] * len(fieldNames) return offset + fieldNames.index(name) # Return data for a given marker and marker field names. def fields(marker, names): indices = [index(marker, name) for name in names] return data[:, indices[0]:indices[-1] + 1] # Iterate through markers to fill in their data. for marker in capture.markers: # Get position data. positions = fields(marker, ['X', 'Y', 'Z']) / 1000 if type == '6D': # Get rotation matrices. matrix_coeffs = fields(marker, ['Rot[%d]' % i for i in range(9)]) # Mark invalid data (residual = -1) with NaNs. invalid = fields(marker, ['Residual'])[:, 0] == -1.0 matrix_coeffs[invalid] = np.nan positions[invalid] = np.nan # Convert to quaternions and unflip. quats = QuaternionArray(np.empty((capture.frameCount, 4))) for i in range(len(quats)): q = Quaternion() q.setFromMatrix(matrix_coeffs[i].reshape((3, 3)).T) quats[i] = q rotations = quats.unflipped() # Add rotation data to marker. for time, rotation in zip(capture.frameTimes, rotations): marker.rotationKeyFrames.add(time, rotation) # Add position data to marker. for time, position in zip(capture.frameTimes, positions): marker.positionKeyFrames.add(time, position.reshape(3, 1)) return capture
def load_timeseries(group): timestamps = group['timestamps'].value data = group['data'].value if data.shape[1] == 4: data = QuaternionArray(data) return TimeSeries(timestamps, data)
def gyro_data_to_quaternion_array(gyro_data, gyro_times): dt = float(gyro_times[1] - gyro_times[0]) nt.assert_almost_equal(np.diff(gyro_times), dt) q = integrate_gyro_quaternion_uniform(gyro_data, dt) return QuaternionArray(q)