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 leastsq_method2(markers, offset=10):
T_hip = []
T_thigh = []
T_shank = []
centers = []
axes = []
for frame in xrange(200, 325):
m = markers.get_rigid_body("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("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("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)
axises = []
centers = []
shank_markers = markers.get_rigid_body("RightShank")
m1 = markers.get_rigid_body("RightShank")[0][200:325]
m2 = markers.get_rigid_body("RightShank")[1][200:325]
m3 = markers.get_rigid_body("RightShank")[2][200:325]
m4 = markers.get_rigid_body("RightShank")[3][200:325]
adjusted = Markers.transform_markers(T_thigh, shank_markers)
m = [m1, m2, m3, m4]
core = Markers.calc_CoR(m)
axis = Markers.calc_AoR(m)
print "axis ", axis
return [core], [axis]
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