def projection(markers): T_hip = [] T_shank = [] T_thigh = [] 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) for ii in xrange(200, 325): points = [] for jj in xrange(4): points.append(adjusted[jj][ii]) points.append(adjusted[jj][ii + 1]) fit, residual = Markers.fit_to_plane(points)
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
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 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)