Python assertQuaternionAlmostEqual Examples

Example #1

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

Example #2

File: vector_observation_test.py Project: VincentLouiss/IMU-Fushion-algorithm

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

Example #3

File: bvh_tests.py Project: JonFountain/imusim

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

Example #4

File: vector_observation_test.py Project: JonFountain/imusim

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

Example #5

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

Example #6

File: trajectories.py Project: JonFountain/imusim

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

Example #7

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

Example #8

File: bvh_tests.py Project: JonFountain/imusim

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

Example #9

File: matrices_test.py Project: windsmell/imusim

def checkMatrixToEuler(order, degrees):
    q1 = randomRotation()
    m = q1.toMatrix()
    e = matrixToEuler(m, order, degrees)
    q2 = QuaternionFromEuler(e, order, degrees)
    assertQuaternionAlmostEqual(q1,q2)

Example #10

File: quat_splines_test.py Project: VincentLouiss/IMU-Fushion-algorithm

def checkQuaternionSpline(splineClass, t, qa):
    spline = splineClass(t,qa)
    qs,_,_ = spline(t)
    assertQuaternionAlmostEqual(qa[2:-2], qs[2:-2])