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