def testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0), vector(0, 0, 0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1, 0, 0, 0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10, 0, 0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10, 0, 0)))
assertQuaternionAlmostEqual(
j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0, 10, 0))
assert_almost_equal(j1end.position(0), vector(0, 0, 0))
def checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)], [0], [math.sin(inclination)]])
g = np.array([[0, 0, 1]], dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q, vo(g, m))
def testBVHInput():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 1
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
"""
testFile = tempfile.NamedTemporaryFile()
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
assert len(list(model.points)) == 3
assert model.name == 'root'
assert len(model.channels) == 6
assert len(model.children) == 1
assert_almost_equal(model.position(0),vector(0,0,0))
assertQuaternionAlmostEqual(model.rotation(0),
convertCGtoNED(Quaternion(1,0,0,0)))
j1 = model.getJoint('j1')
assert j1.parent is model
assert len(j1.channels) == 3
assert len(j1.children) == 1
assert_almost_equal(j1.positionOffset, vector(10,0,0))
assert_almost_equal(j1.position(0), convertCGtoNED(vector(10,0,0)))
assertQuaternionAlmostEqual(j1.rotation(0),
convertCGtoNED(Quaternion.fromEuler((90, 0, 0), order='zxy')))
j1end = model.getPoint('j1_end')
assert j1end.parent is j1
assert len(j1end.channels) == 0
assert not j1end.isJoint
assert_almost_equal(j1end.positionOffset, vector(0,10,0))
assert_almost_equal(j1end.position(0), vector(0,0,0))
def checkVectorObservation(vectorObservationMethod, eulerAngles, inclination):
if issubclass(vectorObservationMethod,
vector_observation.LeastSquaresOptimalVectorObservation):
vo = vectorObservationMethod(inclinationAngle=inclination)
else:
vo = vectorObservationMethod()
q = Quaternion.fromEuler(eulerAngles)
inclination = math.radians(inclination)
m = np.array([[math.cos(inclination)],[0],
[math.sin(inclination)]])
g = np.array([[0,0,1]],dtype=float).T
g = q.rotateFrame(g)
m = q.rotateFrame(m)
assertQuaternionAlmostEqual(q,vo(g,m))
def checkTrajectory(T, truePositions, trueRotations):
"""
Check the outputs of a trajectory model agree with truth values.
@param T: Trajectory to check.
@param truePositions: L{TimeSeries} of true position values.
@param trueRotations: L{TimeSeries} of true rotation values.
"""
# Get time indices at which position comparisons valid
t = truePositions.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
dt = np.gradient(t)
p = truePositions.values[:, validity]
# Check position
assert_vectors_correlated(T.position(t), p)
# Check velocity
v = np.array(map(np.gradient, p)) / dt
assert_vectors_correlated(T.velocity(t[2:-2]), v[:, 2:-2])
# Check acceleration
a = np.array(map(np.gradient, v)) / dt
assert_vectors_correlated(T.acceleration(t[4:-4]), a[:, 4:-4])
# Get time indices at which rotation comparisons valid
t = trueRotations.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
r = trueRotations.values[validity]
# Check rotation
assertQuaternionAlmostEqual(T.rotation(t), r, tol=0.05)
# Check angular velocity
r, lastR = r[1:], r[:-1]
t, dt = t[1:], np.diff(t)
diffOmega = (2 * (r - lastR) * lastR.conjugate).array.T[1:] / dt
trajOmega = T.rotationalVelocity(t - dt / 2)
assert_vectors_correlated(trajOmega[:, 2:-2], diffOmega[:, 2:-2])
# Check angular acceleration
diffAlpha = np.array(map(np.gradient, diffOmega)) / dt
trajAlpha = T.rotationalAcceleration(t - dt / 2)
assert_vectors_correlated(trajAlpha[:, 4:-4], diffAlpha[:, 4:-4])
def checkTrajectory(T, truePositions, trueRotations):
"""
Check the outputs of a trajectory model agree with truth values.
@param T: Trajectory to check.
@param truePositions: L{TimeSeries} of true position values.
@param trueRotations: L{TimeSeries} of true rotation values.
"""
# Get time indices at which position comparisons valid
t = truePositions.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
dt = np.gradient(t)
p = truePositions.values[:,validity]
# Check position
assert_vectors_correlated(T.position(t), p)
# Check velocity
v = np.array(map(np.gradient, p)) / dt
assert_vectors_correlated(T.velocity(t[2:-2]), v[:,2:-2])
# Check acceleration
a = np.array(map(np.gradient, v)) / dt
assert_vectors_correlated(T.acceleration(t[4:-4]), a[:,4:-4])
# Get time indices at which rotation comparisons valid
t = trueRotations.timestamps
validity = (t >= T.startTime) & (t <= T.endTime)
t = t[validity]
r = trueRotations.values[validity]
# Check rotation
assertQuaternionAlmostEqual(T.rotation(t), r, tol=0.05)
# Check angular velocity
r, lastR = r[1:], r[:-1]
t, dt = t[1:], np.diff(t)
diffOmega = (2 * (r - lastR) * lastR.conjugate).array.T[1:] / dt
trajOmega = T.rotationalVelocity(t - dt/2)
assert_vectors_correlated(trajOmega[:,2:-2], diffOmega[:,2:-2])
# Check angular acceleration
diffAlpha = np.array(map(np.gradient, diffOmega)) / dt
trajAlpha = T.rotationalAcceleration(t - dt/2)
assert_vectors_correlated(trajAlpha[:,4:-4], diffAlpha[:,4:-4])
def testBVH_IO():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 5
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
0.0 1.0 0.0 0.0 0.0 0.0 80.0 0.0 5.0
0.0 2.0 0.0 0.0 0.0 0.0 70.0 0.0 10.0
0.0 3.0 0.0 0.0 0.0 0.0 60.0 0.0 15.0
0.0 4.0 0.0 0.0 0.0 0.0 50.0 0.0 20.0
"""
testFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
exportFile = tempfile.NamedTemporaryFile(mode='w+t', encoding='utf-8')
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
saveBVHFile(model, exportFile.name, 0.1)
exportedModel = loadBVHFile(exportFile.name)
assert len(list(model.points)) == len(list(exportedModel.points))
for orig, exported in zip(model.points, exportedModel.points):
assert orig.name == exported.name
assert_almost_equal(orig.positionOffset, exported.positionOffset)
assert_almost_equal(orig.position(0), exported.position(0))
if orig.isJoint:
assertQuaternionAlmostEqual(orig.rotation(0),
exported.rotation(0))
def testBVH_IO():
data = r"""HIERARCHY
ROOT root
{
OFFSET 0.0 0.0 0.0
CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation
JOINT j1
{
OFFSET 10.0 0.0 0.0
CHANNELS 3 Zrotation Xrotation Yrotation
End Site
{
OFFSET 0.0 10.0 0.0
}
}
}
MOTION
Frames: 5
Frame Time: 0.1
0.0 0.0 0.0 0.0 0.0 0.0 90.0 0.0 0.0
0.0 1.0 0.0 0.0 0.0 0.0 80.0 0.0 5.0
0.0 2.0 0.0 0.0 0.0 0.0 70.0 0.0 10.0
0.0 3.0 0.0 0.0 0.0 0.0 60.0 0.0 15.0
0.0 4.0 0.0 0.0 0.0 0.0 50.0 0.0 20.0
"""
testFile = tempfile.NamedTemporaryFile()
exportFile = tempfile.NamedTemporaryFile()
with testFile:
testFile.write(data)
testFile.flush()
model = loadBVHFile(testFile.name)
saveBVHFile(model, exportFile.name, 0.1)
exportedModel = loadBVHFile(exportFile.name)
assert len(list(model.points)) == len(list(exportedModel.points))
for orig, exported in zip(model.points, exportedModel.points):
assert orig.name == exported.name
assert_almost_equal(orig.positionOffset, exported.positionOffset)
assert_almost_equal(orig.position(0), exported.position(0))
if orig.isJoint:
assertQuaternionAlmostEqual(orig.rotation(0),
exported.rotation(0))
def checkMatrixToEuler(order, degrees):
q1 = randomRotation()
m = q1.toMatrix()
e = matrixToEuler(m, order, degrees)
q2 = QuaternionFromEuler(e, order, degrees)
assertQuaternionAlmostEqual(q1,q2)
def checkQuaternionSpline(splineClass, t, qa):
spline = splineClass(t,qa)
qs,_,_ = spline(t)
assertQuaternionAlmostEqual(qa[2:-2], qs[2:-2])