def sphere_method(markers, offset=1):
T_hip = []
T_thigh = []
T_shank = []
centers = []
shank_markers = markers.get_rigid_body("RightShank")[0:]
thigh_markers = markers.get_rigid_body("RightThigh")[0:]
print len(thigh_marker[0])
for frame in xrange(1000):
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)
shank_markers = markers.get_rigid_body("RightShank")[0:]
print T_thigh
adjusted = Markers.transform_markers(np.linalg.pinv(T_thigh), shank_markers)
centers = []
axises = []
CoM = []
CoM_fixed = []
# Get all the mass centers of the frame
for ii in xrange(1000):
temp = Markers.calc_mass_vect([adjusted[0][ii],
adjusted[1][ii],
adjusted[2][ii],
adjusted[3][ii]])
CoM.append(np.asarray(temp))
for ii in xrange(200,325):
raduis, C = Markers.sphereFit(CoM_fixed[ii:ii+2])
T1 = [shank_markers[0][frame], shank_markers[1][frame], shank_markers[2][frame], shank_markers[3][frame]]
T2 = [shank_markers[0][frame + 1], shank_markers[1][frame + 1], shank_markers[2][frame + 1],
shank_markers[3][frame + 1]]
T, err = Markers.cloud_to_cloud(T1, T2)
ax, angle = Markers.R_to_axis_angle(T)
C = np.row_stack((C,[1]))
C = np.dot(T_thigh[ii],C)
centers.append(C[0:3])
axises.append(ax)
return centers, axises
def sphere_method2(markers, offset=1):
T_hip = []
T_thigh = []
T_shank = []
centers = []
shank_markers = markers.get_rigid_body("ben:RightShank")[0:]
thigh_markers = markers.get_rigid_body("ben:RightThigh")[0:]
print len(thigh_marker[0])
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)
shank_markers = markers.get_rigid_body("ben:RightShank")[0:]
adjusted = Markers.transform_markers(np.linalg.pinv(T_thigh), shank_markers)
centers = []
axises = []
CoM = []
CoM_fixed = []
#Get all the mass centers of the frame
for ii in xrange(200,325):
C = np.array([[0.0],[0.0],[0.0]])
for jj in xrange(4):
arr1 = np.array([adjusted[jj][ii].x, adjusted[jj][ii].y, adjusted[jj][ii].z ])
arr2 = np.array([adjusted[jj][ii+1].x, adjusted[jj][ii+1].y, adjusted[jj][ii+1].z])
raduis, C_ = Markers.sphereFit([arr1,arr2])
C += 0.25*C_
T1 = [shank_markers[0][frame], shank_markers[1][frame], shank_markers[2][frame], shank_markers[3][frame]]
T2 = [shank_markers[0][frame + 1], shank_markers[1][frame + 1], shank_markers[2][frame + 1],
shank_markers[3][frame + 1]]
T, err = Markers.cloud_to_cloud(T1, T2)
ax, angle = Markers.R_to_axis_angle(T)
C = np.row_stack((C,[1]))
C = np.dot(T_thigh[ii],C)
centers.append(C[0:3])
axises.append(ax)
return centers, axises
