Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

adding frame finding/alignment

aaroncherian4b99ba1dfd6f
diff --git a/.gitignore b/.gitignore
index 122a22d4..24e67860 100644
--- a/.gitignore
+++ b/.gitignore
@@ -147,4 +147,6 @@ data/
*.mp4
*.divx
freemocap_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.npy
diff --git a/freemocap/fmc_good_frame_finder.py b/freemocap/fmc_good_frame_finder.py
new file mode 100644
index 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_frame
diff --git a/freemocap/fmc_mediapipe.py b/freemocap/fmc_mediapipe.py
index d9e6c153..4c79804e 100644
--- a/freemocap/fmc_mediapipe.py
+++ b/freemocap/fmc_mediapipe.py
@@ -12,7 +12,42 @@ mp_drawing = mp.solutions.drawing_utils
mp_drawing_styles = mp.solutions.drawing_styles
mp_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 parsed
diff --git a/freemocap/fmc_origin_alignment.py b/freemocap/fmc_origin_alignment.py
index 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 pd
import 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_data
diff --git a/freemocap/fmc_runme.py b/freemocap/fmc_runme.py
index fbbd7733..2c8b7e36 100644
--- a/freemocap/fmc_runme.py
+++ b/freemocap/fmc_runme.py
@@ -25,6 +25,7 @@ from rich.padding import Padding
from 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 npy
else:
diff --git a/freemocap/fmc_skeleton_data_holder.py b/freemocap/fmc_skeleton_data_holder.py
new file mode 100644
index 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