def align(self):
     import quaternions
     radians, a, b, c = self.alignment
     m = rot_matrix(radians)
     intercept = np.array([a, b, c])
     for label in KIN_LABELS:
         try:
             self.qual.data.smooth[label] = self.qual.data.smooth[label].dot(m.T) + intercept
         except KeyError:
             pass
     # replace HipCenterQ by canonical form eventually
     self.qual.data.smooth['HipCenterQ'] = quaternions.conjugate(
                                             np.array([np.cos(0.5*radians), 
                                                       0, 0, 
                                                       np.sin(0.5*radians)]), 
                                             self.qual.data.smooth['HipCenterQ'])
Пример #2
0
def measurement_update(xk, g):
    yk = qt.quaternion_rotate(qt.conjugate(xk), np.array([0, 0, g]))

    yk2 = 2 * np.arctan2(xk[3], xk[0])

    return np.array([yk[0], yk[1], yk[2]])#, yk2])
def normalize_data(data, center_floor = False):
    """Assume that data is aligned (so qual and kin have best fit)"""

    qual_present = hasattr(data, 'qual')

    k = data['kin']
    kn = dict()

    if qual_present:
        q = data['qual']
        qn = dict()

    center = center_of_mass(k)
    if center_floor:
        center *= np.array((1,1,0))

    # first translate center of mass to the origin
    for key in k.keys():
        if key in data_model.KIN_LABELS:
            kn[key] = np.array(k[key]) - center
        else:
            kn[key] = k[key]
    if qual_present:
        for key in q.keys():
            if len(q[key][0]) == 3:
                qn[key] = np.array(q[key]) - center
            else:
                qn[key] = q[key]

    # coordinate system to use: z axis, hip orientation vector (suitably
    # gram schmidted) --  call this x, whatever is necessary to make right-handed system -- y

    hr = kn['HipRight']
    hl = kn['HipLeft']
    n = normalize_rows(np.cross(hl, hr))
    x = normalize_rows(n - n * np.array([0,0,1]))
    y = normalize_rows(np.cross(np.array([0,0,1]), x))

    outk = {}
    if qual_present:
        outq = {}

    for key in kn.keys():
        if key in data_model.KIN_LABELS:
            outk[key] = np.column_stack(((kn[key] * x).sum(-1), (kn[key] * y).sum(-1), kn[key].T[2]))
        else:
            outk[key] = k[key]

    if qual_present:
        for key in qn.keys():
            if len(q[key][0]) == 3:
                outq[key] = np.column_stack(((qn[key] * x).sum(-1), (qn[key] * y).sum(-1), qn[key].T[2]))
            elif key != 'Time' and key != 'HipCenterQ':
                outq[key] = q[key]
            elif key == 'HipCenterQ':
                theta = np.arccos(x.T[0])
                s = np.cos(0.5 * theta)
                t = np.sin(0.5 * theta)
                outq[key] = quaternions.conjugate(np.column_stack([s, np.zeros(len(s)), np.zeros(len(s)), t]), np.array(q[key]))

    # try:
    #     outk['Time'] = k['Time']
    # except KeyError:
    #     pass
    if qual_present:
        try:
            outq['Time'] = q['Time']
        except KeyError:
            pass

    if qual_present:
        return {'unnormalized_kin': k, 'unnormalized_qual': q, 'kin': outk, 'qual': outq}
    else:
        return {'unnormalized_kin': k, 'kin': outk}