Free Motion Capture for Everyone πβ¨
TypeScript
adding frame finding/alignment
aaroncherian4b99ba1dfd6f
diff --git a/.gitignore b/.gitignoreindex 122a22d4..24e67860 100644--- a/.gitignore+++ b/.gitignore@@ -147,4 +147,6 @@ data/*.mp4*.divxfreemocap_runme_script_jsm.py-freemocap-ui/threejs_stuff/\ No newline at end of file+freemocap-ui/threejs_stuff/+freemocap/fmc_calibration/previous_calibration.toml+freemocap/fmc_calibration/charuco_2d_points.npydiff --git a/freemocap/fmc_good_frame_finder.py b/freemocap/fmc_good_frame_finder.pynew file mode 100644index 00000000..efbf603b--- /dev/null+++ b/freemocap/fmc_good_frame_finder.py@@ -0,0 +1,103 @@++import numpy as np+import matplotlib.pyplot as plt+++++def find_velocity_values_within_limit(skeleton_velocity_data, velocity_limit):+ """+ This function takes in a skeleton velocity data array and a limit and returns the indices of the values that are within the limit+ """+ indices = []+ for i in range(len(skeleton_velocity_data)):+ if abs(skeleton_velocity_data[i]) <= velocity_limit:+ indices.append(i+1) #add 1 to account for the difference in indices between the position and velocity data+ return indices++def find_matching_indices_in_lists(list_1,list_2,list_3,list_4):+ """+ This function takes in four lists and returns the indices of the values that are in all four lists+ """+ matching_values = [x for x in list_1 if x in list_2 and x in list_3 and x in list_4]++ return matching_values++def find_best_velocity_guess(skeleton_velocity_data, velocity_guess, iteration_range):+ """+ This function iterates over velocity data and tries to pare down to a single frame that has the closest velocity to 0 for all foot markers+ """++ right_heel_index = 30+ right_toe_index = 32+ left_heel_index = 29+ left_toe_index = 31++ skeleton_data_velocity_x_right_heel = skeleton_velocity_data[:,right_heel_index,0]+ skeleton_data_velocity_x_right_toe = skeleton_velocity_data[:,right_toe_index,0]+ skeleton_data_velocity_x_left_heel = skeleton_velocity_data[:,left_heel_index,0]+ skeleton_data_velocity_x_left_toe = skeleton_velocity_data[:,left_toe_index,0]++ #get a list of the frames where the velocity for that marker is within the velocity limit+ right_heel_x_velocity_limits = find_velocity_values_within_limit(skeleton_data_velocity_x_right_heel, velocity_guess)+ right_toe_x_velocity_limits = find_velocity_values_within_limit(skeleton_data_velocity_x_right_toe, velocity_guess)+ left_heel_x_velocity_limits = find_velocity_values_within_limit(skeleton_data_velocity_x_left_heel, velocity_guess)+ left_toe_x_velocity_limits = find_velocity_values_within_limit(skeleton_data_velocity_x_left_toe, velocity_guess)++ #return a list of matching frame indices from the four lists generated above+ matching_values = find_matching_indices_in_lists(right_heel_x_velocity_limits, right_toe_x_velocity_limits, left_heel_x_velocity_limits, left_toe_x_velocity_limits)+ matching_values = [x for x in matching_values if x>75]+ if len(matching_values) < 10:+ print(matching_values)+ if len(matching_values) > 1 and velocity_guess > 0:+ #if there are multiple matching values, decrease the guess a little bit and run the function again+ #+ velocity_guess = velocity_guess - iteration_range+++ print('Current Velocity Guess: ',velocity_guess, '\n','Number of Matching Frames: ', len(matching_values))+ matching_values, velocity_guess = find_best_velocity_guess(skeleton_velocity_data, velocity_guess, iteration_range)++ f = 2+ elif len(matching_values) == 0:+ #if there are no matching values (we decreased our guess too far), reset the guess to be a bit smaller and run the function again with smaller intervals between the guesses+ iteration_range = iteration_range/2+ matching_values, velocity_guess = find_best_velocity_guess(skeleton_velocity_data, velocity_guess + iteration_range*2, iteration_range)++ f = 2+ elif len(matching_values) == 1:+ print('Final Velocity Value: ',velocity_guess, '\n','Good Frame: ', matching_values)++ return matching_values, velocity_guess++def set_axes_ranges(plot_ax,skeleton_data,ax_range):++ mx = np.nanmean(skeleton_data[:,0])+ my = np.nanmean(skeleton_data[:,1])+ mz = np.nanmean(skeleton_data[:,2])++ plot_ax.set_xlim(mx-ax_range,mx+ax_range)+ plot_ax.set_ylim(my-ax_range,my+ax_range)+ plot_ax.set_zlim(mz-ax_range,mz+ax_range)++def find_good_frame(skeleton_data, initial_velocity_guess, debug = False):++ print('Finding Good Frame')+ skeleton_velocity_data = np.diff(skeleton_data, axis=0)++ matching_values, velocity_guess = find_best_velocity_guess(skeleton_velocity_data, initial_velocity_guess, iteration_range=.1)++ good_frame = matching_values[0]++ if debug:++ figure = plt.figure()+ ax = figure.add_subplot(111, projection = '3d')++ ax.scatter(skeleton_data[good_frame,:,0], skeleton_data[good_frame,:,1], skeleton_data[good_frame,:,2], c='r', marker='o')++ set_axes_ranges(ax, skeleton_data[good_frame,:,:], 1000)++ plt.show()++ return good_framediff --git a/freemocap/fmc_mediapipe.py b/freemocap/fmc_mediapipe.pyindex d9e6c153..4c79804e 100644--- a/freemocap/fmc_mediapipe.py+++ b/freemocap/fmc_mediapipe.py@@ -12,7 +12,42 @@ mp_drawing = mp.solutions.drawing_utilsmp_drawing_styles = mp.solutions.drawing_stylesmp_holistic = mp.solutions.holistic-+mediapipe_indices = [+ 'nose',+ 'left_eye_inner',+ 'left_eye',+ 'left_eye_outer',+ 'right_eye_inner',+ 'right_eye',+ 'right_eye_outer',+ 'left_ear',+ 'right_ear',+ 'mouth_left',+ 'mouth_right',+ 'left_shoulder',+ 'right_shoulder',+ 'left_elbow',+ 'right_elbow',+ 'left_wrist',+ 'right_wrist',+ 'left_pinky',+ 'right_pinky',+ 'left_index',+ 'right_index',+ 'left_thumb',+ 'right_thumb',+ 'left_hip',+ 'right_hip',+ 'left_knee',+ 'right_knee',+ 'left_ankle',+ 'right_ankle',+ 'left_heel',+ 'right_heel',+ 'left_foot_index',+ 'right_foot_index'+ ]+def runMediaPipe(session, dummyRun=False):"""Run MediaPipe on synced videos, and save body tracking data to be parseddiff --git a/freemocap/fmc_origin_alignment.py b/freemocap/fmc_origin_alignment.pyindex c4be8cce..9c767cbd 100644--- a/freemocap/fmc_origin_alignment.py+++ b/freemocap/fmc_origin_alignment.py@@ -1,283 +1,232 @@import numpy as np-import pandas as pdimport matplotlib.pyplot as plt-from pathlib import Path-from rich.progress import track--def align_skeleton_with_origin(session, mediapipe_skeleton_data_XYZ, good_clean_frame_number):-- #skeleton_data_path = session.dataArrayPath/'mediaPipeSkel_3d_smoothed.npy'- debug = False- right_heel_index = 30- right_toe_index = 32- left_heel_index = 29- left_toe_index = 31-- num_pose_joints = 33-- primary_foot_indices = [left_heel_index,left_toe_index]- secondary_foot_index = [right_heel_index]- num_frames = session.numFrames- base_frame = good_clean_frame_number #just changing the variable names to work with the code I wrote- ## functions used in getting a matrix- def create_vector(point1,point2):- """Put two points in, make a vector [point1,point2]"""- vector = point2 - point1- return vector-- def create_normal_vector(vector1,vector2):- """Put two vectors in, make a normal vector"""- normal_vector = np.cross(vector1,vector2)- return normal_vector-- def create_unit_vector(vector):- """Take in a vector, make it a unit vector"""- unit_vector = vector/np.linalg.norm(vector)- return unit_vector-- def calculate_skewed_symmetric_cross_product(cross_product_vector):- skew_symmetric_cross_product = np.array([[0, -cross_product_vector[2], cross_product_vector[1]],- [cross_product_vector[2], 0, -cross_product_vector[0]],- [-cross_product_vector[1], cross_product_vector[0], 0]])- return skew_symmetric_cross_product-- def calculate_rotation_matrix(vector1,vector2):- """Put in two vectors to calculate the rotation matrix between those two vectors"""- #based on the code found here: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d"""- identity_matrix = np.identity(3)- vector_cross_product = np.cross(vector1,vector2)- vector_dot_product = np.dot(vector1,vector2)- skew_symmetric_cross_product = calculate_skewed_symmetric_cross_product(vector_cross_product)- rotation_matrix = identity_matrix + skew_symmetric_cross_product + (np.dot(skew_symmetric_cross_product,skew_symmetric_cross_product))*(1 - vector_dot_product)/(np.linalg.norm(vector_cross_product)**2)- return rotation_matrix-- def rotate_point(point,rotation_matrix):- rotated_point = np.dot(rotation_matrix,point)- return rotated_point-- def rotate_skeleton_frame(this_frame_aligned_skeleton_data, rotation_matrix):- """Take in a frame of skeleton data, and apply the rotation matrix to each point in the skeleton"""- this_frame_rotated_skeleton = np.zeros(this_frame_aligned_skeleton_data.shape) #initialize the array to hold the rotated skeleton data for this frame- num_tracked_points = this_frame_aligned_skeleton_data.shape[0]- for i in range(num_tracked_points):- this_frame_rotated_skeleton[i,:] = rotate_point(this_frame_aligned_skeleton_data[i,:],rotation_matrix)- return this_frame_rotated_skeleton-- def calculate_translation_distance(skeleton_right_heel):- """Take in the right heel point, and calculate the distance between the right heel and the origin"""- translation_distance = skeleton_right_heel - [0,0,0]- return translation_distance-- def translate_skeleton_frame(rotated_skeleton_data_frame, translation_distance):- """Take in a frame of rotated skeleton data, and apply the translation distance to each point in the skeleton"""- translated_skeleton_frame = rotated_skeleton_data_frame - translation_distance- return translated_skeleton_frame-- def calculate_normal_vector_to_foot(heel_one_index, toe_one_index, heel_two_index, skeleton_data):- foot_one_vector = create_vector(skeleton_data[heel_one_index,:],skeleton_data[toe_one_index,:])- heel_vector = create_vector(skeleton_data[heel_one_index,:],skeleton_data[heel_two_index,:])- foot_normal_vector = create_normal_vector(heel_vector,foot_one_vector)- return foot_normal_vector, foot_one_vector, heel_vector-- ##Create the Origin ---------------------------------------------------------------- origin = np.array([0, 0, 0])- x_axis = np.array([1, 0, 0])- y_axis = np.array([0, 1, 0])- z_axis = np.array([0, 0, 1])- x_vector = create_vector(origin,x_axis)- y_vector = create_vector(origin,y_axis)- z_vector = create_vector(origin,z_axis)- origin_normal_unit_vector = z_vector+from rich.progress import track- ##Get the unrotated skeleton data ------------------------------------------------- base_frame_skeleton_data = mediapipe_skeleton_data_XYZ[base_frame,:,:]- base_frame_normal_vector_to_foot,base_frame_left_foot_vector,base_frame_heel_vector = calculate_normal_vector_to_foot(left_heel_index,left_toe_index,right_heel_index,base_frame_skeleton_data)- ##SKELETON TRANSLATION---------------------- translation_distance = calculate_translation_distance(base_frame_skeleton_data[primary_foot_indices[0],:])- translated_skeleton_data = np.zeros(mediapipe_skeleton_data_XYZ.shape)-- for frame in track(range(num_frames), description= 'Translating Skeleton to Origin'):- translated_skeleton_data[frame,:,:] = translate_skeleton_frame(mediapipe_skeleton_data_XYZ[frame,:,:],translation_distance) #translate the skeleton data for each frame+from freemocap import fmc_skeleton_data_holder-- ##SKELETON +Z-AXIS ROTATION/ALIGNMENT---------------------- translated_base_frame_skeleton_data = translated_skeleton_data[base_frame,:,:]- translated_normal_vector_to_left_foot, translated_left_foot_vector, translated_heel_vector = calculate_normal_vector_to_foot(primary_foot_indices[0],primary_foot_indices[1], secondary_foot_index[0],translated_base_frame_skeleton_data)- unit_translated_normal_vector_to_left_foot = create_unit_vector(translated_normal_vector_to_left_foot)+def create_vector(point1,point2):+ """Put two points in, make a vector"""+ vector = point2 - point1+ return vector- rotation_matrix = calculate_rotation_matrix(unit_translated_normal_vector_to_left_foot,origin_normal_unit_vector)+def calculate_translation_distance(skeleton_right_heel):+ """Take in the right heel point, and calculate the distance between the right heel and the origin"""- translated_and_rotated_skeleton_data = np.zeros(mediapipe_skeleton_data_XYZ.shape) #create an array to hold the rotated skeleton data+ translation_distance = skeleton_right_heel - [0,0,0]+ return translation_distance- for frame in track(range(num_frames), description= 'Rotating Skeleton to Align With +Z'): #rotate the skeleton on each frame- translated_and_rotated_skeleton_data[frame,:,:] = rotate_skeleton_frame(translated_skeleton_data[frame,:,:],rotation_matrix)- ##SKELETON +Y-AXIS ALIGNMENT---------------------+def translate_skeleton_frame(rotated_skeleton_data_frame, translation_distance):+ """Take in a frame of rotated skeleton data, and apply the translation distance to each point in the skeleton"""- translated_and_rotated_base_frame_skeleton_data = translated_and_rotated_skeleton_data[base_frame,:,:]- translated_and_rotated_normal_vector_to_left_foot, translated_and_rotated_left_foot_vector, translated_and_rated_heel_vector = calculate_normal_vector_to_foot(primary_foot_indices[0],primary_foot_indices[1], secondary_foot_index[0],translated_and_rotated_base_frame_skeleton_data)-- unit_translated_and_rotated_normal_vector_to_left_foot = create_unit_vector(translated_and_rotated_normal_vector_to_left_foot)+ translated_skeleton_frame = rotated_skeleton_data_frame - translation_distance+ return translated_skeleton_frame- translated_and_rotated_heel_vector_for_y_axis_rotation = create_vector(translated_and_rotated_skeleton_data[base_frame,right_heel_index,:],translated_and_rotated_skeleton_data[base_frame,left_heel_index,:])- unit_translated_and_rotated_heel_vector = create_unit_vector(translated_and_rotated_heel_vector_for_y_axis_rotation)+def calculate_skewed_symmetric_cross_product(cross_product_vector):+ skew_symmetric_cross_product = np.array([[0, -cross_product_vector[2], cross_product_vector[1]],+ [cross_product_vector[2], 0, -cross_product_vector[0]],+ [-cross_product_vector[1], cross_product_vector[0], 0]])+ return skew_symmetric_cross_product- rotation_matrix_to_align_skeleton_with_positive_y = calculate_rotation_matrix(unit_translated_and_rotated_heel_vector,-1*x_vector)+def calculate_rotation_matrix(vector1,vector2):+ """Put in two vectors to calculate the rotation matrix between those two vectors"""+ #based on the code found here: https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d"""- origin_aligned_skeleton_data = np.zeros(mediapipe_skeleton_data_XYZ.shape)-- for frame in track(range(num_frames),description= 'Rotating Skeleton to Align With +Y'):- origin_aligned_skeleton_data[frame,:,:] = rotate_skeleton_frame(translated_and_rotated_skeleton_data[frame,:,:],rotation_matrix_to_align_skeleton_with_positive_y)-- origin_aligned_base_frame_skeleton_data = origin_aligned_skeleton_data[base_frame,:,:]- origin_aligned_normal_vector_to_left_foot, origin_aligned_left_foot_vector, origin_aligned_heel_vector = calculate_normal_vector_to_foot(primary_foot_indices[0],primary_foot_indices[1], secondary_foot_index[0],origin_aligned_base_frame_skeleton_data)+ identity_matrix = np.identity(3)+ vector_cross_product = np.cross(vector1,vector2)+ vector_dot_product = np.dot(vector1,vector2)+ skew_symmetric_cross_product = calculate_skewed_symmetric_cross_product(vector_cross_product)+ rotation_matrix = identity_matrix + skew_symmetric_cross_product + (np.dot(skew_symmetric_cross_product,skew_symmetric_cross_product))*(1 - vector_dot_product)/(np.linalg.norm(vector_cross_product)**2)+ return rotation_matrix+def rotate_point(point,rotation_matrix):+ rotated_point = np.dot(rotation_matrix,point)+ return rotated_point- if debug:-- def plot_origin_vectors(plot_ax,x_vector,y_vector,z_vector,origin):- Zvector_X,Zvector_Y,Zvector_Z = zip(origin_normal_unit_vector*800)- Xvector_X,Xvector_Y,Xvector_Z = zip(x_vector*800)- Yvector_X,Yvector_Y,Yvector_Z = zip(y_vector*800)+def rotate_skeleton_frame(this_frame_aligned_skeleton_data, rotation_matrix):+ """Take in a frame of skeleton data, and apply the rotation matrix to each point in the skeleton"""- Origin_X,Origin_Y,Origin_Z = zip(origin)+ this_frame_rotated_skeleton = np.zeros(this_frame_aligned_skeleton_data.shape) #initialize the array to hold the rotated skeleton data for this frame+ num_tracked_points = this_frame_aligned_skeleton_data.shape[0]- plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Zvector_X,Zvector_Y,Zvector_Z,arrow_length_ratio=0.1,color='b', label = 'Z-axis')- plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Xvector_X,Xvector_Y,Xvector_Z,arrow_length_ratio=0.1,color='r', label = 'X-axis')- plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Yvector_X,Yvector_Y,Yvector_Z,arrow_length_ratio=0.1,color='g', label = 'Y-axis')+ for i in range(num_tracked_points):+ this_frame_rotated_skeleton[i,:] = rotate_point(this_frame_aligned_skeleton_data[i,:],rotation_matrix)- def calculate_COM(skeleton_data):- COM_XYZ = np.nanmean(skeleton_data,axis=0)- return COM_XYZ+ return this_frame_rotated_skeleton- def calculate_COM_ground_projection_y(center_of_mass_XYZ, skeleton_data):- projection_distance = center_of_mass_XYZ[1] - skeleton_data[right_heel_index,1]- COM_ground_projected_XYZ = center_of_mass_XYZ - projection_distance*np.array([0,1,0])- return COM_ground_projected_XYZ-- def calculate_COM_ground_projection_z(center_of_mass_XYZ, skeleton_data):- projection_distance = center_of_mass_XYZ[2] - skeleton_data[right_heel_index,2]- COM_ground_projected_XYZ = center_of_mass_XYZ - projection_distance*np.array([0,0,1])-- return COM_ground_projected_XYZ+def align_skeleton_with_origin(skeleton_data, skeleton_indices ,good_frame, debug = False):+ #skeleton_type_to_plot = session_info['skeleton_type']- def plot_normal_unit_vector_to_foot(normal_vector_to_foot, origin_foot_index, skeleton_data,plot_ax):+ #save_file = this_freemocap_data_array_path/'{}_origin_aligned_skeleton_3D.npy'.format(skeleton_type_to_plot)- normal_vector_to_foot_unit_vector = create_unit_vector(normal_vector_to_foot)+ origin = np.array([0, 0, 0])+ x_axis = np.array([1, 0, 0])+ y_axis = np.array([0, 1, 0])+ z_axis = np.array([0, 0, 1])- normal_vector_to_foot_X,normal_vector_to_foot_Y,normal_vector_to_foot_Z = zip(normal_vector_to_foot_unit_vector*800)- plot_ax.quiver(skeleton_data[origin_foot_index,0],skeleton_data[origin_foot_index,1],skeleton_data[origin_foot_index,2],normal_vector_to_foot_X,normal_vector_to_foot_Y,normal_vector_to_foot_Z,arrow_length_ratio=0.1,color='pink')+ x_vector = create_vector(origin,x_axis)+ y_vector = create_vector(origin,y_axis)+ z_vector = create_vector(origin,z_axis)- def set_axes_ranges(plot_ax,skeleton_data, ax_range):+ #origin_normal = create_normal_vector(x_vector,y_vector) #create a normal vector to the origin (basically the z axis)+ origin_normal_unit_vector = z_vector #note - this is kinda unncessary because the origin normal unit vector == original normal vector- mx = np.nanmean(skeleton_data[:,0])- my = np.nanmean(skeleton_data[:,1])- mz = np.nanmean(skeleton_data[:,2])-- plot_ax.set_xlim(mx-ax_range,mx+ax_range)- plot_ax.set_ylim(my-ax_range,my+ax_range)- plot_ax.set_zlim(mz-ax_range,mz+ax_range)+ num_frames = skeleton_data.shape[0]- def plot_COM_point_and_projection(plot_ax,COM_XYZ,COM_ground_projected_XYZ):+ #Original Raw Data+ raw_skeleton_holder = fmc_skeleton_data_holder.SkeletonDataHolder(skeleton_data, skeleton_indices, good_frame)- plot_ax.scatter(COM_XYZ[0],COM_XYZ[1],COM_XYZ[2],color='b')- plot_ax.scatter(COM_ground_projected_XYZ[0],COM_ground_projected_XYZ[1],COM_ground_projected_XYZ[2], color = 'b')- plot_ax.plot([COM_XYZ[0],COM_ground_projected_XYZ[0]],[COM_XYZ[1],COM_ground_projected_XYZ[1]],[COM_XYZ[2],COM_ground_projected_XYZ[2]],color='b', alpha = .5)+ raw_good_frame_skeleton_data = raw_skeleton_holder.good_frame_skeleton_data+ raw_mid_hip_XYZ = raw_skeleton_holder.mid_hip_XYZ+ raw_spine_unit_vector = raw_skeleton_holder.spine_unit_vector- figure = plt.figure()- ax1 = figure.add_subplot(221,projection = '3d')- ax2 = figure.add_subplot(222,projection = '3d')- ax3 = figure.add_subplot(223,projection = '3d')- ax4 = figure.add_subplot(224,projection = '3d')+ #Translating Data+ mid_hip_translation_distance = calculate_translation_distance(raw_mid_hip_XYZ)+ hip_translated_skeleton_data = np.zeros(skeleton_data.shape)+ foot_translated_skeleton_data = hip_translated_skeleton_data.copy()- ax1.set_title('Original Skeleton')- ax2.set_title('Skeleton Translated to Origin')- ax3.set_title('Skeleton Rotated to Make +Z Up')- ax4.set_title('Skeleton Rotated to Make +Y Forwards')+ for frame in track(range(num_frames), description = 'Translating Skeleton Hips to Origin'):+ hip_translated_skeleton_data[frame,:,:] = translate_skeleton_frame(skeleton_data[frame,:,:],mid_hip_translation_distance) #translate the skeleton data for each frame- ax_range = 1800+ hip_translated_skeleton_holder = fmc_skeleton_data_holder.SkeletonDataHolder(hip_translated_skeleton_data, skeleton_indices, good_frame)+ hip_translated_mid_foot_XYZ = hip_translated_skeleton_holder.mid_foot_XYZ+ mid_foot_translated_distance = calculate_translation_distance(hip_translated_mid_foot_XYZ)- set_axes_ranges(ax1,base_frame_skeleton_data,ax_range)- set_axes_ranges(ax2,translated_base_frame_skeleton_data,ax_range)- set_axes_ranges(ax3,translated_and_rotated_base_frame_skeleton_data,ax_range)- set_axes_ranges(ax4,origin_aligned_base_frame_skeleton_data,ax_range)+ for frame in track (range(num_frames), description = 'Translating Skeleton Feet to Origin'):+ foot_translated_skeleton_data[frame,:,:] = translate_skeleton_frame(hip_translated_skeleton_data[frame,:,:],mid_foot_translated_distance)+ foot_translated_skeleton_holder = fmc_skeleton_data_holder.SkeletonDataHolder(foot_translated_skeleton_data, skeleton_indices, good_frame)+ foot_translated_good_frame_skeleton_data = foot_translated_skeleton_holder.good_frame_skeleton_data- ax1.scatter(base_frame_skeleton_data[:,0],base_frame_skeleton_data[:,1],base_frame_skeleton_data[:,2],c='r')- plot_origin_vectors(ax1,x_vector,y_vector,z_vector,origin)+ foot_translated_mid_hip_XYZ = foot_translated_skeleton_holder.mid_hip_XYZ+ foot_translated_spine_unit_vector = foot_translated_skeleton_holder.spine_unit_vector- original_COM_XYZ = calculate_COM(base_frame_skeleton_data[0:num_pose_joints,:])- original_COM_XYZ_ground_projection = calculate_COM_ground_projection_y(original_COM_XYZ,base_frame_skeleton_data)+ foot_translated_heel_unit_vector = foot_translated_skeleton_holder.heel_unit_vector+ foot_translated_heel_vector_origin = foot_translated_skeleton_holder.heel_vector_origin- plot_COM_point_and_projection(ax1,original_COM_XYZ,original_COM_XYZ_ground_projection)- plot_normal_unit_vector_to_foot(base_frame_normal_vector_to_foot,primary_foot_indices[0],base_frame_skeleton_data,ax1)+ #Rotating for +y alignment- ax2.scatter(translated_base_frame_skeleton_data[:,0],translated_base_frame_skeleton_data[:,1],translated_base_frame_skeleton_data[:,2],c='g')- plot_origin_vectors(ax2,x_vector,y_vector,z_vector,origin)+ rotation_matrix_to_align_skeleton_with_positive_y = calculate_rotation_matrix(foot_translated_heel_unit_vector,-1*x_vector)- translated_COM_XYZ = calculate_COM(translated_base_frame_skeleton_data[0:num_pose_joints,:])- translated_COM_XYZ_ground_projection = calculate_COM_ground_projection_y(translated_COM_XYZ,translated_base_frame_skeleton_data)+ y_aligned_skeleton_data = np.zeros(skeleton_data.shape)- plot_COM_point_and_projection(ax2,translated_COM_XYZ,translated_COM_XYZ_ground_projection)- plot_normal_unit_vector_to_foot(translated_normal_vector_to_left_foot,primary_foot_indices[0],translated_base_frame_skeleton_data,ax2)+ for frame in track(range(num_frames), description = 'Rotating Feet to Align with Positive Y'):+ y_aligned_skeleton_data [frame,:,:] = rotate_skeleton_frame(foot_translated_skeleton_data[frame,:,:],rotation_matrix_to_align_skeleton_with_positive_y)-- ax3.scatter(translated_and_rotated_base_frame_skeleton_data[:,0],translated_and_rotated_base_frame_skeleton_data[:,1],translated_and_rotated_base_frame_skeleton_data[:,2],c='orange')- plot_origin_vectors(ax3,x_vector,y_vector,z_vector,origin)+ y_aligned_skeleton_holder = fmc_skeleton_data_holder.SkeletonDataHolder(y_aligned_skeleton_data, skeleton_indices, good_frame)+ y_aligned_good_frame_skeleton_data = y_aligned_skeleton_holder.good_frame_skeleton_data- z_rotated_COM_XYZ = calculate_COM(translated_and_rotated_base_frame_skeleton_data[0:num_pose_joints,:])- z_rotated_COM_XYZ_ground_projection = calculate_COM_ground_projection_z(z_rotated_COM_XYZ,translated_and_rotated_base_frame_skeleton_data)+ y_aligned_mid_hip_XYZ = y_aligned_skeleton_holder.mid_hip_XYZ+ y_aligned_spine_unit_vector = y_aligned_skeleton_holder.spine_unit_vector- plot_COM_point_and_projection(ax3,z_rotated_COM_XYZ,z_rotated_COM_XYZ_ground_projection)- plot_normal_unit_vector_to_foot(translated_and_rotated_normal_vector_to_left_foot,primary_foot_indices[0],translated_and_rotated_base_frame_skeleton_data,ax3)- # ax3.quiver(rotated_right_heel_x,rotated_right_heel_y,rotated_right_heel_z,rotated_foot_x,rotated_foot_y,rotated_foot_z,arrow_length_ratio=0.1,color='pink')- # ax3.quiver(rotated_right_heel_x,rotated_right_heel_y,rotated_right_heel_z,rotated_left_heel_x,rotated_left_heel_y,rotated_left_heel_z,arrow_length_ratio=0.1,color='pink')+ y_aligned_heel_unit_vector = y_aligned_skeleton_holder.heel_unit_vector+ y_aligned_heel_vector_origin = y_aligned_skeleton_holder.heel_vector_origin- ax4.scatter(origin_aligned_base_frame_skeleton_data[:,0],origin_aligned_base_frame_skeleton_data[:,1],origin_aligned_base_frame_skeleton_data[:,2],c='purple')- plot_origin_vectors(ax4,x_vector,y_vector,z_vector,origin)+ #Rotating for spine alignment- origin_aligned_COM_XYZ = calculate_COM(origin_aligned_base_frame_skeleton_data[0:num_pose_joints,:])- origin_aligned_COM_XYZ_ground_projection = calculate_COM_ground_projection_z(origin_aligned_COM_XYZ,origin_aligned_base_frame_skeleton_data)-- plot_COM_point_and_projection(ax4,origin_aligned_COM_XYZ,origin_aligned_COM_XYZ_ground_projection)- plot_normal_unit_vector_to_foot(origin_aligned_normal_vector_to_left_foot,left_heel_index,origin_aligned_base_frame_skeleton_data,ax4)- # ax4.quiver(origin_aligned_right_heel_x,origin_aligned_right_heel_y,origin_aligned_right_heel_z,origin_aligned_left_heel_x,origin_aligned_left_heel_y,origin_aligned_left_heel_z,arrow_length_ratio=0.1,color='pink')-- ax1.legend()- ax2.legend()- ax3.legend()- plt.show()+ spine_aligned_skeleton_data = np.zeros(skeleton_data.shape)+ rotation_matrix_to_align_spine = calculate_rotation_matrix(y_aligned_spine_unit_vector,origin_normal_unit_vector)- return origin_aligned_skeleton_data+ for frame in track(range(num_frames), description = 'Rotating Spine to Align with Positive Z'):+ spine_aligned_skeleton_data [frame,:,:] = rotate_skeleton_frame(y_aligned_skeleton_data[frame,:,:],rotation_matrix_to_align_spine)+ spine_aligned_skeleton_holder = fmc_skeleton_data_holder.SkeletonDataHolder(spine_aligned_skeleton_data, skeleton_indices, good_frame)+ spine_aligned_good_frame_skeleton_data = spine_aligned_skeleton_holder.good_frame_skeleton_data+ spine_aligned_mid_hip_XYZ = spine_aligned_skeleton_holder.mid_hip_XYZ+ spine_aligned_spine_unit_vector = spine_aligned_skeleton_holder.spine_unit_vector+ spine_aligned_heel_unit_vector = spine_aligned_skeleton_holder.heel_unit_vector+ spine_aligned_heel_vector_origin = spine_aligned_skeleton_holder.heel_vector_origin+ if debug:+ def plot_origin_vectors(plot_ax,x_vector,y_vector,z_vector,origin):+ Zvector_X,Zvector_Y,Zvector_Z = zip(origin_normal_unit_vector*800)+ Xvector_X,Xvector_Y,Xvector_Z = zip(x_vector*800)+ Yvector_X,Yvector_Y,Yvector_Z = zip(y_vector*800)+ Origin_X,Origin_Y,Origin_Z = zip(origin)+ plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Zvector_X,Zvector_Y,Zvector_Z,arrow_length_ratio=0.1,color='b', label = 'Z-axis')+ plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Xvector_X,Xvector_Y,Xvector_Z,arrow_length_ratio=0.1,color='r', label = 'X-axis')+ plot_ax.quiver(Origin_X,Origin_Y,Origin_Z,Yvector_X,Yvector_Y,Yvector_Z,arrow_length_ratio=0.1,color='g', label = 'Y-axis')++ def set_axes_ranges(plot_ax,skeleton_data, ax_range):+ mx = np.nanmean(skeleton_data[:,0])+ my = np.nanmean(skeleton_data[:,1])+ mz = np.nanmean(skeleton_data[:,2])++ plot_ax.set_xlim(mx-ax_range,mx+ax_range)+ plot_ax.set_ylim(my-ax_range,my+ax_range)+ plot_ax.set_zlim(mz-ax_range,mz+ax_range)+ def plot_spine_unit_vector(plot_ax,skeleton_data,skeleton_mid_hip_XYZ,skeleton_spine_unit_vector):+ skeleton_spine_unit_x, skeleton_spine_unit_y, skeleton_spine_unit_z = zip(skeleton_spine_unit_vector*800)--+ plot_ax.quiver(skeleton_mid_hip_XYZ[0],skeleton_mid_hip_XYZ[1],skeleton_mid_hip_XYZ[2], skeleton_spine_unit_x,skeleton_spine_unit_y,skeleton_spine_unit_z,arrow_length_ratio=0.1,color='pink')+ def plot_heel_unit_vector(plot_ax,skeleton_heel_unit_vector, heel_vector_origin_XYZ):+ origin_heel_x, origin_heel_y, origin_heel_z = zip(heel_vector_origin_XYZ)+ skeleton_heel_unit_x, skeleton_heel_unit_y, skeleton_heel_unit_z = zip(skeleton_heel_unit_vector*500)+ plot_ax.quiver(origin_heel_x,origin_heel_y,origin_heel_z, skeleton_heel_unit_x,skeleton_heel_unit_y,skeleton_heel_unit_z,arrow_length_ratio=0.1,color='orange')++ figure = plt.figure()+ ax1 = figure.add_subplot(221,projection = '3d')+ ax2 = figure.add_subplot(222,projection = '3d')+ ax3 = figure.add_subplot(223,projection = '3d')+ ax4 = figure.add_subplot(224,projection = '3d')++ ax1.set_title('Original Skeleton')+ ax2.set_title('Skeleton Translated to Origin')+ ax3.set_title('Skeleton Rotated to Make +Y Forwards')+ ax4.set_title('Skeleton Rotated to Make +Z Up')++ ax_range = 1800++ set_axes_ranges(ax1,raw_good_frame_skeleton_data,ax_range)+ ax1.scatter(raw_good_frame_skeleton_data[:,0],raw_good_frame_skeleton_data[:,1],raw_good_frame_skeleton_data[:,2],c='r')+ plot_origin_vectors(ax1,x_vector,y_vector,z_vector,origin)+ plot_spine_unit_vector(ax1,raw_good_frame_skeleton_data,raw_mid_hip_XYZ,raw_spine_unit_vector)++ set_axes_ranges(ax2,foot_translated_good_frame_skeleton_data,ax_range)+ plot_origin_vectors(ax2,x_vector,y_vector,z_vector,origin)+ ax2.scatter(foot_translated_good_frame_skeleton_data[:,0],foot_translated_good_frame_skeleton_data[:,1],foot_translated_good_frame_skeleton_data[:,2],c='r')+ plot_heel_unit_vector(ax2,foot_translated_heel_unit_vector,foot_translated_heel_vector_origin)++ set_axes_ranges(ax3,y_aligned_good_frame_skeleton_data,ax_range)+ plot_origin_vectors(ax3,x_vector,y_vector,z_vector,origin)+ ax3.scatter(y_aligned_good_frame_skeleton_data[:,0],y_aligned_good_frame_skeleton_data[:,1],y_aligned_good_frame_skeleton_data[:,2],c='r')+ plot_heel_unit_vector(ax3,y_aligned_heel_unit_vector,y_aligned_heel_vector_origin)+ plot_spine_unit_vector(ax3,y_aligned_good_frame_skeleton_data,y_aligned_mid_hip_XYZ,y_aligned_spine_unit_vector)+ ax3.legend()++ set_axes_ranges(ax4,spine_aligned_good_frame_skeleton_data,ax_range)+ plot_origin_vectors(ax4,x_vector,y_vector,z_vector,origin)+ ax4.scatter(spine_aligned_good_frame_skeleton_data[:,0],spine_aligned_good_frame_skeleton_data[:,1],spine_aligned_good_frame_skeleton_data[:,2],c='r')+ plot_heel_unit_vector(ax4,spine_aligned_heel_unit_vector,spine_aligned_heel_vector_origin)+ plot_spine_unit_vector(ax4,spine_aligned_good_frame_skeleton_data,spine_aligned_mid_hip_XYZ,spine_aligned_spine_unit_vector)++ plt.show()++ return spine_aligned_skeleton_datadiff --git a/freemocap/fmc_runme.py b/freemocap/fmc_runme.pyindex fbbd7733..2c8b7e36 100644--- a/freemocap/fmc_runme.py+++ b/freemocap/fmc_runme.py@@ -25,6 +25,7 @@ from rich.padding import Paddingfrom freemocap import (+ fmc_good_frame_finder,recordingconfig,runcams,calibrate,@@ -33,6 +34,7 @@ from freemocap import (fmc_deeplabcut,fmc_origin_alignment,fmc_mediapipe_annotation,+ fmc_skeleton_data_holder,reconstruct3D,play_skeleton_animation,session,@@ -249,8 +251,11 @@ def RunMe(sessionID=None,if place_skeleton_on_origin:sesh.mediaPipeSkel_fr_mar_xyz_smoothed_unrotated = sesh.mediaPipeSkel_fr_mar_xyz.copy()np.save(sesh.dataArrayPath/'mediaPipeSkel_3d_smoothed_unrotated.npy', sesh.mediaPipeSkel_fr_mar_xyz_smoothed_unrotated) #save data to npy+ good_frame = fmc_good_frame_finder.find_good_frame(sesh.mediaPipeSkel_fr_mar_xyz, .6, debug = True) #.6 is an initial guess for the velocity. seems to be a safe bet for for all the cases I've tried- origin_aligned_skeleton_data_XYZ = fmc_origin_alignment.align_skeleton_with_origin(sesh,sesh.mediaPipeSkel_fr_mar_xyz,good_clean_frame_number)+ mediapipe_indices = fmc_mediapipe.mediapipe_indices++ origin_aligned_skeleton_data_XYZ = fmc_origin_alignment.align_skeleton_with_origin(sesh.mediaPipeSkel_fr_mar_xyz, mediapipe_indices, good_frame, debug = True)np.save(sesh.dataArrayPath/'mediaPipeSkel_3d_smoothed.npy', origin_aligned_skeleton_data_XYZ) #save data to npyelse:diff --git a/freemocap/fmc_skeleton_data_holder.py b/freemocap/fmc_skeleton_data_holder.pynew file mode 100644index 00000000..b3f41428--- /dev/null+++ b/freemocap/fmc_skeleton_data_holder.py@@ -0,0 +1,100 @@++import numpy as np+++++class SkeletonDataHolder:+ def __init__(self, skeleton_data, skeleton_indices_list,good_frame):+ self.skeleton_data = skeleton_data+ self.skeleton_indices_list = skeleton_indices_list+++ self.set_indices_as_properties(skeleton_indices_list)+ self.good_frame_skeleton_data = self.get_good_frame_of_data(good_frame,skeleton_data)+ self.run_joint_finder_calculations()+ self.run_vector_calculations()+++ def run_joint_finder_calculations(self):+ self.mid_hip_XYZ = self.calculate_mid_hip_XYZ_coordinates(self.good_frame_skeleton_data, self.left_hip_index,self.right_hip_index)+ self.mid_foot_XYZ = self.calculate_mid_foot_XYZ_coordinate(self.good_frame_skeleton_data, self.left_heel_index, self.right_heel_index)+ self.shoulder_center_XYZ = self.calculate_shoulder_center_XYZ_coordinates(self.good_frame_skeleton_data, self.left_shoulder_index, self.right_shoulder_index)++ def run_vector_calculations(self):+ self.spine_vector, self.spine_unit_vector = self.calculate_spine_vector_and_unit_vector(self.good_frame_skeleton_data, self.mid_hip_XYZ, self.shoulder_center_XYZ)+++ self.heel_vector_origin = self.good_frame_skeleton_data[self.right_heel_index,:]+ self.heel_vector = self.create_vector(self.heel_vector_origin ,self.good_frame_skeleton_data[self.left_heel_index,:])++ self.heel_unit_vector = self.create_unit_vector(self.heel_vector)++ def set_indices_as_properties(self, indices_list):+ self.left_hip_index = indices_list.index('left_hip')+ self.right_hip_index = indices_list.index('right_hip')++ self.left_shoulder_index = indices_list.index('left_shoulder')+ self.right_shoulder_index = indices_list.index('right_shoulder')++ self.left_toe_index = indices_list.index('left_foot_index')+ self.right_toe_index = indices_list.index('right_foot_index')+ self.left_heel_index = indices_list.index('left_heel')+ self.right_heel_index = indices_list.index('right_heel')++ def get_good_frame_of_data(self,good_frame,skeleton_data):+ """Take in a good frame, and return the base frame"""+ good_frame_skeleton_data = skeleton_data[good_frame,:,:]+ return good_frame_skeleton_data++ def create_vector(self,point1,point2):+ """Put two points in, make a vector"""+ vector = point2 - point1+ return vector++ def create_normal_vector(self,vector1,vector2):+ """Put two vectors in, make a normal vector"""+ normal_vector = np.cross(vector1,vector2)+ return normal_vector++ def create_unit_vector(self,vector):+ """Take in a vector, make it a unit vector"""+ unit_vector = vector/np.linalg.norm(vector)+ return unit_vector++ def calculate_normal_vector_to_foot(self,heel_one_index, toe_one_index, heel_two_index, skeleton_data):+ foot_one_vector = self.create_vector(skeleton_data[heel_one_index,:],skeleton_data[toe_one_index,:])+ heel_vector = self.create_vector(skeleton_data[heel_one_index,:],skeleton_data[heel_two_index,:])++ foot_normal_vector = self.create_normal_vector(heel_vector,foot_one_vector)++ return foot_normal_vector, foot_one_vector, heel_vector++ def calculate_shoulder_center_XYZ_coordinates(self, skeleton_data,left_shoulder_index,right_shoulder_index ):+ """Take in the left and right shoulder indices, and calculate the shoulder center point"""+ left_shoulder_point = skeleton_data[left_shoulder_index,:]+ right_shoulder_point = skeleton_data[right_shoulder_index,:]+ shoulder_center_XYZ_coordinates = (left_shoulder_point + right_shoulder_point)/2++ return shoulder_center_XYZ_coordinates++ def calculate_mid_hip_XYZ_coordinates(self,skeleton_data,left_hip_index,right_hip_index):+ """Take in the left and right hip indices, and calculate the mid hip point"""+ left_hip_point = skeleton_data[left_hip_index,:]+ right_hip_point = skeleton_data[right_hip_index,:]+ mid_hip_XYZ_coordinates = (left_hip_point + right_hip_point)/2++ return mid_hip_XYZ_coordinates++ def calculate_spine_vector_and_unit_vector(self,skeleton_data,mid_hip_XYZ, mid_shoulder_XYZ):+ spine_vector = self.create_vector(mid_hip_XYZ,mid_shoulder_XYZ)+ spine_unit_vector = self.create_unit_vector(spine_vector)+ return spine_vector, spine_unit_vector++ def calculate_mid_foot_XYZ_coordinate(self, skeleton_data, right_heel_index, left_heel_index):+ """Take in the primary and secondary foot indices, and calculate the mid foot point"""+ right_foot_point = skeleton_data[right_heel_index,:]+ left_foot_point = skeleton_data[left_heel_index,:]+ mid_foot_XYZ_coordinates = (right_foot_point + left_foot_point)/2++ return mid_foot_XYZ_coordinates\ No newline at end of file