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)
