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])