def leastsq_method(markers, offset=1):
axises = []
centers = []
shank_markers = markers.get_rigid_body("ben:RightShank")[0:1000]
T_Th = markers.get_frame("ben:RightThigh")
adjusted = Markers.transform_markers(np.linalg.inv(T_Th), shank_markers)
shank_markers = markers.get_rigid_body("ben:RightShank")[0:]
thigh_markers = markers.get_rigid_body("ben:RightThigh")[0:]
cor_filter = Mean_filter.Mean_Filter(20)
aor_filter = Mean_filter.Mean_Filter(20)
for frame in xrange(200, 325):
m1 = shank_markers[0][frame:frame + 2]
m2 = shank_markers[1][frame:frame + 2]
m3 = shank_markers[2][frame:frame + 2]
m4 = shank_markers[3][frame:frame + 2]
data = [m1, m2, m3, m4]
core = cor_filter.update(Markers.calc_CoR(data))
axis = aor_filter.update(Markers.calc_AoR(data))
thigh = Markers.calc_mass_vect([
thigh_markers[0][frame], thigh_markers[1][frame],
thigh_markers[2][frame], thigh_markers[3][frame]
])
shank = Markers.calc_mass_vect([
shank_markers[0][frame], shank_markers[1][frame],
shank_markers[2][frame], shank_markers[3][frame]
])
sol = Markers.minimize_center([thigh, shank],
axis=axis,
initial=(core[0][0], core[1][0],
core[2][0]))
#centers.append((core[0][0], core[1][0], core[2][0]))
centers.append(sol.x)
axises.append(axis)
return centers, axises
def get_right_knee(file, start, end):
vicon = Vicon(file)
markers = vicon.get_markers()
markers.smart_sort()
shank = markers.get_rigid_body("R_Tibia")
thigh = markers.get_rigid_body("R_Femur")
transforms = []
error = []
m1 = shank[0][start:end]
m2 = shank[1][start:end]
m3 = shank[2][start:end]
m4 = shank[3][start:end]
data = [m1, m2, m3, m4]
core = Markers.calc_CoR(data)
axis = Markers.calc_AoR(data)
core = [[core[0][0]], [core[1][0]], [core[2][0]], [1.0]]
core = np.array(core)
vect = np.array([[0.0], [0.0], [0.0], [0.0]])
max_error = 10000000000
for frame in xrange(start, end):
f = [
shank[0][frame], shank[1][frame], shank[2][frame], shank[3][frame]
]
T, err = Markers.cloud_to_cloud(cloud, f)
if err < max_error:
max_error = err
vect = np.dot(np.linalg.pinv(T), core)
error.append(err)
transforms.append(T)
#vect = vect/(end - start)
centers = []
for frame in xrange(len(shank[0])):
f = [
shank[0][frame], shank[1][frame], shank[2][frame], shank[3][frame]
]
T, err = Markers.cloud_to_cloud(cloud, f)
point = np.dot(T, vect)[0:3]
_thigh = Markers.calc_mass_vect([
thigh[0][frame], thigh[1][frame], thigh[2][frame], thigh[3][frame]
])
_shank = Markers.calc_mass_vect([
shank[0][frame], shank[1][frame], shank[2][frame], shank[3][frame]
])
sol = Markers.minimize_center([_thigh, _shank],
axis=axis,
initial=(point[0][0], point[1][0],
point[2][0]))
#centers.append( sol.x )
centers.append(point)
keys = markers._filtered_markers.keys()
nfr = len(markers._filtered_markers[keys[0]]) # Number of frames
x_total = []
y_total = []
z_total = []
for frame in xrange(nfr):
x = []
y = []
z = []
for key in keys:
point = markers._filtered_markers[key][frame]
x += [point.x]
y += [point.y]
z += [point.z]
x_total.append(x)
y_total.append(y)
z_total.append(z)
fps = 100 # Frame per sec
keys = markers._filtered_markers.keys()
nfr = len(markers._filtered_markers[keys[0]]) # Number of frames
print "sldfj ", nfr
ani = animation.FuncAnimation(fig,
animate,
nfr,
fargs=(x_total, y_total, z_total, centers,
axis),
interval=100 / fps)
plt.show()
return centers
def rotation_method(markers, offset=1):
shank_markers = markers.get_rigid_body("ben:RightShank")[0:]
T_Th = markers.get_frame("ben:RightThigh")
adjusted = Markers.transform_markers(np.linalg.inv(T_Th), shank_markers)
shank_markers = markers.get_rigid_body("ben:RightShank")[0:]
thigh_markers = markers.get_rigid_body("ben:RightThigh")[0:]
T_Th = markers.get_frame("ben:RightThigh")[0:]
T_Sh = markers.get_frame("ben:RightShank")[0:]
centers = []
axises = []
for frame in xrange(200, 350):
T_TH_SH_1 = np.dot(
np.linalg.pinv(T_Th[frame]), T_Sh[frame]
) # Markers.get_all_transformation_to_base(T_Th, T_Sh)[frame]
T_TH_SH_2 = np.dot(np.linalg.pinv(T_Th[frame + offset]),
T_Sh[frame + offset])
R1 = T_TH_SH_1[:3, :3]
R2 = T_TH_SH_2[:3, :3]
R1_2 = np.dot(np.transpose(R2), R1)
rp_1 = Markers.calc_mass_vect([
shank_markers[0][frame], shank_markers[1][frame],
shank_markers[2][frame], shank_markers[3][frame]
])
rp_2 = Markers.calc_mass_vect([
shank_markers[0][frame + offset], shank_markers[1][frame + offset],
shank_markers[2][frame + offset], shank_markers[3][frame + offset]
])
rd_1 = Markers.calc_mass_vect([
thigh_markers[0][frame], thigh_markers[1][frame],
thigh_markers[2][frame], thigh_markers[3][frame]
])
rd_2 = Markers.calc_mass_vect([
thigh_markers[0][frame + offset], thigh_markers[1][frame + offset],
thigh_markers[2][frame + offset], thigh_markers[3][frame + offset]
])
rdp1 = np.dot(T_Sh[frame][:3, :3], rd_1 - rp_1)
rdp2 = np.dot(T_Sh[frame + offset][:3, :3], rd_2 - rp_2)
print
P = np.eye(3) - R1_2
Q = rdp2 - np.dot(R1_2, rdp1)
rc = np.dot(np.linalg.pinv(P), Q)
thigh = Markers.calc_mass_vect([
thigh_markers[0][frame], thigh_markers[1][frame],
thigh_markers[2][frame], thigh_markers[3][frame]
])
shank = Markers.calc_mass_vect([
shank_markers[0][frame], shank_markers[1][frame],
shank_markers[2][frame], shank_markers[3][frame]
])
axis, angle = Markers.R_to_axis_angle(T_TH_SH_1[0:3, 0:3])
Rc = rp_1 + np.dot(np.transpose(T_Sh[frame][:3, :3]), rc)
sol = Markers.minimize_center([thigh, shank],
axis=axis,
initial=(Rc[0], Rc[1], Rc[2]))
centers.append(sol.x)
axises.append(axis)
return centers, axises
T_hip = []
T_thigh = []
T_shank = []
for frame in xrange(1000):
m = markers.get_rigid_body("ben:hip")
f = [m[0][frame], m[1][frame], m[2][frame], m[3][frame]]
T, err = Markers.cloud_to_cloud(hip_marker, f)
T_hip.append(T)
m = markers.get_rigid_body("ben:RightShank")
f = [m[0][frame], m[1][frame], m[2][frame], m[3][frame]]
T, err = Markers.cloud_to_cloud(shank_marker, f)
T_shank.append(T)
m = markers.get_rigid_body("ben:RightThigh")
f = [m[0][frame], m[1][frame], m[2][frame], m[3][frame]]
T, err = Markers.cloud_to_cloud(thigh_marker, f)
T_thigh.append(T)